#include "h0d_af.h"
#include <stdint.h>

int sv630p_h0d_17_read_force_output_mode(sv630p_handle_t *dev, DIDO_ForceMode *mode)
{
    uint16_t value = 0;
    SV630P_ARGS_CHECK(dev);
    if (dev->ops->read_holding_regs(dev, REG_H0D_17_FORCE_DIDO_OUTPUT, 1, &value) == SV630P_OK)
    {
        *mode = (DIDO_ForceMode)value;
        return SV630P_OK;
    }
    return SV630P_BUS_ERR;
}

/**
 * @brief Set the Force Output Mode
 * @param dev SV630P device
 * @param mode DIDO_ForceMode
*/
int sv630p_h0d_17_write_force_output_mode(sv630p_handle_t *dev, DIDO_ForceMode mode)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H0D_17_FORCE_DIDO_OUTPUT, (uint16_t)mode);
}

int sv630p_h0d_18_read_force_input_value(sv630p_handle_t *dev, uint16_t *value)
{
    SV630P_ARGS_CHECK(dev);
    if (dev->ops->read_holding_regs(dev, REG_H0D_18_FORCE_DI_INPUT, 1, value) == SV630P_OK)
    {
        return SV630P_OK;
    }
    return SV630P_BUS_ERR;
}

/**
 * @brief Set the Force Input Value
 * @param dev SV630P device
 * @param high_mask Bit mask that needs to be set to high level
 * @param low_mask Bit mask that needs to be set to high level
 * @note Check H17 and H0D.17 H0D.18
*/
int sv630p_h0d_18_write_force_input_bits(sv630p_handle_t *dev, uint16_t high_mask, uint16_t low_mask)
{
    SV630P_ARGS_CHECK(dev);
    uint16_t current_val = 0;
    if(dev->ops->read_holding_regs(dev, REG_H0D_18_FORCE_DI_INPUT, 1, &current_val) != SV630P_OK)
    {
        SV630_ERROR(dev, "Failed to read current force input value");
        return SV630P_BUS_ERR;
    }
    uint16_t new_val = (current_val | high_mask) & (~low_mask);
    return dev->ops->write_single_reg(dev, REG_H0D_18_FORCE_DI_INPUT, new_val);
}