#include "PID_Gen.h"
// #include "stm32f10x.h"
#include "math.h"

// #include "usart.h"
// #include "dma.h"
// #include "gpio.h"
// #include "protocol.h"

// #include "monitor.h"

#define PI 3.14159265f
#define TWO_PI 6.28318531f

#define D2R (PI / 180.0f)

#define R2D (180.0f / PI)

#define _constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))

/**
 * @brief Absolute limiting function, compares the given input with the reference value & limiting difference.
 *        If the difference between the input and the reference value is greater than the limiting interpolation,
 *        the reference value + limiting difference is output (the same applies to negative numbers)
 * @param input the input need to be process
 * @param ref referenve value
 * @param err limiting difference
 * @return  the Limited vaule
 */
static float abs_Limit(float input, float ref, float err)
{
    float temp;
    temp = input - ref;   // calibration Coordinate System - Set the ref to zero
    if (fabs(temp) > err) // check the err between input and ref
    {
        if (temp >= 0.0f)     // input bigger than ref
            return ref + err; // output the right Limit
        else
            return ref - err; // output the left Limit
    }
    return input;
}

/**
 * @brief The relative limit function compares a given input to the limit difference.
 *        If the input is greater than the limit interpolation,
 *        Output limit difference (the same applies to negative numbers)
 * @param input the input need to be process
 * @param err limiting difference
 * @return
 */
static float rel_Limit(float input, float err)
{
    if (fabs(input) > err) // check the err between input and ref
    {
        if (input >= 0.0f) // input bigger than ref
            return err;    // output the right Limit
        else
            return -err; // output the left Limit
    }
    return input;
}

//========================== Var PID Content =============================

static float Variable_KP(float err, varPID *conf)
{
    return conf->KP_a + conf->KP_b * (1 - expf(-conf->KP_c * fabsf(err)));
}

static float Variable_KI(float err, varPID *conf)
{
    return conf->KI_a * expf(-conf->KI_c * fabsf(err));
}

static float Variable_KI_Gain(float err, varPID *conf)
{
    if (fabsf(err) > conf->KI_e)
    {
        return 1;
    }
    else
    {
        return conf->KI_1 * expf(-conf->KI_0 * fabsf(err));
    }
}

static float Variable_KD(float err, varPID *conf)
{
    if (conf->KD_a <= conf->KD_b)
    {
        return 0;
    }

    return conf->KD_a - conf->KD_b * (1 - expf(-conf->KD_c * fabsf(err)));
}

//========================== Var PID Content==============================

/**
 * @brief Init PID : Include init the timer ...
 *        (It is best to call it just before starting the PID operation)
 * @param pid the PID need to be init
 */
void GenPID_Init(GenPID_s *pid)
{
    TimeFlash(&pid->time);
}

int GenPID_Set(GenPID_s *pid,GenPID_CONFIG config)
{
    memset(pid,0x00,sizeof(GenPID_s));
    pid->config = config;
    return 0;
}

int GenPID_Set_Needle(GenPID_s *pid,GenPID_CONFIG *config)
{
    memset(pid,0x00,sizeof(GenPID_s));
    pid->config = *config;
    return 0;
}


// float GenPID_Process(GenPID_s *pid, float sv, float cv)
// {
//     float ts, err, Ierr, perr;
//     float p, i, iadd, d;
//     float output;

//     err = sv - cv; // get the err

//     // get the time
//     ts = TimeFlash(&pid->time);

//     // PID parameter transmit DuoWays
//     // pid->sv = sv;
//     // pid->cv = cv;
//     perr = pid->perr;

//     //------------------------------------can combine

//     // p = pid->kp * err;
//     p = Variable_KP(err, &(pid->varPID)) * err;
//     // integral = integral_vel_prev + pid_vel_I * Ts * 0.5 * (error + error_vel_prev);
//     // i = pid->pi + pid->ki * ts * 0.5f * (err + pid->perr);

//     iadd = Variable_KI_Gain(err, &(pid->varPID)) * Variable_KI(err, &(pid->varPID)) * ts * 0.5f * (err + perr);

