#include "Slope.h"
//#include <QDebug>

void Slope::slopeCalcStandardProfile(PrivateSlopeCalcData &pscd)
{
    double deltaV=0;
    pscd.s1=0;
    pscd.s2=0;
    /* Phasen 1-3 */
    if (fabs(pscd.vs - pscd.vn)<SLOPECALC_EPSILON) {
        pscd.T1 = pscd.T2 = pscd.T3 = 0.0;
        pscd.s1 = 0.0;
    } else {
        deltaV = fabs(pscd.vs - pscd.vn);
        if (deltaV <= pscd.vLimit) {
            pscd.T1 = pscd.T3 = sqrt(deltaV / pscd.r_);
            pscd.T2 = 0.0;
        } else {
            pscd.T1 = pscd.T3 = pscd.tLimit;
            pscd.T2 = (deltaV - pscd.vLimit) / pscd.amax;
        }
        pscd.s1 = (pscd.vs + pscd.vn) * (pscd.T1 + pscd.T2 + pscd.T3) / 2.0;
    }

    /* Phasen 5-7 */
    if (fabs(pscd.vn - pscd.ve)<SLOPECALC_EPSILON) {
        pscd.T5 = pscd.T6 = pscd.T7 = 0.0;
        pscd.s2 = 0.0;
    } else {
        deltaV = fabs(pscd.vn - pscd.ve);
        if (deltaV <= pscd.vLimit2) {
            pscd.T5 = pscd.T7 = sqrt(deltaV / pscd.r2_);
            pscd.T6 = 0.0;
        } else {
            pscd.T5 = pscd.T7 = pscd.tLimit2;
            pscd.T6 = (deltaV - pscd.vLimit2) / pscd.amax2;
        }
        pscd.s2 = (pscd.vn + pscd.ve) * (pscd.T5 + pscd.T6 + pscd.T7) / 2.0;
    }

    /* benoetigte Strecke */
    pscd.minS = pscd.s1 + pscd.s2;
}

void Slope::slopeCalcEstimateVN(PrivateSlopeCalcData &pscd, double lowerBound)
{
    double vn1, vn2;
    unsigned int count;

    vn1 = lowerBound;
    vn2 = pscd.vn;

    /* Überprüfung ob untere Intervallgrenze gültig ist */
    if (lowerBound > 0.0) {
        pscd.vn = lowerBound;
        slopeCalcStandardProfile(pscd);
        if (pscd.minS > pscd.s) {
            return;  /* Ist sie nicht -> VE muss angepasst werden */
        }
    }

    count = 0;
    while(1) {
        pscd.vn = 0.5 * (vn1 + vn2);
        slopeCalcStandardProfile(pscd);

        if (pscd.minS / pscd.s > (1 + SLOPECALC_EPSILON)) {  /* vn immer noch zu hoch */
            vn2 = pscd.vn;
        } else {
            if (pscd.minS / pscd.s > 0.9) {
                return;    /* Gut genug -> Abbruch */
            }
            vn1 = pscd.vn;
        }

        count++;
        if (count > 12 && lowerBound > 0.0) break;  /* Zu viele Iterationen */
    }

    pscd.vn = lowerBound;
    slopeCalcStandardProfile(pscd);
}

void Slope::slopeCalcAdjustVN(PrivateSlopeCalcData &pscd)
{
    double minV, maxV;

    minV = pscd.vs;
    if (minV > pscd.ve)
        minV = pscd.ve;

    maxV = pscd.vs;

    if (maxV < pscd.ve)
        maxV = pscd.ve;

    if (pscd.vn > maxV) {
        slopeCalcEstimateVN(pscd, maxV);
        return;                      /* Profil ist bereits berechnet */
    } else {
        if (pscd.vn < minV) {
            pscd.vn = minV;
        } else {
            pscd.vn = maxV;
        }
    }

    slopeCalcStandardProfile(pscd);
}

