/*
 * @Author: DaMing zxm5337@163.com
 * @Date: 2024-04-17 18:30:06
 * @LastEditors: 张小明 zxm5337@163.com
 * @LastEditTime: 2024-05-28 16:26:27
 * @FilePath: \Proxi_CheckBoard\User\App\ctrl_logic.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include "ctr_logic.h"
#include "communication.h"
#include "FreeRTOS.h"
#include "task.h"
#include "dac.h"
#include "ProximitySwitches.h"
#include "bldc.h"

beep_t beep_1;
pars_cmd_time_t pars_cmd_time_A;
pars_cmd_time_t pars_cmd_time_B;

static void fun_get_limit_flag(motor_t *motor)
{
    if (!motor)
        return;

    BOOL *limit_flag = &motor->limit_flag;
    BOOL *limit_near_flag = &motor->limit_near_flag;
    BOOL *limit_far_flag = &motor->limit_far_flag;
    BOOL *stall_flag = &motor->stall_flag;

    modbus_rtu_t *modbus_rtu = NULL;

    if (motor == &motor_A)
    {
        modbus_rtu = &modbus_rtu_A;
        if (HAL_GPIO_ReadPin(RSVD_LIMIT_A_GPIO_Port, RSVD_LIMIT_A_Pin) == GPIO_PIN_RESET)
            *limit_flag = TRUE;
        else
            *limit_flag = FALSE;
    }
    else if (motor == &motor_B)
    {
        modbus_rtu = &modbus_rtu_B;
        if (HAL_GPIO_ReadPin(RSVD_LIMIT_B_GPIO_Port, RSVD_LIMIT_B_Pin) == GPIO_PIN_RESET)
            *limit_flag = TRUE;
        else
            *limit_flag = FALSE;
    }
    if ((*limit_flag == TRUE) || (*stall_flag == TRUE))
    {
        if (modbus_rtu->distance > DISTANCE_RATED / 2)
        {
            *limit_near_flag = FALSE;
            *limit_far_flag = TRUE;
        }
        else if (modbus_rtu->distance < DISTANCE_RATED / 2)
        {
            *limit_near_flag = TRUE;
            *limit_far_flag = FALSE;
        }
    }
    else
    {
        *limit_near_flag = FALSE;
        *limit_far_flag = FALSE;
    }
}

static void fun_get_stall_flag(motor_t *motor)
{
    if (!motor)
        return;

    BOOL en = motor->en;
    BOOL *stall_flag = &motor->stall_flag;
    uint32_t freq = motor->freq;
    encoder_t *encoder = NULL;

    if (motor == &motor_A)
        encoder = &encoder_A;
    if (motor == &motor_B)
        encoder = &encoder_B;

    uint32_t *timer = &encoder->timer;
    int32_t enc = encoder->enc;
    int16_t *enc2 = &encoder->enc2;
    int32_t *enc_old = &encoder->enc_old;
    int32_t *enc_sub = &encoder->enc_sub;
    uint32_t alarm_L, alarm_H;

    if (en == FALSE)
    {
        *enc_old = 0;
        *enc2 = 0;
        return;
    }

    if ((motor->position_pv >= 0.1f) && (motor->position_zero != 0))
        return;

    if (*timer + 30 < xTaskGetTickCount())
    {
        *timer = xTaskGetTickCount();

        *enc_sub = abs(enc - *enc_old);

        if (*enc_old)
        {
            if (freq == 20000)
            {
                alarm_L = 50;
                alarm_H = 350;
            }
            else if (freq == 80000)
            {
                alarm_L = 200;
                alarm_H = 600;
            }
            if ((*enc_sub > alarm_L) && (*enc_sub < alarm_H))
                *stall_flag = FALSE;
            else
                *stall_flag = TRUE;
        }
        *enc_old = enc;
    }
}

static void fun_get_lvdt_flag(motor_t *motor)
{
    if (!motor)
        return;

    BOOL *lvdt_flag = &motor->lvdt_flag;

    if (motor == &motor_A)
        *lvdt_flag = modbus_rtu_A.lvdt_flag;
    else if (motor == &motor_B)
        *lvdt_flag = modbus_rtu_B.lvdt_flag;
}

static void fun_get_position(motor_t *motor)
{
    if (!motor)
        return;

    modbus_rtu_t *modbus_rtu = NULL;
    if (motor == &motor_A)
        modbus_rtu = &modbus_rtu_A;
    else if (motor == &motor_B)
        modbus_rtu = &modbus_rtu_B;

    motor->position_pv = modbus_rtu->distance - motor->position_zero;
}

static void fun_get_input(motor_t *motor)
{
    fun_get_limit_flag(motor);
    fun_get_stall_flag(motor);
    fun_get_lvdt_flag(motor);
    fun_get_position(motor);
}

static void fun_prot_position(motor_t *motor)
{
    if (!motor)
        return;

    BOOL *en = &motor->en;
    uint8_t direction = motor->direction;
    uint32_t *step = &motor->step;
    uint32_t *step_count = &motor->step_count;
    /*
        modbus_rtu_t *modbus_rtu = NULL;

        if (motor == &motor_A)
        {
            modbus_rtu = &modbus_rtu_A;
        }
        else if (motor == &motor_B)
        {
            modbus_rtu = &modbus_rtu_B;
        }

        if ((motor->limit_flag == TRUE) && (modbus_rtu->con_flag == FALSE))
        {
            *step = 0;
            *step_count = 0;
            *en = FALSE;
            return;
        }
    */
    if ((motor->limit_near_flag == TRUE) && (direction == DIR_REVERSED))
    {
        *step = 0;
        *step_count = 0;
        *en = FALSE;
    }
    else if ((motor->limit_far_flag == TRUE) && (direction == DIR_FORWARD))
    {
        *step = 0;
        *step_count = 0;
        *en = FALSE;
    }
    else if (*step > 0)
    {
        *en = TRUE;
    }
}