//     // i = abs_Limit(i , 0 , pid->intLimit);
//     if (fabs(pid->pout) > pid->outputLimit)
//     {
//         if ((pid->pi > 0) && (iadd > 0))
//         {
//             iadd = pid->pi;
//         }
//         else
//         {
//             if ((pid->pi < 0) < (iadd < 0))
//             {
//                 iadd = pid->pi;
//             }
//         }
//     }
//     i = pid->pi + iadd;
//     i = _constrain(i, -pid->intLimit, pid->intLimit);

//     // i = pid->ki * Ierr;
//     // d = pid->kd * (err - perr) / ts; // the kd preformence is not verify
//     d = Variable_KD(err, &(pid->varPID)) * (err - perr) / ts;

//     output = p + i + d;
//     output = rel_Limit(output, pid->outputLimit);

//     // PID parameter transmit SingleWay
//     pid->cv = cv;
//     pid->sv = sv;
//     pid->pv = cv;
//     // pid->err = err;
//     pid->perr = err;
//     pid->pi = i;
//     pid->pout = output;

// // debug---------------------------------------------------------
// #if PID_DEBUG_SW
//     // SIGN1_1_UP;

//     // 串口调试发送 - start
//     DebugFrameLoad_F(2, p);
//     DebugFrameLoad_F(3, i);
//     DebugFrameLoad_F(4, d);
//     DebugFrameLoad_F(5, (output * D2R));
//     DebugFrameLoad_F(6, err);
//     DebugFrameLoad_F(7, Ierr);
//     DebugFrameLoad_F(8, pid->outputLimit);
//     DebugFrameLoad_F(9, pid->varPID.KP_a);
//     DebugFrameLoad_F(10, pid->varPID.KP_b);
//     DebugFrameLoad_F(11, pid->varPID.KP_c);
//     DebugFrameLoad_F(12, pid->varPID.KI_a);
//     DebugFrameLoad_F(13, pid->varPID.KI_c);
//     DebugFrameLoad_F(14, pid->varPID.KI_e);
//     DebugFrameLoad_F(15, pid->varPID.KI_0);
//     DebugFrameLoad_F(16, pid->varPID.KI_1);
//     DebugFrameLoad_F(17, pid->varPID.KD_a);
//     DebugFrameLoad_F(18, pid->varPID.KD_b);
//     DebugFrameLoad_F(19, pid->varPID.KD_c);
// #endif
//     // debug---------------------------------------------------------

//     return output;
// }

// float GenPID_ProcessSTD(GenPID_s *pid, float sv, float cv)
// {
//     float ts, err, perr;
//     float p, i, iadd, d;
//     float output;

//     err = sv - cv; // get the err

//     // get the time
//     ts = TimeFlash(&pid->time);

//     perr = pid->perr;

//     //------------------------------------can combine

//     p = pid->kp * err;
//     iadd = pid->ki * ts * 0.5 * (err + perr);

//     // i = abs_Limit(i , 0 , pid->intLimit);
//     if (fabs(pid->pout) >= pid->outputLimit)
//     {
//         if ((pid->pi > 0) && (iadd > 0))
//         {
//             iadd = pid->pi;
//         }
//         else
//         {
//             if ((pid->pi < 0) < (iadd < 0))
//             {
//                 iadd = pid->pi;
//             }
//         }
//     }
//     i = pid->pi + iadd;
//     i = _constrain(i, -pid->intLimit, pid->intLimit);

//     // i = pid->ki * Ierr;
//     d = pid->kd * (err - perr) / ts; // the kd preformence is not verify
//     // d = Variable_KD(err, pid->varPID) * (err - perr) / ts;

//     output = p + i + d;
//     output = rel_Limit(output, pid->outputLimit);

//     // PID parameter transmit SingleWay
//     pid->cv = cv;
//     pid->sv = sv;
//     pid->pv = cv;
//     // pid->err = err;
//     pid->perr = err;
//     pid->pi = i;
//     pid->pout = output;

// // debug---------------------------------------------------------
// #if PID_DEBUG_SW
//     // SIGN1_1_UP;

