#include <string.h>
#include "h03_di_do.h"


/* Complete mapping table with function descriptions */
static const funin_map_entry_t funin_map1[] = {
    {    SV630P_FunIN_1_S_ON,  SV630P_DIFUNC1_FUNIN_1,                       "Servo enable"},
    { SV630P_FunIN_2_ALM_RST,  SV630P_DIFUNC1_FUNIN_2,            "Fault and warning reset"},
    {SV630P_FunIN_3_GAIN_SEL,  SV630P_DIFUNC1_FUNIN_3,                     "Gain selection"},
    { SV630P_FunIN_4_CMD_SEL,  SV630P_DIFUNC1_FUNIN_4,     "Main spindle command selection"},
    { SV630P_FunIN_5_DIR_SEL,  SV630P_DIFUNC1_FUNIN_5, "Multi-speed DI direction selection"},
    {    SV630P_FunIN_6_CMD1,  SV630P_DIFUNC1_FUNIN_6,     "Multi-step command selection 1"},
    {    SV630P_FunIN_7_CMD2,  SV630P_DIFUNC1_FUNIN_7,     "Multi-step command selection 2"},
    {    SV630P_FunIN_8_CMD3,  SV630P_DIFUNC1_FUNIN_8,     "Multi-step command selection 3"},
    {    SV630P_FunIN_9_CMD4,  SV630P_DIFUNC1_FUNIN_9,     "Multi-step command selection 4"},
    { SV630P_FunIN_10_M1_SEL, SV630P_DIFUNC1_FUNIN_10,                   "Mode selection 1"},
    { SV630P_FunIN_11_M2_SEL, SV630P_DIFUNC1_FUNIN_11,                   "Mode selection 2"},
    { SV630P_FunIN_12_ZCLAMP, SV630P_DIFUNC1_FUNIN_12,                  "Zero clamp enable"},
    {SV630P_FunIN_13_INHIBIT, SV630P_DIFUNC1_FUNIN_13,           "Position command inhibit"},
    {   SV630P_FunIN_14_P_OT, SV630P_DIFUNC1_FUNIN_14,         "Positive overtravel switch"},
    {   SV630P_FunIN_15_N_OT, SV630P_DIFUNC1_FUNIN_15,         "Negative overtravel switch"},
};

/**
 * @brief Find mask by DI function enum (FunIN)
 * @param funin DI function enum
 * @param[out] mask Output mask value
 * @return 0 if found, -1 otherwise
 */
int find_mask1_by_funin(SV630P_DIDO_FunIN funin, SV630P_FuncTypeMask1 *mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP_SIZE(funin_map1); i++)
    {
        if (funin_map1[i].funin == funin)
        {
            *mask = funin_map1[i].mask;
            return 0;
        }
    }
    return -1;
}

/**
 * @brief Find index in mapping table by mask
 * @param mask Mask value
 * @return Index if found, -1 otherwise
 */
int find_index_by_mask1(SV630P_FuncTypeMask1 mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP_SIZE(funin_map1); i++)
    {
        if (funin_map1[i].mask == mask)
        {
            return i;
        }
    }
    return -1;
}

/**
 * @brief Find function description by mask
 * @param mask Mask value
 * @return Description string if found, NULL otherwise
 */
char *find_funin_desc_by_mask1(SV630P_FuncTypeMask1 mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP_SIZE(funin_map1); i++)
    {
        if (funin_map1[i].mask == mask)
        {
            return (char *)funin_map1[i].desc;
        }
    }
    return NULL;
}

/**
 * @brief Read power-on DI function assignment 1
 * @param dev Pointer to servo handle
 * @param funin Output DI function enum
 * @return SV630P_OK on success, error code otherwise
 */
int sv630p_h03_00_read_power_on_di_func_assign1(sv630p_handle_t *dev, SV630P_DIDO_FunIN *funin)
{
    uint16_t value = 0;
    int      index = -1;

    SV630P_ARGS_CHECK(dev);
    if (dev->ops->read_holding_regs(dev, REG_H03_00_POWER_ON_DI1, 1, &value) == SV630P_OK)
    {
        index = find_index_by_mask1(value);
        if (index >= 0)
        {
            SV630_DEBUG(dev, "DI function: %s\n", funin_map1[index].desc);
            *funin = funin_map1[index].funin;
            return SV630P_OK;
        }
        else
        {
            SV630_DEBUG(dev, "DI function: %s\n", "Unknown");
            return SV630P_INDEX_ERR;
        }
    }
    return SV630P_BUS_ERR;
}

