#include <math.h>
#include <stdlib.h>
#include "bldc_ctrl.h"
#include "motor.h"
#include "gpio.h"
#include "pwm.h"
#include "bsp_pid.h"

/* ˽ÓбäÁ¿ */
static bldcm_data_t bldcm_data;

/* ¾Ö²¿º¯Êý */
static void sd_gpio_config(void);

/**
  * @brief  µç»ú³õʼ»¯
  * @param  ÎÞ
  * @retval ÎÞ
  */
void bldcm_init(void)
{
  // PWM_TIMx_Configuration(); // µç»ú¿ØÖƶ¨Ê±Æ÷£¬Òý½Å³õʼ»¯
  // hall_tim_config();        // »ô¶û´«¸ÐÆ÷³õʼ»¯
  // sd_gpio_config(); // sd Òý½Å³õʼ»¯
  Set_SDH2136_Enable();
  // Set_SDH2136_Disable();
  bldcm_data.direction = MOTOR_FWD;
  bldcm_data.is_enable = 1;
  bldcm_data.dutyfactor = 1500;
}

/**
  * @brief  µç»ú SD ¿ØÖÆÒý½Å³õʼ»¯
  * @param  ÎÞ
  * @retval ÎÞ
  */
static void sd_gpio_config(void)
{
  Set_SDH2136_Disable();
}

/**
  * @brief  ÉèÖõç»úËÙ¶È
  * @param  v: Ëٶȣ¨Õ¼¿Õ±È£©
  * @retval ÎÞ
  */
void set_bldcm_speed(uint16_t v)
{
  bldcm_data.dutyfactor = v;

  set_pwm_pulse(v); // ÉèÖÃËÙ¶È
}

/**
  * @brief  ÉèÖõç»ú·½Ïò
  * @param  ÎÞ
  * @retval ÎÞ
  */
void set_bldcm_direction(motor_dir_t dir)
{
  bldcm_data.direction = dir;
}

/**
  * @brief  »ñÈ¡µç»úµ±Ç°·½Ïò
  * @param  ÎÞ
  * @retval ÎÞ
  */
motor_dir_t get_bldcm_direction(void)
{
  return bldcm_data.direction;
}

/**
  * @brief  ʹÄܵç»ú
  * @param  ÎÞ
  * @retval ÎÞ
  */
void set_bldcm_enable(void)
{
  bldcm_data.is_enable = 1;
  hall_enable();
}

/**
  * @brief  ½ûÓõç»ú
  * @param  ÎÞ
  * @retval ÎÞ
  */
void set_bldcm_disable(void)
{
  /* ½ûÓûô¶û´«¸ÐÆ÷½Ó¿Ú */
  hall_disable();

  /* Í£Ö¹ PWM Êä³ö */
  stop_pwm_output();

  /* ¹Ø±Õ MOS ¹Ü */
  bldcm_data.is_enable = 0;
}

/**
  * @brief  µç»úλÖÃʽ PID ¿ØÖÆʵÏÖ(¶¨Ê±µ÷ÓÃ)
  * @param  ÎÞ
  * @retval ÎÞ
  */
void bldcm_pid_control(void)
{
  int32_t speed_actual = get_motor_speed(); // µç»úÐýתµÄµ±Ç°ËÙ¶È

  if (bldcm_data.is_enable)
  {
    float cont_val = 0; // µ±Ç°¿ØÖÆÖµ

    cont_val = PID_realize(speed_actual);

    if (cont_val < 0)
    {
      cont_val = -cont_val;
      bldcm_data.direction = MOTOR_REV;
    }
    else
    {
      bldcm_data.direction = MOTOR_FWD;
    }

    cont_val = (cont_val > PERIOD_CMP) ? PERIOD_CMP : cont_val; // ÉÏÏÞ´¦Àí

    set_bldcm_speed(cont_val);

    // #ifdef PID_ASSISTANT_EN
    //     set_computer_value(SEND_FACT_CMD, CURVES_CH1, &speed_actual, 1); // ¸øͨµÀ 1 ·¢ËÍʵ¼ÊÖµ
    // #else
    //     printf("ʵ¼ÊÖµ£º%d, Ä¿±êÖµ£º%.0f£¬¿ØÖÆÖµ: %.0f\n", speed_actual, get_pid_target(), cont_val);
    // #endif
  }
}