/*
 * @Author: mypx
 * @Date: 2025-08-11 08:30:45
 * @LastEditTime: 2025-09-24 16:12:52
 * @LastEditors: mypx mypx_coder@163.com
 * @Description: 
 */
#include "pm_meas.h"
#include "data_process.h"
#include "data_sampling.h"
#include "et_waveform_analysis.h"
#include "etk_diff.h"
#include "etk_ringbuffer.h"
#include "servo.h"
#include <float.h>
#include <rtthread.h>
#include <stdio.h>


static pm_fsm_t *pm_fsm         = NULL;

pm_fsm_t *get_pm_fsm(void)
{
    return pm_fsm;
}

void fsm_state_toggle(void)
{
    pm_fsm_t *fsm = get_pm_fsm();
    if (!fsm)
        return;
    fsm->fz_fsm_state = !fsm->fz_fsm_state;
}
MSH_CMD_EXPORT_ALIAS(fsm_state_toggle, fsm, calculate state toggle);

void calibrate_mode_toggle(void)
{
    pm_fsm_t *fsm = get_pm_fsm();
    if (!fsm)
        return;
    fsm->fz_fsm_state = false;
    fsm->meas_mode = fsm->meas_mode == PM_MEAS_NORMAL_Mode ? PM_MEAS_TEST_Mode : PM_MEAS_NORMAL_Mode;
}
MSH_CMD_EXPORT_ALIAS(calibrate_mode_toggle, cali, calibrate mode toggle);

void trend_handle_init(trend_handle_t *handle)
{
    handle->last_val = NAN;
    handle->inc_cnt  = 0;
    handle->dec_cnt  = 0;
}

TrendStatus get_data_trend(trend_handle_t *handle, double new_val)
{
    const double EPS = 1e-6; // 浮点数比较精度（避免误差）

    // 1. 首次调用：无历史数据，返回“未知”并初始化上次值
    if (isnan(handle->last_val))
    {
        handle->last_val = new_val;
        return TREND_UNKNOWN;
    }

    // 2. 比较本次与上次数据，更新计数
    if (new_val > handle->last_val + EPS)
    {
        // 数据上升
        handle->inc_cnt++;
        handle->dec_cnt = 0;
    }
    else if (new_val < handle->last_val - EPS)
    {
        // 数据下降
        handle->dec_cnt++;
        handle->inc_cnt = 0;
    }

    handle->last_val = new_val;
    // 3. 判断是否达到“连续6次”的触发阈值（原逻辑：cnt > 5）
    if (handle->inc_cnt > TREND_MAX_CNT)
    {
        //inc_cnt  = 0; // 触发后清零，重新开始计数
        return TREND_INCREASE;
    }
    if (handle->dec_cnt > TREND_MAX_CNT)
    {
        //dec_cnt  = 0; // 触发后清零，重新开始计数
        return TREND_DECREASE;
    }
    return TREND_STABLE;
}

void trend_handle_reset(trend_handle_t *handle)
{
    handle->last_val = NAN;
    handle->inc_cnt  = 0;
    handle->dec_cnt  = 0;
}

