#include "stdio.h"
#include "stdint.h"
#include "stdbool.h"

#include "math.h"

#include "string.h"

#include "T_SpeedShape.h"


static float shortARCCalculateGen(float cur, float target, float cycleRange)
{
    float A, B;
    cycleRange = fabsf(cycleRange);
    if (!((cur >= -cycleRange) && (cur <= cycleRange) && (target >= -cycleRange) && (target <= cycleRange)))
    {
        return 0;
    }
    if (cur >= target)
    {
        A = cycleRange * 2.0f - cur + target;
        B = target - cur;
        if (A > -B)
        {
            return B;
        }
        else
        {
            return A;
        }
    }
    else
    {
        A = target - cur;
        B = - (cycleRange * 2.0f)  - cur + target;
        if (A > -B)
        {
            return B;
        }
        else
        {
            return A;
        }
    }
}


/**
 * @brief Init the Trap Curve Struct
 * @param s the object which need to be init
 * @param vel_Max_Limit the Maximum speed
 * @param acc_global the Maximum acceleration
 * @param time_Now the time Now , it Will reduce accuracy after 3 hours
 * @param cycMode select the cycle mode , Must be one of TRAP_CURVE_CYCMODE
 * @param cycRange bigger than zero is requested ; cycle Rangle( -cycle Rangle < s < cycle Rangle ). where the cycle mode is not disable , This parameter is valid
 */
void trap_Curve_Init(TRAP_CURVE_s *s, float vel_Max_Limit, float acc_global, float time_Now, int cycMode, float cycRange)
{
    if (s == NULL)
    {
        return;
    }

    memset(s, 0x00, sizeof(TRAP_CURVE_s));

    s->vel_Max_Limit = vel_Max_Limit;
    s->acc_global = acc_global;
    s->time_Now = time_Now;
    s->time_Start = time_Now;

    if (cycMode == TRAP_CURVE_CYCMODE_M1)
    {
        if (cycRange > 0)
        {
            s->cycMode = TRAP_CURVE_CYCMODE_M1;
            s->cycRange = cycRange;
            s->cycRange_2 = cycRange + cycRange ;
        }
        else
        {
            s->cycMode = TRAP_CURVE_CYCMODE_OFF;
        }
    }
    else
    {
        s->cycMode = TRAP_CURVE_CYCMODE_OFF;
    }

    s->status = TRAP_CURVE_STA_4_STOP;
    s->cycSign = TRAP_CURVE_CYCSIGN_NONE;
}

