/*
 * Copyright 2020-2024 Yuntu Microelectronics co.,ltd
 * All rights reserved.
 *
 * YUNTU Confidential. This software is owned or controlled by YUNTU and may
 * only be used strictly in accordance with the applicable license terms. By expressly
 * accepting such terms or by downloading, installing, activating and/or otherwise
 * using the software, you are agreeing that you have read, and that you agree to
 * comply with and are bound by, such license terms. If you do not agree to be
 * bound by the applicable license terms, then you may not retain, install,
 * activate or otherwise use the software. The production use license in
 * Section 2.3 is expressly granted for this software.
 */
/*!
 * @file hw_motor.c
 */

#include "hw_motor.h"
#include "motor_foc_lib.h"
#include "sdk_project_config.h"
#include "motor_parameters.h"
#include "hw_adc.h"
#include "hw_pwm.h"
#include "hw_config.h"

motor_state_params_t gs = {
    .bemf_acc = 1296000,
    .period_30d = 1200,
    .sector = 1,
    .sample_offset1 = 85,
    .sample_offset2 = 85,
    .vq = MOTOR_OPENLOOP_VOLTAGE,
    .use_hall = HALL_USED,
};
motor_estimator_t ge;
motor_pid_diff_t speed_pi = {
    .q15_kp = SPEED_CTRL_KP,
    .q15_ki = SPEED_CTRL_KI,
    .q15_out_max = SPEED_CTRL_OUTMAX,
    .q15_out_min = SPEED_CTRL_OUTMIN,
    .q15_error_max = MOTOR_ACCRPM2THETA(300),
    .q15_kf = 0,
};

motor_pid_diff_t id_pi = {
    .q15_kp = 2*D_CURRENT_KP,
    .q15_ki = D_CURRENT_KI,
    .q15_out_max = D_CURRENT_OUTMAX,
    .q15_out_min = D_CURRENT_OUTMIN,
    .q15_error_max = 30000,
};
motor_pid_diff_t iq_pi  = {
    .q15_kp = 2*Q_CURRENT_KP,
    .q15_ki = Q_CURRENT_KI,
    .q15_out_max = Q_CURRENT_OUTMAX,
    .q15_out_min = Q_CURRENT_OUTMIN,
    .q15_error_max = 30000,
    .q15_kf = 0,
};

#define MAX_MODULE      29818   // root(Vd^2+Vq^2) <= MAX_MODULE = 32767*91%
#define START_INDEX     52

static const uint16_t circle_limit_table[76]=
{
32588,32411,32066,31732,31569,31250,30940,30789,30492,30205,
29925,29788,29519,29258,29130,28879,28634,28395,28278,28048,
27823,27713,27497,27285,27181,26977,26777,26581,26485,26296,
26110,26019,25840,25664,25492,25407,25239,25076,24995,24835,
24679,24602,24450,24301,24155,24082,23940,23800,23731,23594,
23460,23328,23263,23135,23008,22946,22822,22701,22641,22522,
22406,22291,22234,22122,22011,21956,21848,21741,21636,21584,
21482,21380,21330,21231,21133,21037
};

motor_circle_limit_t circle_limit = {
    .square_max = MAX_MODULE,
    .start_index = START_INDEX,
    .circle_limit_table = (uint16_t *)circle_limit_table,
};