void measurement_info_print(pm_meas_t *fz)
{
    ET_WARN("------------------------Calibration Info Start----------------------------");
    ET_INFO("Find zero time: %lu ms", fz->calib_info[0].end_time - fz->calib_info[0].start_time);
    float min_angle = FLT_MAX;
    float max_angle = -FLT_MAX;

    for (int i = 0; i < fz->calib_cnt; i++)
    {
#if (ENABLE_CALIB_DOUBLE_DIR == 1)
        if (fz->calib_info[i].m_dir == -1)
            fz->calib_info[i].cali_angle = fz->calib_info[i].angle - CALIB_OFFSET;
        else
#endif
            fz->calib_info[i].cali_angle = fz->calib_info[i].angle;
        if (fz->calib_info[i].cali_angle < min_angle)
        {
            min_angle = fz->calib_info[i].cali_angle;
        }
        if (fz->calib_info[i].cali_angle > max_angle)
        {
            max_angle = fz->calib_info[i].cali_angle;
        }
        // ET_INFO("[%d]: Used time:%d ms, start:%.3f, end zero pos:%.3f, cali_angle:%.2f, power:%.3f", i,
        //         fz->calib_info[i].end_time - fz->calib_info[i].start_time, fz->calib_info[i].target_angle,
        //         fz->calib_info[i].angle, fz->calib_info[i].cali_angle, fz->calib_info[i].power);
        ET_INFO("[%d]: Used time:%d ms, start:%.3f, end zero pos:%.3f, power:%.3f", i,
                fz->calib_info[i].end_time - fz->calib_info[i].start_time, fz->calib_info[i].target_angle,
                fz->calib_info[i].angle, fz->calib_info[i].power);
    }
    ET_INFO("Final min_angle: %.3f, max_angle: %.3f", min_angle, max_angle);
    if (min_angle != FLT_MAX && max_angle != FLT_MIN)
    {
        float accuracy = (max_angle - min_angle) / ENCODER_TO_POLARIZER_RATIO;
        ET_INFO("Accuracy: %.3f (max_angle-min_angle=%.3f, ratio=%d)", accuracy, max_angle - min_angle,
                ENCODER_TO_POLARIZER_RATIO);
    }
    else
    {
        ET_WARN("Accuracy not calculated: min_angle=%.3f, max_angle=%.3f", min_angle, max_angle);
    }
    ET_WARN("------------------------Calibration Info End----------------------------");
}

void accuracy_auto_measurement_once(pm_meas_t *fz, float angle)
{
    ET_INFO("Start to calibrate again:%d", fz->calib_cnt);

#if (CALIB_TO_FIX_STAGE == 1)
    float diff = angle - fz->stage_info[ZF_PREPARE_ENTER_STAGE].angle;
#else
    float diff = CALIB_OSCI_ANGLE;
#endif
    ET_INFO("diff:%.2f", diff);
    float target_angle = 0.0f;
#if (ENABLE_CALIB_DOUBLE_DIR == 1)
    if (fz->calib_cnt % 2 == 0)
    {
        target_angle = angle - diff;
        ET_INFO("Odd times, turn back");
    }
    else
    {
        target_angle = angle + diff;
        ET_INFO("Even times, turn forward");
    }
#else
    target_angle = angle + diff * CALIB_ONE_DIR;
#endif
    uint32_t delay = convert_encoder_angle_rpm_to_to_time(diff, RUN_OFFSET_ANGLE_RPM);
    ET_DBG("wait %d ms to target:%.2f", delay, target_angle);
    int speed = (target_angle > angle) ? -RUN_OFFSET_ANGLE_RPM
                                       : RUN_OFFSET_ANGLE_RPM; // FIXME:note encoder dir and motor dir
    sv630p_speed_mode_start(&sv630p_handle, speed);
    fz->target_dir = (speed > 0) ? -1 : 1;
    ET_WARN("SPEED:%d", speed);
    while (1)
    {
        angle = get_real_time_angle_degree();
        //ET_DBG("angle:%.2f", angle);
        // 根据方向判断是否达到目标角度，处理负数角度情况
        if (((speed > 0) && (target_angle > angle)) || ((speed < 0) && (target_angle < angle)))
        {
            sv630p_speed_mode_stop(&sv630p_handle);
            ET_INFO("Set angle:%.3f, reached:%.3f", target_angle, angle);
#if (CALIB_TO_FIX_STAGE == 1)
            fz->state = ZF_PREPARE_ENTER_STAGE;
            ET_WARN("Back to ZF_PREPARE_ENTER_STAGE state");
#else
            fz->state = ZF_FAST_CLOSE_STAGE;
            ET_WARN("Back to ZF_FAST_CLOSE_STAGE state");
#endif
            uint8_t index                      = fz->calib_cnt % FZ_CALI_MAX_COUNT;
            fz->calib_info[index].target_angle = target_angle;
            ET_INFO("update start calib index:%d", index);
            fz->calib_info[index].start_time = rt_tick_get_millisecond();
            //fz->target_dir                   *= -1; // reverse dir

            break;
        }
    }
}