static void fun_proc_cmd_position(motor_t *motor)
{
    if (!motor)
        return;

    BOOL limit_near_flag = motor->limit_near_flag;
    BOOL limit_far_flag = motor->limit_far_flag;
    BOOL *force_near_flag = &motor->force_near_flag;
    BOOL *force_far_flag = &motor->force_far_flag;

    if (*force_near_flag == TRUE)
    {
        if (limit_near_flag != TRUE)
        {
            motor->position_sv = -DISTANCE_RATED;
            motor->pre_position = DISTANCE_RATED;
        }
        else
        {
            motor->end_flag = TRUE;
            *force_near_flag = FALSE;
        }
    }
    else if (*force_far_flag == TRUE)
    {
        if (limit_far_flag != TRUE)
        {
            motor->position_sv = DISTANCE_RATED;
            motor->pre_position = -DISTANCE_RATED;
        }
        else
        {
            motor->end_flag = TRUE;
            *force_far_flag = FALSE;
        }
    }
    else
    {
        // motor->pre_position = motor->position_pv;
    }
}

static void fun_zero_cmd_time()
{
    for (uint8_t i = 0; i < 4; i++)
    {
        for (uint8_t j = 0; j < 10; j++)
        {
            prox_switch_A_t[i][j] = 0;
            prox_switch_B_t[i][j] = 0;
        }
    }
}