motor_config_params_t gp = {
    .direct = 1,
    .pbus_pid_interval = 12000,
    .pbus_power = 20000,
    .min_boot_voltage = MOTOR_BOOT_MIN_VOLTAGE,
    .max_boot_voltage = MOTOR_BOOT_MAX_VOLTAGE,
    .min_run_voltage = MOTOR_RUN_MIN_VOLTAGE,
    .max_run_voltage = MOTOR_RUN_MAX_VOLTAGE,
    .voltage_error_thresh = 200,
    .voltage_delay_thresh = 1000,
    .boot_temp = 1000,
    .work_temp = 800,
    .temp_error_thresh = 500,
    .max_current = 380,
    .current_error_thresh = 200,
    .no_cross_thresh = 10,
    .offset = 500000,
    .iq = 0,
    .id = 0,
    // .iq_lock = MOTOR_BOOT_CURRENT, //电流A
    .lock_time = MOTOR_BOOT_LOCK_TIME,
    .acc_time = MOTOR_BOOT_ACC_TIME,
    .boot_speed = MOTOR_BOOT_SPEED,
    .close_loop = MOTOR_RUN_CLOSE_LOOP_MODE,
    .weaken_scale = -30000,
    .rpm = MOTOR_BOOT_SPEED_RPM,
    .pwm_period = PWM_HALF_PERIOD_TICK,
    .pwm_window = PWM_WINDOW,
    .id_pi = &id_pi,
    .iq_pi = &iq_pi,
    //0x2EBAE400 28000
    //0x3220A440 29000
    //0x33D70A3D 29450
    .q15_max_limit = 0x33D70A3D,
    .circle_limit = &circle_limit,
};
motor_parameters_t mp = {
    .rs = MOTOR_RS + 2 * MOS_RS,
    .ls = MOTOR_LS,
    .dead_rate = 1.0f - PWM_DEADTIME_SEC / PWM_PERIOD_SEC,
    .kfi = MOTOR_KFI,
    .alpha = KFILTER_ESDQ,
    .vpeak = PEAK_VOLATGE,
    .ipeak = PEAK_CURRENT,
    .freq = PWM_FREQ,
};

motor_low_pass_filter_t speed_lpf = {
    .qAlpha = 64,
};

motor_low_pass_filter_t current_lpf = {
    .qAlpha = 16,
};

motor_low_pass_filter_t voltage_lpf = {
    .qAlpha = 64,
};

motor_low_pass_filter_t voltage_long_lpf = {
    .qAlpha = 8,
};

motor_low_pass_filter_t temper_lpf = {
    .qAlpha = 512,
};

static const uint8_t motorAdcCalibrationSample[] =
{
    ADC_U_PHASE_CHANNEL,                /* U phase current measurement */
    ADC_V_PHASE_CHANNEL,                /* V phase current measurement */
    ADC_VDCBUS_CHANNEL,                 /* DC BUS voltage measurement */
    ADC_NTC_TEMP_CHANNEL,               /* Tempsensor measurement */
};

static const uint8_t motorAdcFocUserSample[] =
{
    ADC_U_PHASE_CHANNEL,                /* U phase current measurement */
    ADC_V_PHASE_CHANNEL,                /* V phase current measurement */
    ADC_VDCBUS_CHANNEL,                 /* DC BUS voltage measurement */
    ADC_NTC_TEMP_CHANNEL,               /* Tempsensor measurement */
};


typedef struct {
    int16_t ntc_tempsensor;    /*!< NTC temperature sensor ADC value */
    int16_t vdc_bus;           /*!< DC bus voltage ADC value */
    int16_t adc_tempsensor;    /*!< Internal temperature sensor ADC value */
    int16_t adc_vdd25;         /*!< Internal 2.5V reference ADC value */
    int16_t adc_ia;            /*!< Phase A current ADC value */
    int16_t adc_ib;            /*!< Phase B current ADC value */
    int16_t adc_ic;            /*!< Phase C current ADC value */
} adc_foc_sample_result_t;

static volatile adc_foc_sample_result_t adcFocResult;

#define CURRENT_AVG_CNT_LOG2         (6)
#define CURRENT_AVG_CNT              (1 << CURRENT_AVG_CNT_LOG2)
static volatile int16_t g_avgCnt = 0;
static volatile int32_t g_uPhaseCurrentSum = 0;
static volatile int32_t g_vPhaseCurrentSum = 0;
volatile int32_t UserSetRMP = 0;
extern volatile uint8_t MotorDirection;
extern volatile uint16_t hall_speed;
extern volatile int32_t hall_theta;