// 饱和高噪声状态判断，不会误判为其他状态，任何情形都无需从这个状态反转
StageResult is_saturate_starting(pm_meas_t *fz)
{
    if (fabs(fz->slope) < FZ_SATURATED_SLOPE_MAX && fz->p50 < FZ_SATURATED_POWER_MAX && (fz->freq < 10))
    {
        //fz->state       = ZF_SATURATED_STAGE;
        return STAGE_RESULT_OK;
    }
    else
        return STAGE_RESULT_NO;
}

int goto_saturated_stage(pm_meas_t *fz)
{
    fz->state = ZF_SATURATED_STAGE;
    ET_WARN("power state: ZF_SATURATED_STAGE");
    return sv630p_speed_update(&sv630p_handle, FZ_SATURATED_STAGE_RPM * fz->target_dir);
}

StageResult is_wave_enter_stage(pm_meas_t *fz)
{
    if (fz->slope > 100000)
        return STAGE_RESULT_OK;
    else
        return STAGE_RESULT_NO;
}

StageResult is_prepare_enter_stage(pm_meas_t *fz)
{
    if (fz->p50 > 100000 && fz->trend == TREND_DECREASE && fz->state != ZF_WAVE_LEAVE_STAGE)
        return STAGE_RESULT_OK;
    else
        return STAGE_RESULT_NO;
}

int goto_prepare_enter_stage(pm_meas_t *fz, double p50, double slope, float freq, double angle)
{
    fz->state = ZF_PREPARE_ENTER_STAGE;
    if (fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_updated == 0)
    {
        fz->stage_info[ZF_PREPARE_ENTER_STAGE].angle      = angle;
        fz->stage_info[ZF_PREPARE_ENTER_STAGE].time       = rt_tick_get_millisecond();
        fz->stage_info[ZF_PREPARE_ENTER_STAGE].p50        = p50;
        fz->stage_info[ZF_PREPARE_ENTER_STAGE].slope      = slope;
        fz->stage_info[ZF_PREPARE_ENTER_STAGE].freq       = freq;
        fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_updated = 1;
    }
    ET_WARN("enter ZF_PREPARE_ENTER_STAGE angle:%.3f", angle);
    return sv630p_speed_update(&sv630p_handle, FZ_PREPARE_STAGE_RPM * fz->target_dir);
}

int goto_prepare_leave_stage(pm_meas_t *fz, double angle)
{
    fz->target_dir = fz->target_dir * -1;
    if (fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_updated == 0)
    {
        fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_updated = 1;
        fz->stage_info[ZF_PREPARE_LEAVE_STAGE].angle      = angle;
        fz->stage_info[ZF_PREPARE_LEAVE_STAGE].time       = rt_tick_get_millisecond();
        fz->stage_info[ZF_PREPARE_LEAVE_STAGE].p50        = fz->p50;
        fz->stage_info[ZF_PREPARE_LEAVE_STAGE].slope      = fz->slope;
        fz->stage_info[ZF_PREPARE_LEAVE_STAGE].freq       = fz->freq;
    }
    ET_WARN("ZF_PREPARE_LEAVE_STAGE, angle:%.3f", angle);
    return sv630p_speed_update(&sv630p_handle, FZ_PREPARE_STAGE_RPM * fz->target_dir);
}

StageResult is_fast_close_enter(pm_meas_t *fz)
{
    if (fz->p50 <= 100000 && fz->trend == TREND_DECREASE && fz->state == ZF_PREPARE_ENTER_STAGE)
        return STAGE_RESULT_OK;
    else
        return STAGE_RESULT_NO;
}

int goto_fast_close_stage(pm_meas_t *fz, double p50, double slope, float freq, double angle)
{
    fz->state = ZF_FAST_CLOSE_STAGE;
    if (fz->stage_info[ZF_FAST_CLOSE_STAGE].is_updated == 0)
    {
        fz->stage_info[ZF_FAST_CLOSE_STAGE].is_updated = 1;
        fz->stage_info[ZF_FAST_CLOSE_STAGE].angle      = angle;
        fz->stage_info[ZF_FAST_CLOSE_STAGE].time       = rt_tick_get_millisecond();
        fz->stage_info[ZF_FAST_CLOSE_STAGE].p50        = p50;
        fz->stage_info[ZF_FAST_CLOSE_STAGE].slope      = slope;
        fz->stage_info[ZF_FAST_CLOSE_STAGE].freq       = freq;
    }
    ET_WARN("FAST_CLOSE, angle:%.3f\r\n", angle);
    return sv630p_speed_update(&sv630p_handle, FZ_FAST_CLOSE_STAGE_RPM * fz->target_dir);
}

