#ifndef SLOPE_H
#define SLOPE_H

#include <math.h>
#include <stdlib.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#define SLOPECALC_EPSILON 1e-6   /* Relative(!) Abweichung bei Test auf Gleichheit */

enum SlopeErrorCode {
  Slope_Ok = 0,
  Slope_InvalidArguments,
  Slope_VN_Adjusted,
  Slope_VE_Adjusted,
  Slope_RInf
};

/* SlopeCalc verwendet eine beliebige Längeneinheit "w" und Zeiteinheit "z".
   Die Einheit "w" ist z.B. Mikrometer, "z" ist z.B. Sekunde oder Takt.
   Diese Einheiten müssen fuer alle Eingabewerte einheitlich sein.
*/

struct SlopeCalcData {

    SlopeCalcData():
        in_cycle_time(0),
        in_vs(0),
        in_vn(0),
        in_ve(0),
        in_s(0),
        in_amax(0),
        in_amax2(0),
        in_r(0),
        in_r2(0),
        out_delta_s(0),
        out_current_v(0),
        out_current_a(0),
        out_stop(0),
        out_stop_next(0),
        out_acc_zero(0)
        {}
  /*  input (init)  */
  double in_cycle_time;  /*  z          >  0    Taktzeit                                      */
  double in_vs;    /*  w/z        >= 0    Geschwindigkeit am Anfang des Satzes          */
  double in_vn;    /*  w/z        >  0    Sollgeschwindigkeit im Satz                   */
  double in_ve;    /*  w/z        >= 0    Geschwindigkeit am Ende des Satzes            */
  double in_s;           /*  w          >  0    Zu fahrende Strecke                           */
  double in_amax;  /*  w/(z*z)    >  0    Maximale Beschleunigung                       */
  double in_amax2;  /*  w/(z*z)    >  0    Maximale Beschleunigung                       */
  double in_r;     /*  w/(z*z*z)  >  0    Ruck                                          */
  double in_r2;     /*  w/(z*z*z)  >  0    Ruck                                          */
  /*  output (exe)  */
  double    out_delta_s;        /*  w/z                Vorschub im aktuellen Takt                    */
  double    out_current_v;
  double    out_current_a;
  bool      out_stop;           /*                     Letzter Takt erreicht,
                                                slopeCalcExe nicht nochmal aufrufen           */
  bool      out_stop_next;      /*                     Vorletzter Takt erreicht.                     */
  bool      out_acc_zero;       /*                     Beschleunigung ist Null, */

  /*  output (init)  */
  enum SlopeErrorCode out_error;  /*            Fehler/Warnung                                */

                                               /* slopeCalcInit darf erneut aufgerufen werden   */
};

struct PrivateSlopeCalcData {
  double cycle_time;     /*  w          */
  double vs;             /*  w/z        */
  double vn;             /*  w/z        */
  double ve;             /*  w/z        */
  double s;              /*  w          */
  double amax;           /*  w/(z*z)    */
  double amax2;           /*  w/(z*z)    */

  double r_;              /*  w/(z*z*z)  */
  double r2_;              /*  w/(z*z*z)  */

  double tLimit;         /*  z                  maximales T1 fuer Dreiecksprofil                  */
  double vLimit;         /*  w/z                maximales Delta-v bei Dreiecksprofil              */

  double tLimit2;         /*  z                  maximales T1 fuer Dreiecksprofil                  */
  double vLimit2;         /*  w/z                maximales Delta-v bei Dreiecksprofil              */

  double minS;           /*  w                  mindestens benoetigte Strecke fuer Standardprofil */
  double T1, T2, T3;
  double T4;
  double T5, T6, T7;     /*  z                  Laenge der 7 Phasen des Beschleunigungsprofils    */
  double r1, r2;         /*  w/(z*z*z)          Ruck in Phasen 1-3 bzw Phasen 5-7                 */
  bool exceedAR;      /*                     Beschleunigung und Ruck werden ueberschritten     */

  double Tcalc;          /*  z                  berechnete Gesamtzeit                             */
  double phaseT[7];      /*  z                  t am Ende der Phase i bzw. Anfang der Phase i+1   */
  double phaseA[7];      /*  w/(z*z)            a am Ende der Phase i bzw. Anfang der Phase i+1   */
  double phaseV[7];      /*  w/z                v am Ende der Phase i bzw. Anfang der Phase i+1   */
  double phaseS[7];      /*  w                  s am Ende der Phase i bzw. Anfang der Phase i+1   */

  unsigned int  nCycles;        /*                     Anzahl der Taktzyklen im Satz                     */
  double Treal;          /*  z                  reale Gesamtzeit als ganzes Vielfaches des Takts  */
  unsigned int  iCycle;         /*                     Naechster zu berechnender Takt [1..nCycles]       */
  double lastExeS;       /*  w                  Strecke des vorherigen Takts                      */
  double currentV;       /*  w/z                Geschwindigkeit am Ende des aktuellen Takts       */
  double currentA;       /*  w/z                Beschleunigung am Ende des aktuellen Takts       */
  double s_abs;
  double s1;
  double s2;
};