void Motor_AdcCalcISR(void)
{
    /* Clear sequence flag */
    MC_ADC->STS = ADC_STS_EOSEQ_MASK;
    /* Read ADC result */
    g_uPhaseCurrentSum += (uint16_t)MC_ADC->FIFO;
    g_vPhaseCurrentSum += (uint16_t)MC_ADC->FIFO;
    voltage_lpf.qIn = ((uint32_t)MC_ADC->FIFO);
    temper_lpf.qIn = ((uint32_t)MC_ADC->FIFO);
    /* Low pass filter */
    Motor_LpFilterCalc(&voltage_lpf);
    Motor_LpFilterCalc(&temper_lpf);
    
    g_avgCnt += 1;
    if (CURRENT_AVG_CNT == g_avgCnt)
    {
        /* Disable ADC IRQ and PWM */
        HW_PwmStop();
        HW_DeinitADC();
    }
}


void Motor_AdcFOCISR(void)
{
    // GPIOE->PSOR = 1 << 10;
    /* Clear the interrupt flag */
    MC_ADC->STS = ADC_STS_EOSEQ_MASK;
    /* Read current1,2 sample */
    gs.ia = ((uint16_t)(MC_ADC->FIFO) - gs.sample_offset1) << 4;
    gs.ib = ((uint16_t)(MC_ADC->FIFO) - gs.sample_offset2) << 4;
    /* Read DC bus voltage */
    adcFocResult.vdc_bus = (uint16_t)MC_ADC->FIFO;
    /* Read temperature */
    adcFocResult.ntc_tempsensor = (uint16_t)MC_ADC->FIFO;
    /* RUN FOC loop */
    Motor_FOC_ServiceR23(&gs, &gp, &ge, (voltage_lpf.qOut << 4));

    MC_PWM->CH[0].VAL0 = PWM_HALF_PERIOD_TICK - (gs.pwma[0]);
    MC_PWM->CH[0].VAL1 = (PWM_HALF_PERIOD_TICK + (gs.pwma[0]));
    MC_PWM->CH[4].VAL0 = PWM_HALF_PERIOD_TICK - (gs.pwmb[0]);
    MC_PWM->CH[4].VAL1 = (PWM_HALF_PERIOD_TICK + (gs.pwmb[0]));
    MC_PWM->CH[6].VAL0 = PWM_HALF_PERIOD_TICK - (gs.pwmc[0]);
    MC_PWM->CH[6].VAL1 = (PWM_HALF_PERIOD_TICK + (gs.pwmc[0]));

    MC_PWM->SYNC |= eTMR_SYNC_LDOK_MASK;

    if(MotorDirection == 0)
    {
        hall_theta += (int32_t)hall_speed;

        if(gs.theta_diff > 5)
        {
            speed_lpf.qIn = hall_speed;
        }
        else
        {
            speed_lpf.qIn = ge.omega;
        }
    }
    else
    {
        hall_theta += (-(int32_t)hall_speed);
        if(gs.theta_diff < -15)
        {
            speed_lpf.qIn = (-(int32_t)hall_speed);
        }
        else
        {
            speed_lpf.qIn = ge.omega;
        }
    }
    gs.hall_theta = hall_theta;

    Motor_LpFilterCalc(&speed_lpf);

    /* Convert pbus voltage to Q15 format */
    gs.pbus_voltage = adcFocResult.vdc_bus / 2;
    temper_lpf.qIn = adcFocResult.ntc_tempsensor;
    Motor_LpFilterCalc(&temper_lpf);
    if (STATE_STOP == gs.state) {
        HW_PwmStop();
    }
    // GPIOE->PCOR = 1 << 10;
}

void Motor_SetError(uint32_t errorCode)
{
    if (errorCode)
    {
        Motor_Ctrl_ErrorStop();
        gs.error |= errorCode;
    }
}

/*!
 * @brief Clear error code
 */
void Motor_ClearError(void)
{
    gs.error = 0;
}

/*!
 * @brief Get error code
 */
motor_error_t Motor_GetErrorCode(void)
{
    return gs.error;
}

motor_state_t Motor_GetState(void)
{
    return gs.state;
}

void Motor_SetState(motor_state_t newState)
{
    gs.state = newState;
}
motor_run_mode_t Motor_GetRunMode(void)
{
    return gp.close_loop;
}

void Motor_SetRunMode(motor_run_mode_t runMode)
{
    gp.close_loop = runMode;
}