void Slope::slopeCalcAdjustVE_Accelerate(PrivateSlopeCalcData &pscd)
{
    double ve1, ve2;

    ve1 = pscd.vs;
    ve2 = pscd.ve;

    while(1) {
        pscd.ve = 0.5 * (ve1 + ve2);
        pscd.vn = pscd.ve;
        slopeCalcStandardProfile(pscd);

        if (pscd.minS / pscd.s > (1 + SLOPECALC_EPSILON)) {  /* ve immer noch zu hoch */
            ve2 = pscd.ve;
        } else {
            if (pscd.minS / pscd.s > 0.9) {
                return;    /* Gut genug -> Abbruch */
            }
            ve1 = pscd.ve;
        }
    }
}

void Slope::slopeCalcAdjustVE_Decelerate(PrivateSlopeCalcData &pscd)
{
    pscd.T2 = 2 * pscd.s / (pscd.vs + pscd.ve);
    pscd.amax = - (pscd.vs - pscd.ve) / pscd.T2;
    pscd.exceedAR = true;  /* Flag für Ausnahmeprofil setzen (blockfoermige Beschleunigung) */

    pscd.T1 = pscd.T3 = 0;
    pscd.T4 = 0;
    pscd.T5 = pscd.T6 = pscd.T7 = 0;
}

void Slope::slopeCalcPhase1(PrivateSlopeCalcData &pscd, double t, double *pa, double *pv, double *ps)
{
    *pa = pscd.r1 * t;
    *pv = pscd.phaseV[0] + 1.0/2.0 * pscd.r1 * t*t;
    *ps = pscd.phaseV[0] * t + 1.0/6.0 * pscd.r1 * t*t*t;
}

void Slope::slopeCalcPhase2(PrivateSlopeCalcData &pscd, double t, double *pa, double *pv, double *ps)
{
    *pa = pscd.phaseA[1];
    *pv = pscd.phaseV[1] + pscd.phaseA[1] * t;
    *ps = pscd.phaseS[1] + pscd.phaseV[1] * t + 1.0/2.0 * pscd.phaseA[1] * t*t;
}

void Slope::slopeCalcPhase3(PrivateSlopeCalcData &pscd, double t, double *pa, double *pv, double *ps)
{
    *pa = pscd.phaseA[2] - pscd.r1 * t;
    *pv = pscd.phaseV[2] + pscd.phaseA[2] * t - 0.5 * pscd.r1 * t*t;
    *ps = pscd.phaseS[2] + pscd.phaseV[2] * t + 0.5 * pscd.phaseA[2] * t*t - 1.0/6.0 * pscd.r1 * t*t*t;
}

void Slope::slopeCalcPhase4(PrivateSlopeCalcData &pscd, double t, double *pa, double *pv, double *ps)
{
    *pa = 0.0;
    *pv = pscd.phaseV[3];
    *ps = pscd.phaseS[3] + pscd.phaseV[3] * t;
}

void Slope::slopeCalcPhase5(PrivateSlopeCalcData &pscd, double t, double *pa, double *pv, double *ps)
{
    *pa = pscd.r2 * t;
    *pv = pscd.phaseV[4] + 1.0/2.0 * pscd.r2 * t*t;
    *ps = pscd.phaseS[4] + pscd.phaseV[4] * t + 1.0/6.0 * pscd.r2 * t*t*t;
}

void Slope::slopeCalcPhase6(PrivateSlopeCalcData &pscd, double t, double *pa, double *pv, double *ps)
{
    *pa = pscd.phaseA[5];
    *pv = pscd.phaseV[5] + pscd.phaseA[5] * t;
    *ps = pscd.phaseS[5] + pscd.phaseV[5] * t + 1.0/2.0 * pscd.phaseA[5] * t*t;
}

void Slope::slopeCalcPhase7(PrivateSlopeCalcData &pscd, double t, double *pa, double *pv, double *ps)
{
    *pa = pscd.phaseA[6] - pscd.r2 * t;
    *pv = pscd.phaseV[6] + pscd.phaseA[6] * t - 0.5 * pscd.r2 * t*t;
    *ps = pscd.phaseS[6] + pscd.phaseV[6] * t + 0.5 * pscd.phaseA[6] * t*t - 1.0/6.0 * pscd.r2 * t*t*t;
}