/**
 * @brief Write power-on DI function assignment 1
 * @param dev Pointer to servo handle
 * @param func DI function enum to write
 * @return SV630P_OK on success, error code otherwise
 */
int sv630p_h03_00_write_power_on_di_func_assign1(sv630p_handle_t *dev, SV630P_DIDO_FunIN func)
{
    SV630P_FuncTypeMask1 mask;
    SV630P_ARGS_CHECK(dev);

    if (find_mask1_by_funin(func, &mask) == 0)
        return dev->ops->write_single_reg(dev, REG_H03_00_POWER_ON_DI1, mask);
    else
        return SV630P_INDEX_ERR;
}

// ====== SV630P_FuncTypeMask2 mapping table and function ======
static const funin_map_entry_t funin_map2[] = {
    {        SV630P_FunIN_17_N_CL, SV630P_DIFUNC2_FUNIN_17,       "Negative external torque limit"},
    { SV630P_FunIN_18_JOGCMD_PLUS, SV630P_DIFUNC2_FUNIN_18,                        "Jog command +"},
    {SV630P_FunIN_19_JOGCMD_MINUS, SV630P_DIFUNC2_FUNIN_19,                        "Jog command -"},
    {     SV630P_FunIN_20_POSSTEP, SV630P_DIFUNC2_FUNIN_20,                          "Step enable"},
    {         SV630P_FunIN_21_HX1, SV630P_DIFUNC2_FUNIN_21,             "Handwheel ratio signal 1"},
    {         SV630P_FunIN_22_HX2, SV630P_DIFUNC2_FUNIN_22,             "Handwheel ratio signal 2"},
    {       SV630P_FunIN_23_HX_EN, SV630P_DIFUNC2_FUNIN_23,              "Handwheel enable signal"},
    {    SV630P_FunIN_24_GEAR_SEL, SV630P_DIFUNC2_FUNIN_24,               "Electronic gear select"},
    {   SV630P_FunIN_25_TOQDirSel, SV630P_DIFUNC2_FUNIN_25,      "Torque command direction select"},
    {   SV630P_FunIN_26_SPDDirSel, SV630P_DIFUNC2_FUNIN_26,       "Speed command direction select"},
    {   SV630P_FunIN_27_POSDirSel, SV630P_DIFUNC2_FUNIN_27,    "Position command direction select"},
    {    SV630P_FunIN_28_PosinSen, SV630P_DIFUNC2_FUNIN_28,        "Multi-position command enable"},
    {    SV630P_FunIN_29_XintFree, SV630P_DIFUNC2_FUNIN_29, "Interrupt fixed length state release"},
    // SV630P_FunIN_30_xxx, // If you have FunIN.30, add here
    {  SV630P_FunIN_31_HomeSwitch, SV630P_DIFUNC2_FUNIN_31,                          "Home switch"},
    { SV630P_FunIN_32_HomingStart, SV630P_DIFUNC2_FUNIN_32,                        "Homing enable"},
};

#define FUNIN_MAP2_SIZE(x) (sizeof(x) / sizeof(x[0]))

/**
 * @brief Find mask by DI function enum (FunIN) for group 2
 * @param funin DI function enum
 * @param[out] mask Output mask value
 * @return 0 if found, -1 otherwise
 */
int find_mask2_by_funin(SV630P_DIDO_FunIN funin, SV630P_FuncTypeMask2 *mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP2_SIZE(funin_map2); i++)
    {
        if (funin_map2[i].funin == funin)
        {
            *mask = (SV630P_FuncTypeMask2)funin_map2[i].mask;
            return 0;
        }
    }
    return -1;
}

/**
 * @brief Find index in mapping table by mask for group 2
 * @param mask Mask value
 * @return Index if found, -1 otherwise
 */
int find_index_by_mask2(SV630P_FuncTypeMask2 mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP2_SIZE(funin_map2); i++)
    {
        if ((SV630P_FuncTypeMask2)funin_map2[i].mask == mask)
        {
            return i;
        }
    }
    return -1;
}

/**
 * @brief Find function description by mask for group 2
 * @param mask Mask value
 * @return Description string if found, NULL otherwise
 */
char *find_funin_desc_by_mask2(SV630P_FuncTypeMask2 mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP2_SIZE(funin_map2); i++)
    {
        if ((SV630P_FuncTypeMask2)funin_map2[i].mask == mask)
        {
            return (char *)funin_map2[i].desc;
        }
    }
    return NULL;
}