static void fun_proc_cmd_time(motor_t *motor)
{
    if (!motor)
        return;

    prox_swith_pwr_t *prox_swith_pwr;
    float *prox_switch_vol = NULL;
    uint32_t(*time)[10] = NULL;
    pars_cmd_time_t *pars_cmd_time = NULL;

    if (motor == &motor_A)
    {
        prox_swith_pwr = &prox_swith_pwr_A;
        prox_switch_vol = prox_switch_A_vol + 1;
        time = prox_switch_A_t;
        pars_cmd_time = &pars_cmd_time_A;
    }
    else if (motor == &motor_B)
    {
        prox_swith_pwr = &prox_swith_pwr_B;
        prox_switch_vol = prox_switch_B_vol + 1;
        time = prox_switch_B_t;
        pars_cmd_time = &pars_cmd_time_B;
    }

    if (pars_cmd_time->busy_flag == TRUE)
        return;

    uint8_t i = pars_cmd_time->senor_seq + 1;

    if (!pars_cmd_time->delay)
        pars_cmd_time->delay = 1000;

    motor->position_sub = fabs(motor->position_pv - motor->Sn);

    if (fabs(motor->position_sub) <= 0.2f)
    {
        motor->en = FALSE;

        pars_cmd_time->current = prox_switch_vol[i] * 1000 / 250.0f;

        switch (pars_cmd_time->ctr_seq)
        {
        case 0:
            prox_swith_pwr->voltage_sv = PROX_SWITCH_PWR_MIN;
            pars_cmd_time->timer = xTaskGetTickCount();
            pars_cmd_time->ctr_seq++;
            break;
        case 1:
            if (pars_cmd_time->timer + pars_cmd_time->delay <= xTaskGetTickCount())
            {
                prox_swith_pwr->voltage_sv = 8200;
                time[0][i - 1] = xTaskGetTickCount();
                pars_cmd_time->ctr_seq++;
                pars_cmd_time->timer = xTaskGetTickCount();
            }
            break;
        case 2:
            if ((pars_cmd_time->current >= 1650) && (!time[1][i - 1]))
            {
                time[1][i - 1] = xTaskGetTickCount();
                pars_cmd_time->ctr_seq++;
                pars_cmd_time->timer = xTaskGetTickCount();
                break;
            }
            else if (pars_cmd_time->timer + pars_cmd_time->delay / 500 <= xTaskGetTickCount())
            {
                pars_cmd_time->ctr_seq++;
                pars_cmd_time->timer = xTaskGetTickCount();
            }
            break;
        case 3:
            if ((time[1][i - 1]) && (!time[2][i - 1]) && (pars_cmd_time->current < 1650))
            {
                time[2][i - 1] = xTaskGetTickCount();
                pars_cmd_time->ctr_seq++;
                pars_cmd_time->timer = xTaskGetTickCount();
                break;
            }
            else if (pars_cmd_time->timer + pars_cmd_time->delay / 500 <= xTaskGetTickCount())
            {
                pars_cmd_time->ctr_seq++;
                pars_cmd_time->timer = xTaskGetTickCount();
            }
            break;
        case 4:
            if ((!time[3][i - 1]) && (pars_cmd_time->current <= 400))
            {
                time[3][i - 1] = xTaskGetTickCount();
                pars_cmd_time->senor_seq++;
                pars_cmd_time->ctr_seq = 10;
                break;
            }
            else if (pars_cmd_time->timer + pars_cmd_time->delay / 500 <= xTaskGetTickCount())
            {
                pars_cmd_time->senor_seq++;
                pars_cmd_time->ctr_seq = 10;
            }
            break;
        case 10:
            if (pars_cmd_time->senor_seq > 9)
            {
                pars_cmd_time->senor_seq = 0;
                if (modbus_rtu_hmi.upsend_flag == FALSE)
                    modbus_rtu_hmi.upsend_flag = TRUE;
                pars_cmd_time->busy_flag = TRUE;
                // motor->freq = 20000;
                break;
            }
            else
                pars_cmd_time->ctr_seq = 0;
            break;

        default:
            break;
        }
    }
    else
    {
        pars_cmd_time->ctr_seq = 0;
        pars_cmd_time->senor_seq = 0;
        pars_cmd_time->busy_flag = FALSE;

        if (motor->position_pv < motor->Sn)
            motor->direction = DIR_FORWARD;
        else if (motor->position_pv > motor->Sn)
            motor->direction = DIR_REVERSED;

        motor->en = TRUE;
        // motor->freq = 80000;

        fun_zero_cmd_time();
    }
}

static void fun_proc_cmd_freq()
{
    static uint32_t time;
    static uint16_t pre_velocity;

    if (bldc_A.velocity_pv != pre_velocity)
    {
        if (time + 200 <= xTaskGetTickCount())
        {
            modbus_rtu_hmi.upsend_flag = TRUE;
            time = xTaskGetTickCount();
        }
    }
    pre_velocity = bldc_A.velocity_pv;
}

static void fun_proc_zero_position(motor_t *motor)
{
    if (!motor)
        return;

    BOOL *en = &motor->en;
    BOOL *limit_near_flag = &motor->limit_near_flag;
    uint8_t *direction = &motor->direction;
    uint32_t *freq = &motor->freq;
    float32 *position_zero = &motor->position_zero;
    modbus_rtu_t *modbus_rtu = NULL;

    if (motor == &motor_A)
        modbus_rtu = &modbus_rtu_A;
    else if (motor == &motor_B)
        modbus_rtu = &modbus_rtu_B;

    if (*limit_near_flag == TRUE)
    {
        *freq = 20000;
        *en = FALSE;
        *position_zero = modbus_rtu->distance;
        fun_get_position(motor);
        motor->ctr_cmd = CMD_SET_DIST;
    }
    else
    {
        *direction = DIR_REVERSED;
        *freq = 80000;
        *en = TRUE;
    }
}