bool Slope::slopeCalcInit(PrivateSlopeCalcData &pscd, SlopeCalcData &scd)
{

    /* Eingabedaten kopieren und nach REAL konvertieren */
    pscd.cycle_time = scd.in_cycle_time;
    pscd.vs = scd.in_vs;
    pscd.vn = scd.in_vn;
    pscd.ve = scd.in_ve;
    pscd.s = scd.in_s;
    pscd.amax = scd.in_amax;
    pscd.amax2 = scd.in_amax2;
    pscd.r_ = scd.in_r;
    pscd.r2_ = scd.in_r2;
    pscd.currentV = pscd.vs;

    scd.out_error = Slope_Ok;
    pscd.exceedAR = false;

    /* Eingabewerte pruefen */
    if (scd.in_cycle_time <= 0.0
            || (scd.in_vn <= 0 && scd.in_vs <= 0)       /* vn == 0 -> Sonderfall Soforthalt */
            || scd.in_s <= 0.0
            || scd.in_amax <= 0.0
            || scd.in_r <= 0.0)
    {
        pscd.nCycles = 0;
        pscd.iCycle = 1;
        scd.out_error = Slope_InvalidArguments;
        scd.out_delta_s = pscd.currentV;
        scd.out_stop = scd.out_stop_next = true;
        scd.out_acc_zero = true;
        return false;
    }

    /* Profil berechnen */
    pscd.tLimit = pscd.amax / pscd.r_;
    pscd.vLimit = pscd.amax * pscd.tLimit;

    /* Limits Phase5-7 berechnen */
    pscd.tLimit2 = pscd.amax2 / pscd.r2_;
    pscd.vLimit2 = pscd.amax2 * pscd.tLimit2;

    pscd.T4 = -1.0;  /*  < 0 bedeutet: T4 wurde noch nicht berechnet  */


    if (scd.in_vn == 0) {   /* Noch ein Sonderfall: Wenn vn == 0 dann auf 0 runterbremsen (evtl. auf kuerzerer Strecke) */
        pscd.vn = pscd.vs;
        pscd.ve = 0.0;
    }

    slopeCalcStandardProfile(pscd);

    if (scd.in_vn == 0) {  /* Sonderfall Soforthalt */
        if (pscd.s > pscd.minS) {
            pscd.s = pscd.minS;  /* So schnell anhalten wie es geht -> kurze Strecke */
        }
    }

    if (pscd.minS / pscd.s > (1 + SLOPECALC_EPSILON)) {  /*  vn anpassen?  */
        slopeCalcAdjustVN(pscd);
        scd.out_error = Slope_VN_Adjusted;
    }

    if (pscd.minS / pscd.s > (1 + SLOPECALC_EPSILON)) {  /*  ve anpassen?  */

        if (pscd.vs <= pscd.ve && scd.in_vn > 0) {
            slopeCalcAdjustVE_Accelerate(pscd);
            scd.out_error = Slope_VE_Adjusted;
        } else {
            slopeCalcAdjustVE_Decelerate(pscd);
            scd.out_error = Slope_RInf;
        }

        if (pscd.vs > pscd.ve){
            pscd.r1=-pscd.r_;
            pscd.r2=pscd.r2_;
        }else{
            pscd.r1=pscd.r_;
            pscd.r2=-pscd.r2_;
        }

    } else {
        if (pscd.vs > pscd.vn){
            pscd.r1=-pscd.r_;
            pscd.r2=pscd.r2_;
        }else{
            pscd.r1=pscd.r_;
            pscd.r2=-pscd.r2_;
        }

        if (pscd.vn > pscd.ve){
            pscd.r2=-pscd.r2_;
        }else{
            pscd.r2=pscd.r2_;
        }
    }

    if (pscd.T4 < 0.0) {  /* T4 berechnen wenn noch nicht passiert */
        pscd.T4 = (pscd.s - pscd.minS) / pscd.vn;
    }

    pscd.Tcalc = pscd.T1 + pscd.T2 + pscd.T3 + pscd.T4 + pscd.T5 + pscd.T6 + pscd.T7;

    /* Phasenuebergaenge vorberechnen */
    pscd.phaseT[0] = 0.0;
    pscd.phaseA[0] = 0.0;
    pscd.phaseV[0] = pscd.vs;
    pscd.phaseS[0] = 0.0;

    pscd.phaseT[1] = pscd.T1;
    slopeCalcPhase1(pscd, pscd.T1, &pscd.phaseA[1], &pscd.phaseV[1], &pscd.phaseS[1]);
    if (pscd.exceedAR)
        pscd.phaseA[1] = pscd.amax;  /* Ausnahmeprofil */

    pscd.phaseT[2] = pscd.phaseT[1] + pscd.T2;
    slopeCalcPhase2(pscd, pscd.T2, &pscd.phaseA[2], &pscd.phaseV[2], &pscd.phaseS[2]);
    pscd.phaseT[3] = pscd.phaseT[2] + pscd.T3;
    slopeCalcPhase3(pscd, pscd.T3, &pscd.phaseA[3], &pscd.phaseV[3], &pscd.phaseS[3]);
    pscd.phaseT[4] = pscd.phaseT[3] + pscd.T4;
    slopeCalcPhase4(pscd, pscd.T4, &pscd.phaseA[4], &pscd.phaseV[4], &pscd.phaseS[4]);
    pscd.phaseT[5] = pscd.phaseT[4] + pscd.T5;
    slopeCalcPhase5(pscd, pscd.T5, &pscd.phaseA[5], &pscd.phaseV[5], &pscd.phaseS[5]);
    pscd.phaseT[6] = pscd.phaseT[5] + pscd.T6;
    slopeCalcPhase6(pscd, pscd.T6, &pscd.phaseA[6], &pscd.phaseV[6], &pscd.phaseS[6]);

    /* Restliche Werte fuer slopeCalcExe initialisieren */
    pscd.nCycles = (unsigned int)ceil(pscd.Tcalc / pscd.cycle_time);
    pscd.Treal = pscd.nCycles * pscd.cycle_time;
    pscd.iCycle = 1;
    pscd.lastExeS = 0.0;
    /* was wenn nCycles == 0 ?? */

    /* Ausgabewerte setzen */
    scd.out_stop = false;
    scd.out_stop_next = false;
    scd.out_delta_s = 0;

    return true;
}