/**
 * @brief Read power-on DI function assignment 2
 * @param dev Pointer to servo handle
 * @param funin Output DI function enum
 * @return SV630P_OK on success, error code otherwise
 */
int sv630p_h03_01_read_power_on_di_func_assign2(sv630p_handle_t *dev, SV630P_DIDO_FunIN *funin)
{
    uint16_t value = 0;
    int      index = -1;
    SV630P_ARGS_CHECK(dev);

    if (dev->ops->read_holding_regs(dev, REG_H03_01_POWER_ON_DI2, 1, &value) == SV630P_OK)
    {
        index = find_index_by_mask2(value);
        if (index >= 0)
        {
            SV630_DEBUG(dev, "DI function: %s\n", funin_map2[index].desc);
            *funin = funin_map2[index].funin;
            return SV630P_OK;
        }
        else
        {
            SV630_DEBUG(dev, "DI function: %s\n", "Unknown");
            return SV630P_INDEX_ERR;
        }
    }
    return SV630P_BUS_ERR;
}

/**
 * @brief Write power-on DI function assignment 2, 设置某一DI功能(FunIN.17~FunIN.32)重新上电后立即有效
 * @param dev Pointer to servo handle
 * @param func DI function enum to write
 * @return SV630P_OK on success, error code otherwise
 * @description: 再上电生效, 实时更改
 */
int sv630p_h03_01_write_power_on_di_func_assign2(sv630p_handle_t *dev, SV630P_DIDO_FunIN func)
{
    SV630P_FuncTypeMask2 mask;
    SV630P_ARGS_CHECK(dev);

    if (find_mask2_by_funin(func, &mask) == 0)
        return dev->ops->write_single_reg(dev, REG_H03_01_POWER_ON_DI2, mask);
    else
        return SV630P_INDEX_ERR;
}

/**
 * @brief DI1端子功能选择
 * @param dev
 * @param type
 * @return
 * note:实时生效
*/
int sv630p_h03_02_read_di1_function_selection(sv630p_handle_t *dev, DiFunctionType *type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_02_DI1_FUNC, 1, (uint16_t *)type);
}

/**
 * @brief DI1端子功能选择
 * @param dev
 * @param type - 设置硬件DI1端子对应的DI功能
*/
int sv630p_h03_02_write_di1_function_selection(sv630p_handle_t *dev, DiFunctionType type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_02_DI1_FUNC, type);
}

int sv630p_h03_03_read_di1_logic(sv630p_handle_t *dev, DI_LogicType *logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_03_DI1_LOGIC, 1, (uint16_t *)logic);
}

int sv630p_h03_03_write_di1_logic(sv630p_handle_t *dev, DI_LogicType logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_03_DI1_LOGIC, (uint16_t)logic);
}

int sv630p_h03_04_read_di2_function(sv630p_handle_t *dev, DiFunctionType *type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_04_DI2_FUNC, 1, (uint16_t *)type);
}

int sv630p_h03_04_write_di2_function(sv630p_handle_t *dev, DiFunctionType type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_04_DI2_FUNC, type);
}

int sv630p_h03_05_read_di2_logic(sv630p_handle_t *dev, DI_LogicType *logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_05_DI2_LOGIC, 1, (uint16_t *)logic);
}

int sv630p_h03_05_write_di2_logic(sv630p_handle_t *dev, DI_LogicType logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_05_DI2_LOGIC, (uint16_t)logic);
}

int sv630p_h03_06_read_di3_function(sv630p_handle_t *dev, DiFunctionType *type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_06_DI3_FUNC, 1, (uint16_t *)type);
}

int sv630p_h03_06_write_di3_function(sv630p_handle_t *dev, DiFunctionType type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_06_DI3_FUNC, type);
}

int sv630p_h03_07_read_di3_logic(sv630p_handle_t *dev, DI_LogicType *logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_07_DI3_LOGIC, 1, (uint16_t *)logic);
}

int sv630p_h03_07_write_di3_logic(sv630p_handle_t *dev, DI_LogicType logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_07_DI3_LOGIC, (uint16_t)logic);
}

int sv630p_h03_08_read_di4_function(sv630p_handle_t *dev, DiFunctionType *type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_08_DI4_FUNC, 1, (uint16_t *)type);
}

int sv630p_h03_08_write_di4_function(sv630p_handle_t *dev, DiFunctionType type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_08_DI4_FUNC, type);
}

int sv630p_h03_09_read_di4_logic(sv630p_handle_t *dev, DI_LogicType *logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_09_DI4_LOGIC, 1, (uint16_t *)logic);
}