//static struct PrivateSlopeCalcData pscd;
/* Berechnet das normale Trapez-Beschleunigungsprofil
   Ausgabe: T1-T3, T5-T6, minS
 */

class Slope
{

public:
    /* Vorberechnung des Fahrprofils fuer einen neuen Satz.
       Berechnet alle von slopeCalcExe() benoetigten Werte.
       Eingabe: globale struct SlopeCalcData

       Return: true wenn die Eingabewerte gueltig waren.

       out_error ist ungleich Null wenn Geschwindigkeit oder Beschleunigung+Ruck
           angepasst werden mussten.

       Nach slopeCalcInit() muss einmal slopeCalcExe() aufgerufen werden, um den Vorschub
       fuer den ersten Takt zu bekommen.
     */
    bool slopeCalcInit(PrivateSlopeCalcData &pscd, SlopeCalcData &scd);


    /* Berechnung des Vorschubs im aktuellen Takt.
       Ausgabe: out_delta_s

       Diese Funktion muss so lange aufgerufen werden, bis im letzten gueltigen Takt das
       Flag out_stop gesetzt ist.

       Aenderungen der Sollwerte sind nur zulaessig wenn die Beschleunigung Null ist, also
       das Flag out_acc_zero gesetzt ist. Nur dann sollte slopeCalcInit() erneut aufgerufen werden.

       Der Ausgabewert ist immer der naechste Vorschub, und entspricht der mittleren Geschwindigkeit
       auf der Strecke, die in diesem Takt durchfahren wird. Daher ist der letzte ausgegebene
       Vorschub NICHT die Endgeschwindigkeit.
       Durch eine solche falsche Wahl der Anfangsgeschwindigkeit des folgenden Satzes könnte
       ein (sehr kleiner) Ruck entstehen.
       slopeCalcExe() koennte die aktuelle wirkliche Geschwindigkeit am Taktende ausgeben wenn gewuenscht.
     */

    void slopeCalcExe(PrivateSlopeCalcData &pscd, SlopeCalcData &scd);
    void slopeCalcExe_for_time(PrivateSlopeCalcData &pscd, SlopeCalcData &scd, double t);

private:
    void slopeCalcStandardProfile(PrivateSlopeCalcData &pscd);

    /* Abschaetzen einer neuen Sollgeschwindigkeit vn, wenn vn > max(vs, ve)
       Ausgabe: vn, T1-T3, T5-T7, minS
     */
    void slopeCalcEstimateVN(PrivateSlopeCalcData &pscd, double lowerBound);


    /* vn anpassen wenn das vorgegebene vn nicht erreicht werden kann (Strecke zu kurz)
       Ausgabe: vn, T1-T3, T5-T7, minS
     */
    void slopeCalcAdjustVN(PrivateSlopeCalcData &pscd);



    /* ve anpassen wenn das vorgegebene ve nicht erreicht werden kann (Strecke zu kurz),
       wenn die Geschwindigkeit in dem Satz zunimmt.
       Ausgabe: vn, ve, T1-T3, T5-T7, minS
     */
    void slopeCalcAdjustVE_Accelerate(PrivateSlopeCalcData &pscd);


    /* Wenn das vorgegebene ve beim abbremsen nicht erreicht werden kann (Strecke zu kurz),
       werden Ruck und Beschleunigung überschritten. ve soll in diesem Fall unbedingt
       erreicht werden.
       Ausgabe: T1-T7, exceedAR
     */
    void slopeCalcAdjustVE_Decelerate(PrivateSlopeCalcData &pscd);



    /* Berechnung von a, v und s fuer die entsprechende Phase.
       Achtung: t ist die Zeit seit dem Beginn der Phase!
     */

    void slopeCalcPhase1(PrivateSlopeCalcData &pscd, double t, double* pa, double* pv, double* ps);
    void slopeCalcPhase2(PrivateSlopeCalcData &pscd, double t, double* pa, double* pv, double* ps);
    void slopeCalcPhase3(PrivateSlopeCalcData &pscd, double t, double* pa, double* pv, double* ps);
    void slopeCalcPhase4(PrivateSlopeCalcData &pscd, double t, double* pa, double* pv, double* ps);
    void slopeCalcPhase5(PrivateSlopeCalcData &pscd, double t, double* pa, double* pv, double* ps);
    void slopeCalcPhase6(PrivateSlopeCalcData &pscd, double t, double* pa, double* pv, double* ps);
    void slopeCalcPhase7(PrivateSlopeCalcData &pscd, double t, double* pa, double* pv, double* ps);

};
#endif