void Slope::slopeCalcExe(PrivateSlopeCalcData &pscd, SlopeCalcData &scd)
{
    double t;
    double a, v, s;
    /* Letzter Takt schon vorbei? -> Fehler! */
    if (pscd.iCycle > pscd.nCycles) {
        scd.out_stop = scd.out_stop_next = true;
        scd.out_acc_zero = true;
        scd.out_delta_s = pscd.currentV;
        if (pscd.ve == 0.0) scd.out_delta_s = 0.0;
        return;
    }

    /* Aktuelle Zeit seit Satzanfang */
    t = (pscd.iCycle * pscd.Treal) / pscd.nCycles;

#if 0
    /* Lineare Zeitskalierung: Starker Ruck am Satzanfang, falsche Endgeschwindigkeit */
    t = t*pscd.Tcalc/pscd.Treal;
#else
    /* sin^2 Zeitskalierung: Ueberschreitung von amax/r moeglich */
    s = sin( t/pscd.Treal * M_PI/2.0 );
    t = t - (pscd.Treal - pscd.Tcalc) * s*s;
#endif

    scd.out_stop = false;
    scd.out_acc_zero = false;

    /* Auswahl und Berechnung der aktuellen Phase */
    if (t < pscd.phaseT[1]) {
        slopeCalcPhase1(pscd, t - pscd.phaseT[0], &a, &v, &s);
    } else if (t < pscd.phaseT[2]) {
        slopeCalcPhase2(pscd, t - pscd.phaseT[1], &a, &v, &s);
    } else if (t < pscd.phaseT[3]) {
        slopeCalcPhase3(pscd, t - pscd.phaseT[2], &a, &v, &s);
    } else if (t < pscd.phaseT[4]) {
        slopeCalcPhase4(pscd, t - pscd.phaseT[3], &a, &v, &s);
        scd.out_acc_zero = true;  /* Phase 4 -> Beschleunigung ist Null */
    } else if (t < pscd.phaseT[5]) {
        slopeCalcPhase5(pscd, t - pscd.phaseT[4], &a, &v, &s);
    } else if (t < pscd.phaseT[6]) {
        slopeCalcPhase6(pscd, t - pscd.phaseT[5], &a, &v, &s);
    } else {
        slopeCalcPhase7(pscd, t - pscd.phaseT[6], &a, &v, &s);
    }

    scd.out_delta_s = (s - pscd.lastExeS) / pscd.cycle_time;
    pscd.lastExeS = s;
    scd.out_current_a= a;
    pscd.currentV = v;
    scd.out_current_v = v;
    pscd.iCycle++;
    /* Vorletzter Takt erreicht? */
    if (pscd.iCycle >= pscd.nCycles) {
        scd.out_stop_next = true;
    }
    /* Letzter Takt erreicht? */
    if (pscd.iCycle > pscd.nCycles) {
        scd.out_stop = true;     /* Dann stop */
        scd.out_acc_zero = true;
    }
    pscd.s_abs+=scd.out_delta_s;
}