void trap_Curve_SetTarget(TRAP_CURVE_s *s, float target_Position)
{
    float acc;
    float acc_abs;
    float v_Max;
    float v_Max_abs;

    float S_Min;
    float S_Start;
    float S_Target;

    float Sp;
    float Sp_abs;
    float Vs;
    float Vs_abs;

    float Vm;
    float Vm_abs;

    float dir;

    float t1 = 0, t2 = 0, t3 = 0;
    float S1_Int = 0, S2_Int = 0, S3_Int = 0;

    /* Parameter copy */
    acc = s->acc_global;

    if (s->cycMode == TRAP_CURVE_CYCMODE_M1)
    {
        v_Max = s->vel_Max_Limit;
        S_Start = s->pos_Cur;
        S_Target = target_Position;
        Vs = s->vel_Cur;
        // Sp = S_Target - S_Start;
        Sp = shortARCCalculateGen(S_Start,target_Position,s->cycRange);
    }
    else
    {
        v_Max = s->vel_Max_Limit;
        S_Start = s->pos_Cur;
        S_Target = target_Position;
        Vs = s->vel_Cur;
        Sp = S_Target - S_Start;
    }

    acc_abs = fabsf(acc);
    Vs_abs = fabsf(Vs);
    Sp_abs = fabsf(Sp);
    v_Max_abs = fabs(v_Max);

    S_Min = 0.5f * Vs_abs * Vs_abs / acc_abs;

    if (Sp_abs == 0)
    {
        s->status = TRAP_CURVE_STA_4_STOP;
        return;
    }

    if ((Sp > 0) && (Vs >= 0))
    { // Positive direction
        /* No need to do special */
        dir = 1.0f;
    }
    else if ((Sp < 0) && (Vs <= 0))
    { // Reverse direction
        if (acc > 0)
            acc = -acc;
        if (v_Max > 0)
            v_Max = -v_Max;

        dir = -1.0f;
    }
    else
    { // moving to the opposite way , stop it first
        if (Vs <= 0)
        {
            acc = -acc;
        }
        Vm = v_Max;

        s->status = TRAP_CURVE_STA_3_BACK;
        s->t1 = Vs_abs / acc_abs; // the time need to be > 0 Always
        s->t2 = 0;
        s->t3 = 0;
        s->S1_Int = Vs * s->t1 - 0.5f * acc * s->t1 * s->t1;
        s->S2_Int = 0;
        s->S3_Int = 0;

        s->vel_Max = Vs;
        goto Apply_POINT;
    }

    Vm = v_Max;

    if ((Sp_abs < S_Min))
    { // The distance to the target is not sufficient to slow down
        s->status = TRAP_CURVE_STA_3_BACK;
        s->t1 = Vs_abs / acc_abs;
        s->t2 = 0;
        s->t3 = 0;
        s->S1_Int = 0;
        s->S2_Int = 0;
        s->S3_Int = 0;

        s->vel_Max = Vs;
    }
    else
    {
        Vm = sqrtf(0.5f * ((Vs_abs * Vs_abs) + 2 * Sp_abs * acc_abs)); // No direction
        if (Vm >= v_Max_abs)
        {              // complete trapezoidal curve
            Vm *= dir; // Key Action
            Vm_abs = fabsf(Vm);
            s->status = TRAP_CURVE_STA_1_T;
            t1 = (v_Max_abs - Vs_abs) / acc_abs;
            t2 = (Sp_abs - (2 * v_Max_abs * v_Max_abs - Vs_abs * Vs_abs) / (2 * acc_abs)) / v_Max_abs;
            t3 = v_Max_abs / acc_abs;

            S1_Int = Vs * t1 + 0.5f * acc * t1 * t1;
            S2_Int = S1_Int + v_Max * t2;
            S3_Int = S2_Int + v_Max * t3 - 0.5f * acc * t3 * t3; // Can be used for cross validation

            s->vel_Max = v_Max;
        }
        else
        {
            Vm *= dir; // key action
            Vm_abs = fabsf(Vm);
            s->status = TRAP_CURVE_STA_2_TRIANGLE;
            t1 = (Vm_abs - Vs_abs) / acc_abs;
            t2 = 0;
            t3 = Vm_abs / acc_abs;

            S1_Int = Vs * t1 + 0.5f * acc * t1 * t1;
            S2_Int = S1_Int;
            S3_Int = S1_Int + Vm * t3 - 0.5f * acc * t3 * t3; // Can be used for cross validation

            s->vel_Max = Vm;
        }
        // Update the struct value
        s->t1 = t1;
        s->t2 = t2;
        s->t3 = t3;
        s->S1_Int = S1_Int;
        s->S2_Int = S2_Int;
        s->S3_Int = S3_Int;
    }

Apply_POINT:

    s->acc_live = acc;

    s->target_Position = S_Target;

    // s->vel_Cur = Vs;
    s->vel_Orig = Vs;

    s->pos_Start = S_Start;
    // s->pos_Cur = S_Start;
    s->time_Start = s->time_Now;
    // s->time_Now = time_Now;
    s->Vm = Vm;
    s->S_Min = S_Min;
}