int sv630p_h03_09_write_di4_logic(sv630p_handle_t *dev, DI_LogicType logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_09_DI4_LOGIC, (uint16_t)logic);
}

int sv630p_h03_10_read_di5_function(sv630p_handle_t *dev, DiFunctionType *type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_10_DI5_FUNC, 1, (uint16_t *)type);
}

int sv630p_h03_10_write_di5_function(sv630p_handle_t *dev, DiFunctionType type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_10_DI5_FUNC, type);
}

int sv630p_h03_11_read_di5_logic(sv630p_handle_t *dev, DI_LogicType *logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_11_DI5_LOGIC, 1, (uint16_t *)logic);
}

int sv630p_h03_11_write_di5_logic(sv630p_handle_t *dev, DI_LogicType logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_11_DI5_LOGIC, (uint16_t)logic);
}

int sv630p_h03_16_read_di8_function(sv630p_handle_t *dev, DiFunctionType *type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_16_DI8_FUNC, 1, (uint16_t *)type);
}

int sv630p_h03_16_write_di8_function(sv630p_handle_t *dev, DiFunctionType type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_16_DI8_FUNC, type);
}

int sv630p_h03_17_read_di8_logic(sv630p_handle_t *dev, DI_LogicType *logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_17_DI8_LOGIC, 1, (uint16_t *)logic);
}

int sv630p_h03_17_write_di8_logic(sv630p_handle_t *dev, DI_LogicType logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_17_DI8_LOGIC, (uint16_t)logic);
}

int sv630p_h03_18_read_di9_function(sv630p_handle_t *dev, DiFunctionType *type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_18_DI9_FUNC, 1, (uint16_t *)type);
}

int sv630p_h03_18_write_di9_function(sv630p_handle_t *dev, DiFunctionType type)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_18_DI9_FUNC, type);
}

int sv630p_h03_19_read_di9_logic(sv630p_handle_t *dev, DI_LogicType *logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->read_holding_regs(dev, REG_H03_19_DI9_LOGIC, 1, (uint16_t *)logic);
}

int sv630p_h03_19_write_di9_logic(sv630p_handle_t *dev, DI_LogicType logic)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H03_19_DI9_LOGIC, (uint16_t)logic);
}

// ====== SV630P_FuncTypeMask3 mapping table and function ======
static const funin_map_entry_t funin_map3[] = {
    {     SV630P_FunIN_33_XintInhibit, SV630P_DIFUNC3_FUNIN_33,     "Interrupt fixed length prohibit"},
    {   SV630P_FunIN_34_EmergencyStop, SV630P_DIFUNC3_FUNIN_34,                      "Emergency stop"},
    {       SV630P_FunIN_35_ClrPosErr, SV630P_DIFUNC3_FUNIN_35,                "Clear position error"},
    {        SV630P_FunIN_36_V_LmtSel, SV630P_DIFUNC3_FUNIN_36,         "Internal speed limit source"},
    {    SV630P_FunIN_37_PulseInhibit, SV630P_DIFUNC3_FUNIN_37,               "Pulse command inhibit"},
    {  SV630P_FunIN_38_MultiBlockTrig, SV630P_DIFUNC3_FUNIN_38,                 "Multi-block trigger"},
    {    SV630P_FunIN_39_MultiBlockWr, SV630P_DIFUNC3_FUNIN_39,                   "Multi-block write"},
    {SV630P_FunIN_40_ClrCmdOkAndArrOk, SV630P_DIFUNC3_FUNIN_40, "Clear command and position complete"},
    {      SV630P_FunIN_41_HomeRecord, SV630P_DIFUNC3_FUNIN_41,        "Set current position as home"},
    // Add more if FunIN.42~FunIN.47 are defined in your SV630P_DIDO_FunIN
};

#define FUNIN_MAP3_SIZE(x) (sizeof(x) / sizeof(x[0]))

int find_mask3_by_funin(SV630P_DIDO_FunIN funin, SV630P_FuncTypeMask3 *mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP3_SIZE(funin_map3); i++)
    {
        if (funin_map3[i].funin == funin)
        {
            *mask = (SV630P_FuncTypeMask3)funin_map3[i].mask;
            return 0;
        }
    }
    return -1;
}

int find_index_by_mask3(SV630P_FuncTypeMask3 mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP3_SIZE(funin_map3); i++)
    {
        if ((SV630P_FuncTypeMask3)funin_map3[i].mask == mask)
        {
            return i;
        }
    }
    return -1;
}

