#include "io.h"
#include "Board.h"

#define TU 2//2.55
#define KU 0.998
#define TS 0.1

#define KC  (0.6*KU*(1-(TS/TU)))
#define TI (TU-TS)/2
#define TD (TU*TU)/((8*TU)-TS)

#define Q0 (-1.0-(TD/TS))
#define Q1 (1.0+(2*(TD/TS)))
#define Q2 ((-TD/TS))
#define Q3 (TS/TI)

int controller(int dist, int setpoint)
{

    static float output = 0.0;
    static float y0 = 0.0;
    static float y1 = 0.0;
    static float y2 = 0.0;
    static float u = 0.0;
    static float u_oud = 0.0;

    float error = setpoint - dist;
    y0 = dist;
    //float output = Kc * error;
    u = u_oud + KC*(Q0*(y0)+Q1*(y1)+Q2*(y2)+Q3*(error));
    //u *= 10;
    u_oud = u;
   /* if (u > 1000)
    {
        output = 1000;
    }
    else if (u < 0)
    {
        output = 0;
    }
    else
    {
        output = u;
    }*/

    output = u;
    y2 = y1;
    y1 = y0;

    return output;
}

void *mainThread(void *arg0)
{

    if (initUART() == false)
    {
        // initUart() failed
        while(1);
    }
    // Samplefrequency = 10 Hz
    if (initTimer(10) == false)
    {
        sendStringUART("initTimer() failed");
        while(1);
    }

    initPort();

    if (initPWM() == false)
    {
        sendStringUART("initPWM() failed");
        while(1);
    }

    while(1)
    {
        trigger(); // Trigger routine creates a 10 usec trigger pulse for the ultrasonic sensor.
        int PWM = controller(distance, 500);
        setPWM(PWM);

        while (tick == false)
        {
            // wait for next timer tick
        }
        tick = false;
    }

    return 0;
}