#include <math.h>

#include "MOTOR/motor_control.h"
#include "foc.h"
#include "esp_check.h"
#include "motor_timer.h"
#include "step_motor_init.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "motor_control.h"

volatile short Motor_Speed = 0;
volatile short A_PULS_DUTY = 0;
volatile short A_MIUNS_DUTY = 0;
volatile short B_PULS_DUTY = 0;
volatile short B_MIUNS_DUTY = 0;
volatile float Ud = 0;
volatile float Uq = 0;
volatile float theta = 0;
volatile float U_ALPHA = 0;
volatile float U_BETA = 0;

#if MOTOR_WITH_MAGNET_RING
static const float speed_adj   = 0.1f;     //0.22
static const float voltage_adj = 0.22f;   //0.32
#else 
static const float speed_adj   = 0.13f;     //0.22
static const float voltage_adj = 0.22f;     //0.32
#endif


static TaskHandle_t s_foc_task_handle = NULL;
static const char *TAG_FOC_CTRL = "foc_ctrl";
static int64_t s_last_duty_log_us = 0;

static uint16_t clamp_duty(short duty)
{
    if (duty < 0) {
        return 0;
    }
    if (duty > (short)STEPPER_DUTY_MAX) {
        return STEPPER_DUTY_MAX;
    }
    return (uint16_t)duty;
}

static void foc_update_output_once(void)
{
    float speed;
    Uq = voltage_adj;
    Ud = 0;
    if(motor_dir == MOTOR_CW)
    {
        speed = speed_adj; 
    }
    if(motor_dir == MOTOR_CCW)
    {
        speed = -speed_adj; 
    }
    theta += speed;
    if (theta >= (2.0f * (float)M_PI)) {
        theta -= (2.0f * (float)M_PI);
    }

    inv_park(Ud, Uq, theta, &U_ALPHA, &U_BETA);
    PWM_DRIVER_OPENLOOP(U_ALPHA, U_BETA);

    stepper_motor_set_duty_raw(clamp_duty(A_PULS_DUTY),
                               clamp_duty(A_MIUNS_DUTY),
                               clamp_duty(B_PULS_DUTY),
                               clamp_duty(B_MIUNS_DUTY));

    // int64_t now_us = esp_timer_get_time();
    // if (now_us - s_last_duty_log_us >= 1000000) {
    //     s_last_duty_log_us = now_us;
    // //     ESP_LOGI(TAG_FOC_CTRL,
    // //              "duty A+:%d A-:%d B+:%d B-:%d theta:%.3f U_ALPHA:%.3f U_BETA:%.3f",
    // //              A_PULS_DUTY,
    // //              A_MIUNS_DUTY,
    // //              B_PULS_DUTY,
    // //              B_MIUNS_DUTY,
    // //              (double)theta,
    // //              (double)U_ALPHA,
    // //              (double)U_BETA);
    // // }
}

static void foc_task(void *arg)
{
    (void)arg;
    while (1) {
        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
        foc_update_output_once();
    }
}

void park(float ia, float ib, float theta, float *id, float *iq)
{
    float cos_theta = cosf(theta);
    float sin_theta = sinf(theta);
    *id = ia * cos_theta + ib * sin_theta;
    *iq = -ia * sin_theta + ib * cos_theta;
}

void inv_park(float vd, float vq, float theta, float *valpha, float *vbeta)
{
    float cos_theta = cosf(theta);
    float sin_theta = sinf(theta);
    *valpha = vd * cos_theta - vq * sin_theta;
    *vbeta = vd * sin_theta + vq * cos_theta;
}

void PWM_DRIVER_OPENLOOP(float U_ALPHA, float U_BETA)
{
    int IA = (int)(U_ALPHA * 1572.0f);
    int IB = (int)(U_BETA * 1572.0f);

    // A_PULS_DUTY = 0;
    // A_MIUNS_DUTY = 250;//电流为负数

    // B_PULS_DUTY = 250;
    // B_MIUNS_DUTY = 0;//电流为负数90.00

    // B_PULS_DUTY = (short)(IA + 510);
    // A_MIUNS_DUTY = (short)(-IA + 510);

    if (U_ALPHA == 0) {
        A_PULS_DUTY = 500;
        A_MIUNS_DUTY = 500;
    }
    if (U_BETA == 0) {
        B_PULS_DUTY = 500;
        B_MIUNS_DUTY = 500;
    }
    if (U_ALPHA < 0) {
        A_PULS_DUTY = 0;
        A_MIUNS_DUTY = (short)(-IA + 510);
    }
    if (U_ALPHA > 0) {
        A_MIUNS_DUTY = 0;
        A_PULS_DUTY = (short)(IA + 510);
    }
    if (U_BETA > 0) {
        B_MIUNS_DUTY = 0;
        B_PULS_DUTY = (short)(IB + 510);
    }
    if (U_BETA < 0) {
        B_PULS_DUTY = 0;
        B_MIUNS_DUTY = (short)(-IB + 510);
    }
}

bool IRAM_ATTR foc_cal_cb(void *user_data)
{
    TaskHandle_t task_handle = (TaskHandle_t)user_data;
    BaseType_t high_task_wakeup = pdFALSE;

    if (task_handle != NULL) {
        vTaskNotifyGiveFromISR(task_handle, &high_task_wakeup);
    }

    return high_task_wakeup == pdTRUE;
}

esp_err_t foc_openloop_start(void)
{
    if (s_foc_task_handle == NULL) {
        BaseType_t task_ok = xTaskCreatePinnedToCore(foc_task,
                                                     "foc_task",
                                                     4096,
                                                     NULL,
                                                     configMAX_PRIORITIES - 2,
                                                     &s_foc_task_handle,
                                                     0);
        if (task_ok != pdPASS) {
            return ESP_ERR_NO_MEM;
        }
    }

    // if (!stepper_motor_output_is_enabled()) {
    //     stepper_motor_output_enable();
    // }

    foc_update_output_once();

    ESP_RETURN_ON_ERROR(foc_timer_init(FOC_TIMER_DEFAULT_HZ, foc_cal_cb, s_foc_task_handle),
                        TAG_FOC_CTRL,
                        "foc timer init failed");
    ESP_RETURN_ON_ERROR(foc_timer_start(),
                        TAG_FOC_CTRL,
                        "foc timer start failed");
    return ESP_OK;
}