StageResult is_zero_stage(pm_meas_t *fz)
{
    if (fabs(fz->slope) < 0.5 && fz->p50 < ZERO_POS_THRESHOLD && (fz->freq >= 99.95 && fz->freq < 100.3f)
        && fz->trend == TREND_STABLE)
        return STAGE_RESULT_OK;
    else
        return STAGE_RESULT_NO;
}

StageResult is_fast_leave_stage(pm_meas_t *fz)
{
    if ((fz->p50 <= 100000) && (fz->slope > 10000)
        && (fz->freq > FZ_FAST_CLOSE_ENTER_FREQ_MIN && fz->freq < FZ_FAST_CLOSE_ENTER_FREQ_MAX)
        && fz->state != ZF_SATURATED_STAGE)
        return STAGE_RESULT_OK;
    else
        return STAGE_RESULT_NO;
}

StageResult is_prepare_leave_stage(pm_meas_t *fz)
{
    if (fz->p50 >= 100000 && fz->trend == TREND_INCREASE
        && (fz->state == ZF_FAST_CLOSE_STAGE || fz->state == ZF_ZERO_STAGE))
        return STAGE_RESULT_OK;
    else
        return STAGE_RESULT_NO;
}

StageResult is_wave_leave_stage(pm_meas_t *fz)
{
    if ((fz->slope < -100000) || (fz->freq > 52 && fz->freq < 98 && fz->p50 < 1000 && fz->trend == TREND_DECREASE)
        || (fz->freq < 1.0 && fz->trend == TREND_DECREASE))
        return STAGE_RESULT_OK;
    else
        return STAGE_RESULT_NO;
}

ZeroFindState get_find_zero_state(pm_meas_t *fz)
{
    return fz->state;
}

void calculate_stage(pm_meas_t *fz)
{
    if (is_saturate_starting(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_SATURATED_STAGE].is_print == false)
    {
        ET_WARN("enter saturated");
        fz->stage_info[ZF_SATURATED_STAGE].is_print = true;
        fz->state                                   = ZF_SATURATED_STAGE;
    }
    if (is_wave_enter_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_WAVE_ENTER_STAGE].is_print == false)
    {
        ET_WARN("enter 50hz started");
        fz->stage_info[ZF_WAVE_ENTER_STAGE].is_print = true;
        fz->stage_info[ZF_SATURATED_STAGE].is_print  = false;
        fz->state                                    = ZF_WAVE_ENTER_STAGE;
    }
    if (is_prepare_enter_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_print == false)
    {
        ET_WARN("enter prepare stage");
        fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_print = true;
        fz->state                                       = ZF_PREPARE_ENTER_STAGE;
        fz->stage_info[ZF_WAVE_ENTER_STAGE].is_print    = false;
    }
    if (is_fast_close_enter(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_FAST_CLOSE_STAGE].is_print == false)
    {
        ET_WARN("enter fast close stage, dir:%d, target dir :%d", sv630p_get_dir(&sv630p_handle), fz->target_dir);
        fz->stage_info[ZF_FAST_CLOSE_STAGE].is_print    = true;
        fz->stage_info[ZF_PREPARE_ENTER_STAGE].is_print = false;
        fz->state                                       = ZF_FAST_CLOSE_STAGE;
    }

    if (is_zero_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_ZERO_STAGE].is_print == false)
    {
        ET_WARN("enter zero state");
        fz->stage_info[ZF_ZERO_STAGE].is_print = true;
        if (fz->state == ZF_UNKNOWN_STAGE)
            fz->state = ZF_ZERO_STAGE; // 开机判断
    }
    else
    {
        fz->stage_info[ZF_ZERO_STAGE].is_print = false;
    }

    if (is_fast_leave_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_FAST_LEAVE_STAGE].is_print == false)
    {
        ET_WARN("fast leave stage");
        fz->stage_info[ZF_FAST_LEAVE_STAGE].is_print = true;
        //fz->stage_info[ZF_PRECISE_ADJUST_LEAVE_STAGE].is_print = false;
        fz->state = ZF_FAST_LEAVE_STAGE;
    }
    if (is_prepare_leave_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_print == false)
    {
        ET_WARN("prepare leave stage");
        fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_print = true;
        fz->stage_info[ZF_FAST_LEAVE_STAGE].is_print    = false;
        fz->stage_info[ZF_WAVE_LEAVE_STAGE].is_print    = false;
        fz->state                                       = ZF_PREPARE_LEAVE_STAGE;
    }
    if (is_wave_leave_stage(fz) == STAGE_RESULT_OK && fz->stage_info[ZF_WAVE_LEAVE_STAGE].is_print == false)
    {
        ET_WARN("small 50hz leave stage");
        fz->stage_info[ZF_WAVE_LEAVE_STAGE].is_print    = true;
        fz->stage_info[ZF_PREPARE_LEAVE_STAGE].is_print = false;
        fz->state                                       = ZF_WAVE_LEAVE_STAGE;
    }
}