//     // 串口调试发送 - start
//     DebugFrameLoad_F(9, p);
//     DebugFrameLoad_F(10, i);
//     DebugFrameLoad_F(11, d);
//     DebugFrameLoad_F(12, output);
//     DebugFrameLoad_F(13, err);
//     DebugFrameLoad_F(14, iadd);
//     // DebugFrameLoad_F(8, pid->outputLimit);

// #endif
//     // debug---------------------------------------------------------

//     return output;
// }


float GenPID_Process(GenPID_s *pid, float sv, float cv, OBS_GenPID *obs)
{

    float Ts = TimeFlash(&(pid->time));

    float err = sv - cv;
    // u(s) = (P + I/s + Ds)e(s)
    // Discrete implementations
    // proportional part
    // u_p  = P *e(k)
    float proportional = Variable_KP(err, &(pid->config.varPID)) * err;
    // Tustin transform of the integral part
    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
    float integral = pid->pi + Variable_KI_Gain(err, &(pid->config.varPID)) * Variable_KI(err, &(pid->config.varPID)) * Ts * 0.5f * (err + pid->perr);
    // antiwindup - limit the output
    integral = _constrain(integral, -pid->config.intLimit, pid->config.intLimit);
    // Discrete derivation
    // u_dk = D(ek - ek_1)/Ts
    float derivative = Variable_KD(err, &(pid->config.varPID)) * (err - pid->perr) / Ts;

    // sum all the components
    float output = proportional + integral + derivative;
    // antiwindup - limit the output variable
    output = _constrain(output, -pid->config.outputLimit, pid->config.outputLimit);

    // if output ramp defined
    if (pid->config.output_ramp > 0)
    {
        // limit the acceleration by ramping the output
        float output_rate = (output - pid->pout) / Ts;
        if (output_rate > pid->config.output_ramp)
            output = pid->pout + pid->config.output_ramp * Ts;
        else if (output_rate < -pid->config.output_ramp)
            output = pid->pout - pid->config.output_ramp * Ts;
    }
    // saving for the next pass
    // integral_prev = integral;
    pid->pi = integral;
    // output_prev = output;
    pid->pout = output;
    // error_prev = error;
    pid->perr = err;

    // OBS Aera
    if (obs != NULL)
    {
        obs->p = proportional;
        obs->i = integral;
        obs->d = derivative;
        obs->output = output;
        obs->err = err;
        obs->pi = pid->pi;
        obs->Ts = Ts;
    }


    return output;
}

float GenPID_ProcessSTD(GenPID_s *pid, float sv, float cv, OBS_GenPID *obs)
{

    float Ts = TimeFlash(&(pid->time));

    float err = sv - cv;
    // u(s) = (P + I/s + Ds)e(s)
    // Discrete implementations
    // proportional part
    // u_p  = P *e(k)
    float proportional = pid->config.kp * err;
    // Tustin transform of the integral part
    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
    float integral = pid->pi + pid->config.ki * Ts * 0.5f * (err + pid->perr);
    // antiwindup - limit the output
    integral = _constrain(integral, -pid->config.intLimit, pid->config.intLimit);
    // Discrete derivation
    // u_dk = D(ek - ek_1)/Ts
    float derivative = pid->config.kd * (err - pid->perr) / Ts;

    // sum all the components
    float output = proportional + integral + derivative;
    // antiwindup - limit the output variable
    output = _constrain(output, -pid->config.outputLimit, pid->config.outputLimit);

    // // if output ramp defined
    // if (pid->output_ramp > 0)
    // {
    //     // limit the acceleration by ramping the output
    //     float output_rate = (output - pid->pout) / Ts;
    //     if (output_rate > pid->output_ramp)
    //         output = pid->pout + pid->output_ramp * Ts;
    //     else if (output_rate < -pid->output_ramp)
    //         output = pid->pout - pid->output_ramp * Ts;
    // }
    // saving for the next pass
    // integral_prev = integral;
    pid->pi = integral;
    // output_prev = output;
    pid->pout = output;
    // error_prev = error;
    pid->perr = err;

    // OBS Aera
    if (obs != NULL)
    {
        obs->p = proportional;
        obs->i = integral;
        obs->d = derivative;
        obs->output = output;
        obs->err = err;
        obs->pi = pid->pi;
        obs->Ts = Ts;
    }


    return output;
}