void Motor_SetIq(int32_t iq)
{
    /* iq update only valid when in WORK mode and current loop mode */
    if ((STATE_WORK == gs.state) && (MOTOR_CURRENT_LOOP == gp.close_loop))
    {
        gp.iq = iq;
    }
}

uint32_t Motor_GetSpeedRpm(void)
{
    return Motor_Theta2Rpm(gs.speed);
}

int8_t Motor_GetTemperature(void)
{
    uint32_t tsc = 0;
    FLASH_DRV_ReadNVR(0, 0x100103F8, 0x4, &tsc);
    return (tsc - ((530 * 4 * temper_lpf.qOut) >> 16));
}

int16_t Motor_GetBusVoltage(void)
{
    return (int16_t)((voltage_lpf.qOut * (int32_t)PEAK_VOLATGE) >> 15);
}

int16_t Motor_GetBusPower(void)
{
    return (int16_t)(( ((gs.pbus_current * voltage_lpf.qOut) >> 15) *
        (int32_t)PEAK_CURRENT *
        (int32_t)PEAK_VOLATGE ) >> 15);
}


void Motor_Ctrl_RunCalibration(void)
{
    /* Enable ADC */
    /* TMU should be init correctly for ADC trigger */
    HW_SetupAdcSeqMode(motorAdcCalibrationSample, 4, Motor_AdcCalcISR);
    HW_PwmSetup();

    /* init parameters */
    voltage_lpf.qOut = 0;
    temper_lpf.qOut = 0;
    g_avgCnt = 0;
    g_uPhaseCurrentSum = 0;
    g_vPhaseCurrentSum = 0;

    HW_PwmStart((PWM_PERIOD_TICK * 4 / 8));
    /* Wait calibration done */
    while (CURRENT_AVG_CNT > g_avgCnt)
    {

    }
    /* Compiler convert div to shift */
    gs.sample_offset1 = g_uPhaseCurrentSum >> CURRENT_AVG_CNT_LOG2;
    gs.sample_offset2 = g_vPhaseCurrentSum >> CURRENT_AVG_CNT_LOG2;
}

void Motor_CtrlInit(void)
{
    /* Set default state to stop */
    gs.state = STATE_STOP;
    gs.error = 0;
    Motor_SvpwmInit(&gp, PWM_HALF_PERIOD_TICK, PWM_WINDOW);
    Motor_ParameterInit(&mp, &ge);
    /* Start Calibration */
    Motor_Ctrl_RunCalibration();
    /* Reinit ADC with FOC service 2 Sequence mode */
    HW_SetupAdcSeqMode(motorAdcFocUserSample, 4, Motor_AdcFOCISR);
}

void Motor_PI_Reset(motor_pid_diff_t *pid)
{
    pid->q15_sum = 0;
    pid->q15_error = 0;
    pid->q15_out = 0;
}

/*FUNCTION**********************************************************************
 *
 * Function Name : Motor_StateSpeedLoopService
 * Description   : This function execute motor state control and speed control
 * PID loop, Typical execution period is 1ms.
 *
 * Implements :Motor_StateSpeedLoopService_Activity
 *END**************************************************************************/