int find_zero_process(pm_meas_t *fz, const uint16_t *adc_buf, uint32_t len)
{
    pm_fsm_t *fsm = get_pm_fsm();
    if (!fsm)
    {
        ET_ERR("fsm is not init");
        return 0;
    }
    double rpm = 0.0;
    //static uint8_t wait_time = 0;
    float angle;

    fz->p50   = et_goertzel_compute_power_u16(&fz->cfg, adc_buf);
    fz->slope = etk_slope_update_slope(&fz->slope_handle, fz->p50);

    // if (wait_time++ < BUFFER_SIZE)
    //     return 0;
    angle = get_real_time_angle_degree();

    fz->freq  = et_zero_cross_freq_u16(&fz->zc, adc_buf, len, sampling_rate, DC_OFFSET_ADC_VALUE);
    fz->trend = get_data_trend(&fz->trend_handle, fz->p50);
    calculate_stage(fz);

    ET_DBG("[%d]tr:%d, s:%.2f, p50:%.2f, freq:%.2f, angle:%.2f", fz->state, fz->trend, fz->slope, fz->p50, fz->freq,
           angle);
    if (fsm->fz_fsm_state)
    {
        return 0;
    }
    if (fz->calib_info[0].start_time == 0)
    {
        fz->calib_info[0].start_time   = rt_tick_get_millisecond();
        fz->calib_info[0].angle        = 0;
        fz->calib_info[0].target_angle = 0;
    }
    switch (fz->state)
    {
    case ZF_UNKNOWN_STAGE:
    case ZF_SATURATED_STAGE:
        fz->target_dir = MEASURE_DEF_DIR;
        sv630p_speed_update(&sv630p_handle, FZ_UNKNOWN_STAGE_RPM * fz->target_dir);
        break;
    case ZF_WAVE_ENTER_STAGE:
        sv630p_speed_update(&sv630p_handle, FZ_WAVE_ENTER_STAGE_RPM * fz->target_dir);
        break;
    case ZF_PREPARE_ENTER_STAGE:
        sv630p_speed_update(&sv630p_handle, FZ_PREPARE_STAGE_RPM * fz->target_dir);
        break;
    case ZF_FAST_CLOSE_STAGE:
        // 优化方向反转逻辑，减少过零点超调
        if (get_data_trend(&fz->trend_handle, fz->p50) == TREND_INCREASE && fz->slope > 200 && fz->p50 < 40000)
        {
            fz->target_dir *= -1;
            etk_position_pid_reset(&fz->pid);      // 重置PID避免累积误差影响
            trend_handle_reset(&fz->trend_handle); // 防止很快的重复进来
            ET_WARN("passed the zero point and rotated in the opposite direction (slope=%.1f, p50=%.0f)", fz->slope,
                    fz->p50);
        }

        if (fz->p50 < ZERO_POS_THRESHOLD)
        {
            fz->zero_stable_count++;
            if (fz->zero_stable_count > ZC_COUNT_STABLE_TIMES || fz->p50 < 0.03)
            {
                fz->state                           = ZF_ZERO_STAGE;
                fz->calib_info[fz->calib_cnt].m_dir = sv630p_get_last_rpm(&sv630p_handle) > 0 ? 1 : -1;
                sv630p_speed_mode_stop(&sv630p_handle);
                uint8_t index                  = fz->calib_cnt % FZ_CALI_MAX_COUNT;
                fz->calib_info[index].angle    = angle;
                fz->calib_info[index].end_time = rt_tick_get_millisecond();
                fz->calib_info[index].power    = fz->p50;
                ET_INFO("update END calib index:%d", index);
                ET_WARN("Find zero done:%.3f", angle);
                fz->calib_cnt++;
                etk_position_pid_reset(&fz->pid);
                fz->target_dir = MEASURE_DEF_DIR;
            }
        }
        else
        {
            fz->zero_stable_count = 0;
#if 1
            // 根据p50值大小采用三级控制策略，优化不同阶段的控制效果
            if (fz->p50 > 40000)
            {
                rpm = 850.0f;
                rpm = rpm * (fz->target_dir);
            }
            else if (fz->p50 > 10000)
            {
                rpm = etk_position_pid_update(&fz->pid, fz->p50, 0.535, 0.7f);
                rpm = rpm * fz->target_dir;
            }
            else
#endif
            {
                // p50较小时使用保守PID控制，降低微分滤波系数以提高稳定性
                rpm = etk_position_pid_update(&fz->pid, fz->p50, 0.535, 0.9f);
                if (rpm > 350.0f)
                    rpm = 350.0f;
                if (rpm < -350.0f)
                    rpm = -350.0f;
                rpm = rpm * fz->target_dir;
            }

            if (fabs(rpm) < FZ_PRECISE_ADJUST_MIN_RPM)
            {
                rpm = (rpm >= 0) ? FZ_PRECISE_ADJUST_MIN_RPM : -FZ_PRECISE_ADJUST_MIN_RPM;
            }
            ET_DBG("rpm:%.2f", rpm);
            sv630p_speed_update(&sv630p_handle, rpm);
        }
        break;
    case ZF_ZERO_STAGE:
        if (fsm->meas_mode == PM_MEAS_TEST_Mode && fz->calib_cnt < FZ_CALI_MAX_COUNT)
        {
            fz->calib_info[fz->calib_cnt].start_time = rt_tick_get_millisecond();
            accuracy_auto_measurement_once(fz, angle);
        }
        else
        {
            sv630p_speed_mode_stop(&sv630p_handle);
            measurement_info_print(fz);
            fsm->fz_fsm_state      = true; // test use
            fz->calib_cnt  = 0;
            fsm->meas_mode = PM_MEAS_NORMAL_Mode;
            fz->target_dir = MEASURE_DEF_DIR;
            //ET_WARN("Mesaure time:%d ms", rt_tick_get_millisecond() - fz->calib_start_time);
        }
        break;
    case ZF_FAST_LEAVE_STAGE:
        fz->target_dir *= -1;
        fz->state       = ZF_FAST_CLOSE_STAGE;
        sv630p_speed_update(&sv630p_handle, FZ_FAST_CLOSE_STAGE_RPM * fz->target_dir);
        break;
    case ZF_PREPARE_LEAVE_STAGE:
        goto_prepare_leave_stage(fz, angle);
        fz->state = ZF_PREPARE_ENTER_STAGE;
        break;
    case ZF_WAVE_LEAVE_STAGE:
        fz->target_dir *= -1;
        fz->state       = ZF_WAVE_ENTER_STAGE;
        break;
    case ZF_STATE_MAX:
        break;
    default:
        ET_INFO("not process state %d", fz->state);
        break;
    }
    return 0;
}

int pm_fsm_init(pm_fsm_t *fsm)
{
    pm_fsm = fsm;
    return 0;
}