float trap_Curve_Update(TRAP_CURVE_s *s, float time_Now)
{
    float acc;
    float v_Max;

    float S_Min;
    float S_Start;
    float S_Target;

    float Sp;
    float Vs;
    float V_Cur;

    float Vm;

    float T1 = 0, T2 = 0, T3 = 0;
    float S1_Int = 0, S2_Int = 0, S3_Int = 0;

    float S_Cur;

    float time_Pass;

    V_Cur = s->vel_Cur;
    Vs = s->vel_Orig;
    acc = s->acc_live;
    v_Max = s->vel_Max;

    time_Pass = (time_Now - s->time_Start);

    if (s->status == TRAP_CURVE_STA_3_BACK) // Stop the device
    {
        if ((0 <= time_Pass) && (time_Pass < s->t1))
        {
            V_Cur = Vs - acc * time_Pass;
            S_Cur = Vs * time_Pass - 0.5f * acc * time_Pass * time_Pass;

            float temp = S_Cur + s->pos_Start;
            if( s->cycMode == TRAP_CURVE_CYCMODE_M1 ){
                if( (temp) > s->cycRange ){
                    S_Cur = temp - s->cycRange_2;
                }else if( (temp) < -(s->cycRange) ){
                    S_Cur = temp + s->cycRange_2;
                }else{
                    S_Cur = temp;
                }
            }else{
                S_Cur = temp;
            }
            
        }
        else
        {
            // S_Cur = s->target_Position;
            s->status = TRAP_CURVE_STA_4_STOP;
            if (fabsf(s->pos_Cur - s->target_Position) >= 0.01f)
            { // The device has not reached the target yet
                V_Cur = 0;
                s->vel_Cur = 0;
                trap_Curve_SetTarget(s, s->target_Position);
                return s->pos_Cur;
            }
        }
    }
    else if ((s->status == TRAP_CURVE_STA_1_T) || (s->status == TRAP_CURVE_STA_2_TRIANGLE))
    {
        if ((0 <= time_Pass) && (time_Pass < s->t1))
        {
            V_Cur = Vs + acc * time_Pass;
            S_Cur = Vs * time_Pass + 0.5f * acc * time_Pass * time_Pass;

            float temp = S_Cur + s->pos_Start;
            if( s->cycMode == TRAP_CURVE_CYCMODE_M1 ){
                if( (temp) > s->cycRange ){
                    S_Cur = temp - s->cycRange_2;
                }else if( (temp) < -(s->cycRange) ){
                    S_Cur = temp + s->cycRange_2;
                }else{
                    S_Cur = temp;
                }
            }else{
                S_Cur = temp;
            }
        }
        else if ((s->t1 <= time_Pass) && (time_Pass < (s->t1 + s->t2)) && (s->t2 != 0.0f))
        {
            T2 = time_Pass - s->t1;
            V_Cur = v_Max;
            S_Cur = s->S1_Int + v_Max * T2;

            float temp = S_Cur + s->pos_Start;
            if( s->cycMode == TRAP_CURVE_CYCMODE_M1 ){
                if( (temp) > s->cycRange ){
                    S_Cur = temp - s->cycRange_2;
                }else if( (temp) < -(s->cycRange) ){
                    S_Cur = temp + s->cycRange_2;
                }else{
                    S_Cur = temp;
                }
            }else{
                S_Cur = temp;
            }
        }
        else if (((s->t1 + s->t2) <= time_Pass) && (time_Pass < (s->t1 + s->t2 + s->t3)))
        {
            T3 = time_Pass - s->t1 - s->t2;
            V_Cur = v_Max - acc * T3;
            S_Cur = s->S2_Int + v_Max * T3 - 0.5f * acc * T3 * T3;

            float temp = S_Cur + s->pos_Start;
            if( s->cycMode == TRAP_CURVE_CYCMODE_M1 ){
                if( (temp) > s->cycRange ){
                    S_Cur = temp - s->cycRange_2;
                }else if( (temp) < -(s->cycRange) ){
                    S_Cur = temp + s->cycRange_2;
                }else{
                    S_Cur = temp;
                }
            }else{
                S_Cur = temp;
            }
        }
        else
        {
            S_Cur = s->target_Position;
            // Time is Out
            s->status = TRAP_CURVE_STA_4_STOP;
        }
    }

    if ((s->status == TRAP_CURVE_STA_4_STOP))
    {
        V_Cur = s->vel_Cur;
        S_Cur = s->pos_Cur;
    }

    s->vel_Cur = V_Cur;
    s->pos_Cur = S_Cur;
    s->time_Now = time_Now;
    return S_Cur;
}

// TRAP_CURVE_s set;

// FILE *fp = NULL;
// FILE *fpin = NULL;

// int main()
// {
//     // fp = fopen("data.txt", "r");
//     fpin = fopen("dataout.txt", "w+");

//     trap_Curve_Init(&set, 4.0f, 3.0f, 0);
//     set.pos_Cur = 1.0f;
//     set.vel_Cur = 3.6f;
//     trap_Curve_SetTarget(&set, -5.0f, 0);
//     float time = 0;
//     int judge[10] = {0};
//     while (1)
//     {
//         trap_Curve_Update(&set, time);
//         time += 0.01;
//         // printf("%f\n", set.pos_Cur);
//         fprintf(fpin, "%f\n", set.pos_Cur);
//         if ((time >= 5) && (judge[0] == 0))
//         {
//             trap_Curve_SetTarget(&set, -10.0f, time);
//             judge[0] = 1;
//         }
//         if ((time >= 6) && (judge[1] == 0))
//         {
//             trap_Curve_SetTarget(&set, 0.0f, time);
//             judge[1] = 1;
//         }
//         if ((time >= 8) && (judge[2] == 0))
//         {
//             trap_Curve_SetTarget(&set, -11.0f, time);
//             judge[2] = 1;
//         }
//         if ((time >= 12) && (judge[3] == 0))
//         {
//             trap_Curve_SetTarget(&set, -2.0f, time);
//             judge[3] = 1;
//         }
//         if (time >= 20)
//         {
//             break;
//         }
//     }
//     fclose(fpin);
// }