void Motor_StateSpeedLoopService(void)
{
    static uint8_t pTMR_1msCount = 0U;
    pTMR_1msCount++;
    if(pTMR_1msCount >= 16U)
    {
        static int32_t g_count = 0;
        if (STATE_STOP == gs.state) {
            HW_PwmStop();
        }
        if (STATE_ERROR == gs.state) {
            HW_PwmStop();
            if (E_MOTOR_NO_ERROR == gs.error)
            {
                gs.state = STATE_STOP;
            }
        }
        if (STATE_INIT == gs.state) {
            UserSetRMP = MOTOR_BOOT_SPEED_RPM;
            speed_lpf.qState = 0;
            current_lpf.qState = 0;
            Motor_PI_Reset(&speed_pi);
            Motor_PI_Reset(&id_pi);
            Motor_PI_Reset(&iq_pi);
            Motor_EstReset(&ge);
            gp.id = 0;
            gp.iq = 0;
            gs.theta_diff = 0;
            gs.theta = 0;
            g_count = 0;
            /* Reset current offset */
            gp.boot_speed = Motor_Rps2Theta(MOTOR_BOOT_SPEED);
            gp.iq_lock = Motor_ConvCurrent2Adc(MOTOR_BOOT_CURRENT);
            gs.w_step = (gp.boot_speed << 15) / gp.acc_time;
            gs.i_step = gp.iq_lock / gp.lock_time;
            Motor_Ctrl_RunCalibration();
            /* Reinit ADC with FOC service 2 Sequence mode */
            HW_SetupAdcSeqMode(motorAdcFocUserSample, 4, Motor_AdcFOCISR);
            if (gs.error)
            {
                gs.state = STATE_ERROR;
            }
            else
            {
                HW_PwmStart(PWM_PERIOD_TICK >> 1);
                gs.state = STATE_LOCK;
            }
        }
        if (STATE_LOCK == gs.state) {
            if (gp.iq >= gp.iq_lock) 
            {
                gs.state = STATE_ACCE;
            }
            else
            {
                gp.iq += gs.i_step;
            }
        }
        if (STATE_ACCE == gs.state) {
            if (gs.theta_diff < gp.boot_speed) 
            {
                g_count += gs.w_step;
                gs.theta_diff = (int16_t)(g_count >> 15);
                speed_pi.q15_sum = gs.iq << 15;
                gp.speed = speed_lpf.qOut;
                gp.speed = gs.speed;
            }
            else 
            {
                if (gp.close_loop >= MOTOR_CURRENT_LOOP)
                {
                    g_count = 0;
                    if(gs.use_hall == 0)
                    {
                        gs.theta_off = ge.theta - gs.theta;
                    }
                    else
                    {
                        gs.theta_off = gs.hall_theta - gs.theta;
                    }
                    gs.state = STATE_WORK;
                    gp.rpm = MOTOR_BOOT_SPEED_RPM;
                    //gp.rpm = theta2rpm(speed_lpf.qOut);
                }else{
                    speed_pi.q15_sum = gs.iq << 15;
                    gp.speed = speed_lpf.qOut;
                }
                if (gs.speed < 0)
                {
                    Motor_SetError(E_MOTOR_STARTUP_ERROR);
                }
            }   

        }
        if(UserSetRMP>=3500)
        {
            SpeedRAMP(UserSetRMP-100);
        }
        else
        {
            SpeedRAMP(UserSetRMP);
        }
        if (STATE_WORK == gs.state) 
        {
            if (gp.close_loop >= MOTOR_SPEED_LOOP) 
            {
                gp.speed = Motor_Rpm2Theta(gp.rpm);
                speed_pi.q15_error = gp.speed - gs.speed;
                Motor_PI_Ctrl_Diff_Calc(&speed_pi);
                gp.iq = speed_pi.q15_out;
            }
        }
        voltage_lpf.qIn = gs.pbus_voltage;
        Motor_LpFilterCalc(&voltage_lpf);
        gs.speed = speed_lpf.qOut;
        pTMR_1msCount = 0U;
    }
}