static void fun_proc_cmd_zero(motor_t *motor)
{
    if (!motor)
        return;

    BOOL *en = &motor->en;
    BOOL *limit_near_flag = &motor->limit_near_flag;
    uint8_t *direction = &motor->direction;
    uint8_t *ctr_cmd = &motor->ctr_cmd;
    BOOL *zero_flag = &motor->zero_flag;
    BOOL *zero_ack = &motor->zero_ack;
    BOOL *end_flag = &motor->end_flag;
    uint32_t *freq = &motor->freq;
    uint32_t *step = &motor->step;
    uint32_t *step_count = &motor->step_count;
    float32 *position_zero = &motor->position_zero;
    modbus_rtu_t *modbus_rtu = NULL;

    if (motor == &motor_A)
        modbus_rtu = &modbus_rtu_A;
    else if (motor == &motor_B)
        modbus_rtu = &modbus_rtu_B;

    if (*limit_near_flag == TRUE)
    {
        *step = 0;
        *step_count = 0;
        *zero_flag = TRUE;
        *position_zero = modbus_rtu->distance;
        fun_get_position(motor);
    }
    else if (*zero_flag == FALSE)
    {
        *direction = DIR_REVERSED;
        *freq = 80000;
        *en = TRUE;
    }

    if (*zero_flag == TRUE)
    {
        if (motor->position_pv <= (motor->Sn * 2 - 0.2f))
        {
            *direction = DIR_FORWARD;
            *freq = 80000;
            *en = TRUE;
        }
        else
        {
            *en = FALSE;
            *zero_flag = FALSE;
            if (motor->ctr_cmd == CMD_ZERO_DIST)
                *zero_ack = TRUE;
            *end_flag = TRUE;
            *ctr_cmd = CMD_SET_DIST;
            *freq = 20000;
        }
    }
}

static void fun_get_motor_step(motor_t *motor)
{
    if (!motor)
        return;

    uint8_t *direction = &motor->direction;
    uint32_t *step = &motor->step;
    uint32_t *step_count = &motor->step_count;
    float32 *position_sv = &motor->position_sv;
    float32 *position_pv = &motor->position_pv;
    float32 *position_sub = &motor->position_sub;
    float32 *pre_position = &motor->pre_position;

    if (!motor->position_zero)
        return;
    if (*pre_position == *position_sv)
        return;

    //*position_sub = *position_sv > *position_pv ? *position_sv - *position_pv : *position_pv - *position_sv;
    *position_sub = fabs(*position_sv - *position_pv);
    *step = *position_sub * SCREW_PITCH * STEP_PER_CIRL;
    *step_count = 0;
    if (*position_sv > *position_pv)
    {
        *direction = DIR_FORWARD;
    }
    else if (*position_sv < *position_pv)
    {
        *direction = DIR_REVERSED;
    }
    *pre_position = *position_sv;
}

static void fun_proc_ctr_cmd(motor_t *motor)
{
    if (!motor)
        return;

    cmd_e ctr_cmd = (cmd_e)motor->ctr_cmd;

    switch (ctr_cmd)
    {
    case CMD_NO_PROC:
        fun_proc_zero_position(motor);
        break;
    case CMD_SET_DIST:
        fun_proc_cmd_position(motor);
        break;
    case CMD_GET_TIME:
        fun_proc_cmd_time(motor);
        break;
    case CMD_SET_FREQ:
        fun_proc_cmd_freq();
        break;
    case CMD_ZERO_DIST:
        fun_proc_cmd_zero(motor);
        break;
    case CMD_RATED_DIST:
        fun_proc_cmd_zero(motor);
        break;

    default:
        break;
    }
}

void fun_ctr_motor(motor_t *motor)
{
    if (!motor)
        return;

    fun_get_input(motor);
    fun_proc_ctr_cmd(motor);
    fun_get_motor_step(motor);
    fun_prot_position(motor);
    fun_set_motor(motor);
}

void fun_ctr_beep(uint16_t delay_ms)
{
    if (beep_1.en == FALSE)
        return;

    beep_1.delay = delay_ms;

    if (modbus_rtu_hmi.upsend_count > 0)
        beep_1.time = xTaskGetTickCount();

    if (beep_1.time + beep_1.delay > xTaskGetTickCount())
        BEEP_MUTE_OPEN();
    else
        BEEP_MUTE_CLOSE();
}