/*
 * @Date: 2025-06-26 14:49:09
 * @Author: mypx
 * @LastEditors: mypx mypx_coder@163.com
 * @LastEditTime: 2025-08-29 09:08:23
 * @FilePath: servo.c
 * @Description: 
 * Copyright (c) 2025 by mypx, All Rights Reserved.
 */
#include "servo.h"
#include "nanomodbus.h"
#include "pm_board.h"
#include "rtdef.h"
#include "rtthread.h"

#if (ENABLE_SV630P == 1)

#define SERVO_RTU_ADDRESS 0x01

nmbs_t          servo_nmbs;
sv630p_ops_t    sv630p_ops;
sv630p_handle_t sv630p_handle;

#if (DEBUG_SERVO_ENABLE == 1)
#define DBG_SERVO(...) rt_kprintf("[sv630p] " __VA_ARGS__)
#else
#define DBG_SERVO(...)
#endif

int servo_read_holding_regs(sv630p_handle_t *dev, uint16_t address, uint16_t quantity, uint16_t *data)
{
    nmbs_error ret = NMBS_ERROR_NONE;

    ret = nmbs_read_holding_registers(dev->mb_handle, address, quantity, data);
    if (ret != NMBS_ERROR_NONE)
    {
        DBG_SERVO("Read holding regs failed: address:0x%02X, quantity:%d, error:%d", address, quantity, ret);
        return SV630P_MB_ERR;
    }
    DBG_SERVO("Read holding regs: address:0x%02X, quantity:%d success", address, quantity);
    return SV630P_OK;
}

int servo_read_input_regs(sv630p_handle_t *dev, uint16_t address, uint16_t quantity, uint16_t *data)
{
    nmbs_error ret = NMBS_ERROR_NONE;

    ret = nmbs_read_input_registers(dev->mb_handle, address, quantity, data);
    if (ret != NMBS_ERROR_NONE)
    {
        DBG_SERVO("Read input regs failed: address:0x%02X, quantity:%d, error:%d", address, quantity, ret);
        return SV630P_MB_ERR;
    }
    DBG_SERVO("Read input regs: address:0x%02X, quantity:%d success", address, quantity);
    return SV630P_OK;
}

int servo_write_single_reg(sv630p_handle_t *dev, uint16_t address, uint16_t value)
{
    nmbs_error ret = NMBS_ERROR_NONE;

    ret = nmbs_write_single_register(dev->mb_handle, address, value);
    if (ret != NMBS_ERROR_NONE)
    {
        DBG_SERVO("Write single reg failed: address:0x%02X, value:%d, error:%d", address, value, ret);
        return SV630P_MB_ERR;
    }
    DBG_SERVO("Write single reg: address:0x%02X, value:%d success", address, value);
    return SV630P_OK;
}

int servo_write_multiple_regs(sv630p_handle_t *dev, uint16_t address, uint16_t quantity, uint16_t *data)
{
    nmbs_error ret = NMBS_ERROR_NONE;

    ret = nmbs_write_multiple_registers(dev->mb_handle, address, quantity, data);
    if (ret != NMBS_ERROR_NONE)
    {
        DBG_SERVO("Write multiple regs failed: address:0x%02X, quantity:%d, error:%d", address, quantity, ret);
        return SV630P_MB_ERR;
    }
    DBG_SERVO("Write multiple regs: address:0x%02X, quantity:%d success", address, quantity);
    return SV630P_OK;
}

void servo_debug_printf(const char *format, ...)
{
    rt_kprintf("[sv-debug] ");
    rt_kprintf(format);
}


void servo_error_printf(const char *format, ...)
{
    rt_kprintf("[sv-error] ");
    rt_kprintf(format);
}


int servo_init(void)
{
    int                ret = 0;
    nmbs_platform_conf stm32_conf;
    // board layer init
    pm_servo_init();
    // nmbs layer init
    nmbs_platform_conf_create(&stm32_conf);
    stm32_conf.transport = NMBS_TRANSPORT_RTU;
    stm32_conf.read      = pm_servo_uart_read;
    stm32_conf.write     = pm_servo_uart_write;

    nmbs_error status = nmbs_client_create(&servo_nmbs, &stm32_conf);
    if (status != NMBS_ERROR_NONE)
    {
        return status;
    }
    nmbs_set_destination_rtu_address(&servo_nmbs, SERVO_RTU_ADDRESS);
    nmbs_set_byte_timeout(&servo_nmbs, 1000);
    nmbs_set_read_timeout(&servo_nmbs, 1000);

    // sv630p driver init
    sv630p_ops.read_holding_regs   = servo_read_holding_regs;
    sv630p_ops.read_input_regs     = servo_read_input_regs;
    sv630p_ops.write_single_reg    = servo_write_single_reg;
    sv630p_ops.write_multiple_regs = servo_write_multiple_regs;


    sv630p_handle.trace.debug = servo_debug_printf; // set debug function
    sv630p_handle.trace.error = servo_error_printf; // set error function

    ret = sv630p_init(&sv630p_handle, &sv630p_ops, &servo_nmbs);
    if (ret != 0)
    {
        DBG_SERVO("sv630p_init failed");
        return -1;
    }
    sv630p_handle.init = true;

    sv630p_speed_mode_stop(&sv630p_handle);
    if (sv630p_speed_mode_init(&sv630p_handle) != 0)
    {
        DBG_SERVO("sv630p_speed_mode_init failed.");
        return -1;
    }

    return 0;
}
#endif // ENABLE_SV630P