/*
 * @Date: 2025-07-01 17:18:39
 * @Author: mypx
 * @LastEditors: mypx mypx_coder@163.com
 * @LastEditTime: 2025-07-22 09:22:46
 * @FilePath: h05_pos_control.c
 * @Description: 
 * Copyright (c) 2025 by mypx, All Rights Reserved.
 */
#include "h05_pos_control.h"

/**
 * @brief Read main position command source
 * @param dev Pointer to servo handle
 * @param src Pointer to store the read command source
 * @return SV630P_OK on success, error code otherwise
 * @note In position control mode, used to select the position command source.
 *       Pulse command is external, step and multi-segment are internal.
 */
int sv630p_h05_00_read_command_source(sv630p_handle_t *dev, SV630P_PositionCommandSource *src)
{
    uint16_t value = 0;
    SV630P_ARGS_CHECK(dev);
    if (dev->ops->read_holding_regs(dev, REG_H05_00_POSITION_CMD_SRC, 1, &value) == SV630P_OK)
    {
        *src = (SV630P_PositionCommandSource)value;
        return SV630P_OK;
    }
    return SV630P_BUS_ERR;
}

/**
 * @brief Write main position command source
 * @param dev Pointer to servo handle
 * @param src Command source to write
 * @return SV630P_OK on success, error code otherwise
 */
int sv630p_h05_00_write_command_source(sv630p_handle_t *dev, SV630P_PositionCommandSource src)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H05_00_POSITION_CMD_SRC, (uint16_t)src);
}

/**
 * @brief Read pulse input mode
 * @param dev Pointer to servo handle
 * @param mode Pointer to store the read pulse input mode
 * @return SV630P_OK on success, error code otherwise
 */
int sv630p_h05_01_read_pulse_input_mode(sv630p_handle_t *dev, SV630P_PulseInputMode *mode)
{
    uint16_t value = 0;
    SV630P_ARGS_CHECK(dev);
    if (dev->ops->read_holding_regs(dev, REG_H05_01_PULSE_INPUT_MODE, 1, &value) == SV630P_OK)
    {
        *mode = (SV630P_PulseInputMode)value;
        return SV630P_OK;
    }
    return SV630P_BUS_ERR;
}

int sv630p_h05_01_write_pulse_input_mode(sv630p_handle_t *dev, SV630P_PulseInputMode mode)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H05_01_PULSE_INPUT_MODE, (uint16_t)mode);
}

int sv630p_h05_02_read_position_unit(sv630p_handle_t *dev, uint16_t *unit)
{
    uint16_t value = 0;
    SV630P_ARGS_CHECK(dev);
    if (dev->ops->read_holding_regs(dev, REG_H05_02_POSITION_UNIT, 1, &value) == SV630P_OK)
    {
        *unit = value;
        return SV630P_OK;
    }
    return SV630P_BUS_ERR;
}

/**
 * @brief Set the number of position commands required for motor to rotate 1 circle.
 * @description: For example, when motor encoder resolution is 2^18 (262144 pulses/rev), 
 *               mechanical gear ratio 1:1, H05.02 = 262144.
 */
int sv630p_h05_02_write_position_unit(sv630p_handle_t *dev, uint16_t unit)
{
    SV630P_ARGS_CHECK(dev);
    return dev->ops->write_single_reg(dev, REG_H05_02_POSITION_UNIT, unit);
}