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

#include <ti/drivers/GPIO.h>
#include <ti/drivers/UART.h>

#include <unistd.h>

extern volatile int32_t distance;
extern volatile float Kc;

void trigger()
{
    GPIO_write(Board_TRIGGER, 1);
    usleep(10);
    GPIO_write(Board_TRIGGER, 0);
}

int controller(int dist, int SET)
{
    const float TU = 2.55;
    const float KU = 0.998;

    const float TS =  0.2;

    //const float SET =  50.0;

    const float KC = (0.6*KU*(1-(TS/TU)));
    const float TI = (TU-TS)/2;
    const float TD = (TU*TU)/(8*TU-TS);

    const float Q0 = (-1.0-(TD/TS));
    const float Q1 = (1.0+(2*(TD/TS)));
    const float Q2 = ((-TD/TS));
    const float Q3 = (TS/TI);

    float dutyCycle = 0.0;
    float error = 0.0;
    float y0 = 0.0;
    float y1 = 0.0;
    float y2 = 0.0;
    float u = 0.0;
    float u_oud = 0.0;

    error = SET - dist;
    u = u_oud + KC*(Q0*(y0)+Q1*(y1)+Q2*(y2)+Q3*(error));


    if (u > 1000)
    {
        output = 1000;
    }
    else if(u <0)
    {
        output = 200;
    }
    else
    {
        output = u;
    }
    u_oud = u;
    y2 = y1;
    y1 = y0;
    return output;
}

void *mainThread(void *arg0)
{
    initPort();

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

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

    while(1)
    {


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

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

    }

    return 0;
}