void R_Motor_StateSpeedLoopService(void)
{
    static uint8_t pTMR_1msCount = 0U;
    pTMR_1msCount++;
    if(pTMR_1msCount >= 16U)
    {
        static int32_t g_count = 0;
        if (STATE_STOP == gs.state) {
            HW_PwmStop();
        }
        if (STATE_ERROR == gs.state) {
            HW_PwmStop();
            if (E_MOTOR_NO_ERROR == gs.error)
            {
                gs.state = STATE_STOP;
            }
        }
        if (STATE_INIT == gs.state) {
            UserSetRMP = MOTOR_BOOT_SPEED_RPM*(-1);
            speed_lpf.qState = 0;
            current_lpf.qState = 0;
            Motor_PI_Reset(&speed_pi);
            Motor_PI_Reset(&id_pi);
            Motor_PI_Reset(&iq_pi);
            Motor_EstReset(&ge);
            gp.id = 0;
            gp.iq = 0;
            gs.theta_diff = 0;
            gs.theta = 0;
            g_count = 0;
            gs.vq = MOTOR_OPENLOOP_VOLTAGE *(-1);
            gp.rpm = MOTOR_BOOT_SPEED_RPM *(-1),
            /* Reset current offset */
            gp.boot_speed = Motor_Rps2Theta(MOTOR_BOOT_SPEED)*(-1);
            gp.iq_lock =  (Motor_ConvCurrent2Adc(MOTOR_BOOT_CURRENT))*(- 1);
            gs.w_step = ((gp.boot_speed << 15) / gp.acc_time);
            gs.i_step = (gp.iq_lock / gp.lock_time);
            Motor_Ctrl_RunCalibration();
            /* Reinit ADC with FOC service 2 Sequence mode */
            HW_SetupAdcSeqMode(motorAdcFocUserSample, 4, Motor_AdcFOCISR);
            if (gs.error)
            {
                gs.state = STATE_ERROR;
            }
            else
            {
                HW_PwmStart(PWM_PERIOD_TICK >> 1);
                gs.state = STATE_LOCK;
            }
        }
        if (STATE_LOCK == gs.state) {
            if (gp.iq <= gp.iq_lock) 
            {
                gs.state = STATE_ACCE;
            }
            else
            {
                gp.iq += gs.i_step;
            }
        }
        if (STATE_ACCE == gs.state) {
            if (gs.theta_diff > gp.boot_speed) 
            {
                g_count += gs.w_step;
                gs.theta_diff = (int16_t)(g_count >> 15);
                speed_pi.q15_sum = gs.iq << 15;
                gp.speed = speed_lpf.qOut;
                gp.speed = gs.speed;
            }
            else 
            {
                if (gp.close_loop >= MOTOR_CURRENT_LOOP)
                {
                    g_count = 0;
                    if(gs.use_hall == 0)
                    {
                        gs.theta_off = ge.theta - gs.theta;
                    }
                    else
                    {
                        gs.theta_off = gs.theta - gs.hall_theta ;
                    }
                    gs.state = STATE_WORK;
                }else{
                    speed_pi.q15_sum = gs.iq << 15;
                    gp.speed = speed_lpf.qOut;
                }
                if (gs.speed > 0)
                {
                    Motor_SetError(E_MOTOR_STARTUP_ERROR);
                }
            }   
        }

        if(UserSetRMP <= -3500)
        {
            R_SpeedRAMP(UserSetRMP+100);
        }
        else
        {
            R_SpeedRAMP(UserSetRMP);
        }
        if (STATE_WORK == gs.state) 
        {
            if (gp.close_loop >= MOTOR_SPEED_LOOP) 
            {
                gp.speed = Motor_Rpm2Theta(gp.rpm);
                speed_pi.q15_error = gp.speed - gs.speed;
                Motor_PI_Ctrl_Diff_Calc(&speed_pi);
                gp.iq = speed_pi.q15_out;
            }
        }
        voltage_lpf.qIn = gs.pbus_voltage;
        Motor_LpFilterCalc(&voltage_lpf);
        gs.speed = speed_lpf.qOut;
        pTMR_1msCount = 0U;
    }
}

void SpeedRAMP(int32_t SpeedSet)
{
    static int32_t MotorRpmSet = MOTOR_BOOT_SPEED_RPM;

    if(MotorRpmSet != SpeedSet)
    {
        if(MotorRpmSet > SpeedSet)
        {
            MotorRpmSet--;
        }
        else if(MotorRpmSet < SpeedSet)
        {
            if(MotorRpmSet >= 3500)
            {
                MotorRpmSet = MotorRpmSet + 100;
            }
            else
            {
                MotorRpmSet++;
            }
        }
        else
        {
            /* None */
        }       
    }
    gp.rpm = MotorRpmSet;
}

void R_SpeedRAMP(int32_t SpeedSet)
{
    static int32_t MotorRpmSet = - MOTOR_BOOT_SPEED_RPM;

    if(MotorRpmSet != SpeedSet)
    {
        if(MotorRpmSet < SpeedSet)
        {
            MotorRpmSet++;
        }
        else if(MotorRpmSet > SpeedSet)
        {
            MotorRpmSet--;
        }
        else
        {
            /* None */
        }       
    }
    gp.rpm = MotorRpmSet;
}
/*******************************************************************************
 * EOF
 ******************************************************************************/
