#include "nnxt.h"
#include <stdio.h>

#define DEG_TO_RPM   166.6667                 // Umrechnungsfaktor [°/ms] in [U/min]
#define PI           3.14159

double to_cm_in_sek(double rpm) {

    double durchmesser = 4.12;
    double umfang = PI * durchmesser;
    double sekunden = 60.0;

    return rpm * umfang / sekunden;
}

void MotorLinks(){
    char rpmMsg[25];                        // enthält Umdrehung/min als String für Ausgabe auf Display
    uint32_t prev_deg, deg;                 // deg enthält Differenz vom Zähler jetzt und bei der letzten Messung vor 500ms (prev_deg)
    double dt, prev_tick = GetSysTime();  // dt enthält Differenz der Zeiten jetzt und der Zeit der letzten Messung (prev_tick)
    double rpm;                             // enthält Wert der Umdrehungen pro Minute
    double sIst = 0, sSoll, sDelta = 0;

    double s=0,a;
    double sDeltaAlt;
    uint8_t speed = 20;

    MotorPortInit(Port_A);
    MotorPortInit(Port_B);
    Motor_Tacho_GetCounter(Port_A, &prev_deg);
    Motor_Drive(Port_A, Motor_dir_forward, speed);        // hier gerne mal anpassen und probieren
    Motor_Drive(Port_B, Motor_dir_forward, speed);        // hier gerne mal anpassen und probieren
    while(1) {
        Delay(500);                                    // Abtastintervall 500ms
        dt = GetSysTime() - prev_tick;                 // Zeitdifferenz seit letzter Messung ermitteln
        Motor_Tacho_GetCounter(Port_A, &deg);
        deg = deg - prev_deg;                          // um wieviel ist der Umdrehungszähler seit letzter Messung gewachsen
        rpm = ((deg*2.045) * DEG_TO_RPM) / (double)dt;  // deg*1.33 weil Auflösung Drehzahlsensor 1 Digit entspr. 1,33 Grad
        Motor_Tacho_GetCounter(Port_A, &prev_deg);     // Zählerstand für nächste Messung vorbereiten
        prev_tick = GetSysTime();                      // Zeitmessung für nächste Messung vorbereiten


        sIst   = to_cm_in_sek(rpm);  // aktuelle  geschwindigkeit [cm/s]
        sSoll  = 6.0;                // gegebene  geschwindigkeit [cm/s]
        //sDelta = sSoll - sIst;       // differenz geschwindigkeit [cm/s]
        dt     = dt/1000;               // Umwandlung ms in s
        sDelta = sSoll - sIst ;         // strecke


        s = s + sDelta;              // aktuelle differenz der strecke

        // P-Regler
        //a = 1.8 * sDelta;

        // PI-Regler
        //a = 1.4 * sDelta + 1.1 * dt * s;

        // PD-Regler
        //a = 1.8 * sDelta + (1.0 * (sDelta - sDeltaAlt)/dt) ;

        //PID-Regler
        a = 20 + 0.1 * sDelta + 0.2 * dt * s + (0.1 * (sDelta - sDeltaAlt) / dt);

        sDeltaAlt = sDelta;         // speichern des alten Delta

        // Motor geschwindigkeit
        //speed = (int8_t)speed + (int8_t)a;
        Motor_Drive(Port_A, Motor_dir_forward, a);
        Motor_Drive(Port_B, Motor_dir_forward, a);

        // Ausgabe
        sprintf(rpmMsg,"%d  rpm",(int)rpm);
        NNXT_LCD_DisplayStringAtLine(0,rpmMsg);

        sprintf(rpmMsg,"%d cm/s sDelta",(int)sDelta);
        NNXT_LCD_DisplayStringAtLine(1,rpmMsg);

        sprintf(rpmMsg,"%d cm/s sDeltaDif",(int)(sDelta - sDeltaAlt));
        NNXT_LCD_DisplayStringAtLine(2,rpmMsg);

        sprintf(rpmMsg,"%d s",(int)s);
        NNXT_LCD_DisplayStringAtLine(3,rpmMsg);

        sprintf(rpmMsg,"%d a",(int)a);
        NNXT_LCD_DisplayStringAtLine(4,rpmMsg);

        sprintf(rpmMsg,"speed %d  ",speed);
        NNXT_LCD_DisplayStringAtLine(5,rpmMsg);
    }
    return 0;
}

void MotorRechts(){

}

int main(){


    CreateAndStartTask(MotorLinks);
    //CreateAndStartTask(MotorRechts);
    StartScheduler();

    return 0;
}