char *find_funin_desc_by_mask3(SV630P_FuncTypeMask3 mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP3_SIZE(funin_map3); i++)
    {
        if ((SV630P_FuncTypeMask3)funin_map3[i].mask == mask)
        {
            return (char *)funin_map3[i].desc;
        }
    }
    return NULL;
}

int sv630p_h03_34_read_power_on_di_function(sv630p_handle_t *dev, SV630P_DIDO_FunIN *funin)
{
    uint16_t value = 0;
    int      index = -1;
    SV630P_ARGS_CHECK(dev);
    if (dev->ops->read_holding_regs(dev, REG_H03_34_POWER_ON_DI3, 1, &value) == SV630P_OK)
    {
        index = find_index_by_mask3(value);
        if (index >= 0)
        {
            SV630_DEBUG(dev, "DI function: %s\n", funin_map3[index].desc);
            *funin = funin_map3[index].funin;
            return SV630P_OK;
        }
        else
        {
            SV630_DEBUG(dev, "DI function: %s\n", "Unknown");
            return SV630P_INDEX_ERR;
        }
    }
    return SV630P_BUS_ERR;
}

int sv630p_h03_34_write_power_on_di_function(sv630p_handle_t *dev, SV630P_DIDO_FunIN func)
{
    SV630P_FuncTypeMask3 mask;
    SV630P_ARGS_CHECK(dev);

    if (find_mask3_by_funin(func, &mask) == 0)
        return dev->ops->write_single_reg(dev, REG_H03_34_POWER_ON_DI3, mask);
    else
        return SV630P_INDEX_ERR;
}

// ====== SV630P_FuncTypeMask4 mapping table and function ======
static const funin_map_entry_t funin_map4[] = {
    // Add mapping for FunIN.49~FunIN.63 if defined in SV630P_DIDO_FunIN
    // {SV630P_FunIN_49_xxx, SV630P_DIFUNC4_FUNIN_49, "FunIN.49 description"},
    {SV630P_FunIN_33_XintInhibit, SV630P_DIFUNC3_FUNIN_33, "Interrupt fixed length prohibit"}, // for build error, not find define
};

#define FUNIN_MAP4_SIZE(x) (sizeof(x) / sizeof(x[0]))

int find_mask4_by_funin(SV630P_DIDO_FunIN funin, SV630P_FuncTypeMask4 *mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP4_SIZE(funin_map4); i++)
    {
        if (funin_map4[i].funin == funin)
        {
            *mask = (SV630P_FuncTypeMask4)funin_map4[i].mask;
            return 0;
        }
    }
    return -1;
}

int find_index_by_mask4(SV630P_FuncTypeMask4 mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP4_SIZE(funin_map4); i++)
    {
        if ((SV630P_FuncTypeMask4)funin_map4[i].mask == mask)
        {
            return i;
        }
    }
    return -1;
}

char *find_funin_desc_by_mask4(SV630P_FuncTypeMask4 mask)
{
    for (uint8_t i = 0; i < FUNIN_MAP4_SIZE(funin_map4); i++)
    {
        if ((SV630P_FuncTypeMask4)funin_map4[i].mask == mask)
        {
            return (char *)funin_map4[i].desc;
        }
    }
    return NULL;
}

int sv630p_h03_35_read_power_on_di_function(sv630p_handle_t *dev, SV630P_DIDO_FunIN *funin)
{
    uint16_t value = 0;
    int      index = -1;
    SV630P_ARGS_CHECK(dev);
    if (dev->ops->read_holding_regs(dev, REG_H03_35_POWER_ON_DI4, 1, &value) == SV630P_OK)
    {
        index = find_index_by_mask4(value);
        if (index >= 0)
        {
            SV630_DEBUG(dev, "DI function: %s\n", funin_map4[index].desc);
            *funin = funin_map4[index].funin;
            return SV630P_OK;
        }
        else
        {
            SV630_DEBUG(dev, "DI function: %s\n", "Unknown");
            return SV630P_INDEX_ERR;
        }
    }
    return SV630P_BUS_ERR;
}

int sv630p_h03_35_write_power_on_di_function(sv630p_handle_t *dev, SV630P_DIDO_FunIN func)
{
    SV630P_FuncTypeMask4 mask;
    SV630P_ARGS_CHECK(dev);

    if (find_mask4_by_funin(func, &mask) == 0)
        return dev->ops->write_single_reg(dev, REG_H03_35_POWER_ON_DI4, mask);
    else
        return SV630P_INDEX_ERR;
}