void Slope::slopeCalcExe_for_time(PrivateSlopeCalcData &pscd, SlopeCalcData &scd, double t)
{

    //double t;
    double a, v, s;
    /* Letzter Takt schon vorbei? -> Fehler! */
    if (pscd.iCycle > pscd.nCycles) {
        scd.out_stop = scd.out_stop_next = true;
        scd.out_acc_zero = true;
        scd.out_delta_s = pscd.currentV;
        if (pscd.ve == 0.0) scd.out_delta_s = 0.0;
        return;
    }

    /* Aktuelle Zeit seit Satzanfang */
    //t = (pscd.iCycle * pscd.Treal) / pscd.nCycles;

#if 0
    /* Lineare Zeitskalierung: Starker Ruck am Satzanfang, falsche Endgeschwindigkeit */
    t = t*pscd.Tcalc/pscd.Treal;
#else
    /* sin^2 Zeitskalierung: Ueberschreitung von amax/r moeglich */
    //    s = sin( t/pscd.Treal * M_PI/2.0 );
    //    t = t - (pscd.Treal - pscd.Tcalc) * s*s;
#endif

    scd.out_stop = false;
    scd.out_acc_zero = false;

    /* Auswahl und Berechnung der aktuellen Phase */
    if (t < pscd.phaseT[1]) {
        slopeCalcPhase1(pscd, t - pscd.phaseT[0], &a, &v, &s);
    } else if (t < pscd.phaseT[2]) {
        slopeCalcPhase2(pscd, t - pscd.phaseT[1], &a, &v, &s);
    } else if (t < pscd.phaseT[3]) {
        slopeCalcPhase3(pscd, t - pscd.phaseT[2], &a, &v, &s);
    } else if (t < pscd.phaseT[4]) {
        slopeCalcPhase4(pscd, t - pscd.phaseT[3], &a, &v, &s);
        scd.out_acc_zero = true;  /* Phase 4 -> Beschleunigung ist Null */
    } else if (t < pscd.phaseT[5]) {
        slopeCalcPhase5(pscd, t - pscd.phaseT[4], &a, &v, &s);
    } else if (t < pscd.phaseT[6]) {
        slopeCalcPhase6(pscd, t - pscd.phaseT[5], &a, &v, &s);
    } else {
        slopeCalcPhase7(pscd, t - pscd.phaseT[6], &a, &v, &s);
    }

    scd.out_delta_s = (s - pscd.lastExeS) / pscd.cycle_time;

    pscd.lastExeS = s;
    scd.out_current_a= a;
    pscd.currentV = v;
    scd.out_current_v = v;
    pscd.iCycle++;
    /* Vorletzter Takt erreicht? */
    if (pscd.iCycle >= pscd.nCycles) {
        scd.out_stop_next = true;
    }
    /* Letzter Takt erreicht? */
    if (pscd.iCycle > pscd.nCycles) {
        scd.out_stop = true;     /* Dann stop */
        scd.out_acc_zero = true;
    }

    pscd.s_abs+=scd.out_delta_s;

}
