/*
 * @Date: 2025-07-02 13:22:04
 * @Author: mypx
 * @LastEditors: mypx mypx_coder@163.com
 * @LastEditTime: 2025-09-12 13:59:23
 * @FilePath: sv_device.c
 * @Description: 
 * Copyright (c) 2025 by mypx, All Rights Reserved.
 */
#include "sv_device.h"
#include "etk_utils.h"
#include "et_log.h"
#include "h02_basic_control.h"
#include "h03_di_do.h"
#include "h05_pos_control.h"
#include "h06_speed_control.h"
#include "h0d_af.h"
#include "h31_comm_related_var.h"

// FIXME: this function is not debug
int sv630p_position_mode_init(sv630p_handle_t *dev)
{
    sv630p_h02_00_write_control_mode(dev, SV630P_CONTROL_MODE_POSITION);
    sv630p_h05_00_write_command_source(dev, SV630P_POS_CMD_STEP);
    // 关键DI配置
    sv630p_h03_01_write_power_on_di_func_assign2(dev, SV630P_FunIN_1_S_ON);
    sv630p_h03_01_write_power_on_di_func_assign2(dev, SV630P_FunIN_27_POSDirSel);
    sv630p_h03_02_write_di1_function_selection(dev, DI_FUNC_CLEAR_POS_ERROR);
    // 关键DO配置
    return 0;
}

int sv630p_speed_mode_init(sv630p_handle_t *dev)
{
    int ret = 0;

    dev->rpm = 0;
    dev->dir = SV630P_DIR_UNKNOWN;

    ret = sv630p_h02_00_write_control_mode(dev, SV630P_CONTROL_MODE_SPEED);
    ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });
    // ret = sv630p_h06_00_write_main_speed_cmd_a_src(dev, DIGITAL_VALUE_SPECIFIED_CMD_A);
    // ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });

    ret = sv630p_h06_02_write_speed_cmd_select(dev, SV630P_SPEED_CMD_COMMUNICATION);
    ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });

    return 0;
err:
    dev->is_error = true;
    ET_ERR("Failed to init speed mode: %d", ret);
    return -1;
}

int sv630p_speed_mode_start(sv630p_handle_t *dev, double rpm)
{
    int ret = 0;

    //ET_INFO("Start speed mode, rpm:%d", (int)rpm);
    ret = sv630p_h31_09_write_speed_command(dev, rpm);
    ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });

    ret = sv630p_h0d_17_write_force_output_mode(dev, SV630P_DIDO_FORCE_DI_ENABLE);
    ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });
    /* FIXME: The default function of DI5 is servo enabling. Please check the function selection 
    of the H03.10 - DI5 terminal and the logic selection of the H03.11 - DI5 terminal.*/
    ret = sv630p_h0d_18_write_force_input_bits(dev, NOT_CONCERN_BIT, DI5_BIT);
    ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });
    dev->last_rpm = rpm;
    dev->rpm      = rpm;
    dev->dir      = rpm > 0 ? SV630P_DIR_FORWARD : SV630P_DIR_BACKWARD;
    return 0;
err:
    ET_ERR("Failed to start speed mode: %d", ret);
    dev->is_error = true;
    return -1;
}

int sv630p_speed_mode_resume(sv630p_handle_t *dev)
{
    return sv630p_speed_mode_start(dev, dev->last_rpm);
}

int sv630p_speed_update(sv630p_handle_t *dev, double rpm)
{
    int ret = 0;

    SV630P_Dir dir = rpm > 0 ? SV630P_DIR_FORWARD : SV630P_DIR_BACKWARD;

    if (ETK_FLOAT_ALMOST_EQUAL(dev->rpm, rpm, 0.005))
    {
        //ET_DBG("rpm diff small %.2f", rpm);
        return 0;
    }
    dev->last_rpm = rpm;
    if (dev->dir != dir)
    {
        ret = sv630p_speed_mode_stop(dev);
        ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });
        rt_thread_mdelay(50); // 等待停止完成
        ret = sv630p_speed_mode_start(dev, rpm);
        ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });
    }
    else
    {
        ET_INFO("Update rpm: %.2f", rpm);
        ret = sv630p_h31_09_write_speed_command(dev, rpm);
        ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });
        dev->rpm = rpm;
        dev->dir = dir;
    }
    return 0;
err:
    dev->is_error = true;
    ET_ERR("Failed to start speed mode: %d", ret);
    return -1;
}

int sv630p_dir_change(sv630p_handle_t *dev)
{
    int ret = 0;

    double rpm = dev->last_rpm * -1.0;

    ET_INFO("Change dir, rpm:%d", (int)rpm);
    ret = sv630p_speed_mode_stop(dev);
    ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });
    rt_thread_mdelay(50); // 等待停止完成
    ret = sv630p_speed_mode_start(dev, rpm);
    ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });

    return 0;
err:
    dev->is_error = true;
    ET_ERR("Failed to change direction: %d", ret);
    return -1;
}


int sv630p_speed_mode_stop(sv630p_handle_t *dev)
{
    int ret = 0;

    ret = sv630p_h0d_17_write_force_output_mode(dev, SV630P_DIDO_FORCE_NONE);
    ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });
    /* FIXME: The default function of DI5 is servo enabling. Please check the function selection 
    of the H03.10 - DI5 terminal and the logic selection of the H03.11 - DI5 terminal.*/
    ret = sv630p_h0d_18_write_force_input_bits(dev, DI5_BIT, NOT_CONCERN_BIT);
    ERR_CHECK_RET_NONZERO_ACT(ret, { goto err; });
    //ET_DBG("Servo STOP from dir:%d", dev->dir);
    dev->rpm = 0;
    dev->dir = SV630P_DIR_STOP;

    return 0;
err:
    dev->is_error = true;
    ET_ERR("Failed to stop speed mode: %d", ret);
    return -1;
}

double sv630p_get_last_rpm(sv630p_handle_t *dev)
{
    return dev->last_rpm;
}

SV630P_Dir sv630p_get_dir(sv630p_handle_t *dev)
{
    return dev->dir;
}