float GenPID_ProcessSTD_LPIn(GenPID_s *pid, float sv, float cv, float cv_LP,OBS_GenPID *obs)
{

    float Ts = TimeFlash(&(pid->time));

    float err = sv - cv;
    float err_LP = sv - cv_LP;
    // u(s) = (P + I/s + Ds)e(s)
    // Discrete implementations
    // proportional part
    // u_p  = P *e(k)
    float proportional = pid->config.kp * err_LP;
    // Tustin transform of the integral part
    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
    float integral = pid->pi + pid->config.ki * Ts * 0.5f * (err + pid->perr);
    // antiwindup - limit the output
    integral = _constrain(integral, -pid->config.intLimit, pid->config.intLimit);
    // Discrete derivation
    // u_dk = D(ek - ek_1)/Ts
    float derivative = pid->config.kd * (err_LP - pid->perr) / Ts;

    // sum all the components
    float output = proportional + integral + derivative;
    // antiwindup - limit the output variable
    output = _constrain(output, -pid->config.outputLimit, pid->config.outputLimit);

    // if output ramp defined
    if (pid->config.output_ramp > 0)
    {
        // limit the acceleration by ramping the output
        float output_rate = (output - pid->pout) / Ts;
        if (output_rate > pid->config.output_ramp)
            output = pid->pout + pid->config.output_ramp * Ts;
        else if (output_rate < -pid->config.output_ramp)
            output = pid->pout - pid->config.output_ramp * Ts;
    }
    
    // saving for the next pass
    // integral_prev = integral;
    pid->pi = integral;
    // output_prev = output;
    pid->pout = output;
    // error_prev = error;
    pid->perr = err;
    pid->perrLP = err_LP;

    // OBS Aera
    if (obs != NULL)
    {
        obs->p = proportional;
        obs->i = integral;
        obs->d = derivative;
        obs->output = output;
        obs->err = err;
        obs->errLP = err_LP;
        obs->pi = pid->pi;
        obs->Ts = Ts;
    }


    return output;
}



float GenPID_ProcessSTD_ErrIN(GenPID_s *pid, float err,OBS_GenPID *obs)
{
    float Ts = TimeFlash(&(pid->time));

    // u(s) = (P + I/s + Ds)e(s)
    // Discrete implementations
    // proportional part
    // u_p  = P *e(k)
    float proportional = pid->config.kp * err;
    // Tustin transform of the integral part
    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
    float integral = pid->pi + pid->config.ki * Ts * 0.5f * (err + pid->perr);
    // antiwindup - limit the output
    integral = _constrain(integral, -pid->config.intLimit, pid->config.intLimit);
    // Discrete derivation
    // u_dk = D(ek - ek_1)/Ts
    float derivative = pid->config.kd * (err - pid->perr) / Ts;

    // sum all the components
    float output = proportional + integral + derivative;
    // antiwindup - limit the output variable
    output = _constrain(output, -pid->config.outputLimit, pid->config.outputLimit);

    // // if output ramp defined
    // if (pid->output_ramp > 0)
    // {
    //     // limit the acceleration by ramping the output
    //     float output_rate = (output - pid->pout) / Ts;
    //     if (output_rate > pid->output_ramp)
    //         output = pid->pout + pid->output_ramp * Ts;
    //     else if (output_rate < -pid->output_ramp)
    //         output = pid->pout - pid->output_ramp * Ts;
    // }
    // saving for the next pass
    // integral_prev = integral;
    pid->pi = integral;
    // output_prev = output;
    pid->pout = output;
    // error_prev = error;
    pid->perr = err;

    // OBS Aera
    if (obs != NULL)
    {
        obs->p = proportional;
        obs->i = integral;
        obs->d = derivative;
        obs->output = output;
        obs->err = err;
        obs->pi = pid->pi;
        obs->Ts = Ts;
    }

    return output;
}