#include "io.h"

#define Ts 0.1

//float Kc = 1.0;
#define Ku 0.9
#define Tu 2.8

#define Kc 1.2//1//Ku/2.2 //=4.1
#define Ti 3//3//(2.2*Tu)
#define Td 0.5//0.5//(Tu/6.3)

const float p1 = (Kc*Ts)/Ti;
const float q1 = -Kc - (3*Kc*Td)/(2*Ts);
const float q2 = Kc + (7*Kc*Td)/(2*Ts);
const float q3 = -(5*Kc*Td)/(2*Ts);
const float q4 = (Kc*Td)/(2*Ts);

float y[4] = {0, 0, 0, 0};
float n = 0.0;

int controller(int dist, int r)
{
    int i;
    for (i=3; i>0; i--)
    {
        y[i] = y[i-1];
    }
    y[0] = dist;

    n = n + p1 * ( r - y[1] ) + q1 * y[0] + q2 * y[1] + q3 * y[2] + q4 * y[3];
    //n =  Kc * ( r - y[0] );

    if (n > 1000)
    {
        n = 1000;
    }
    else if (n < 0)
    {
        n = 0;
    }
    return (int)(n+0.5);
}

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);
    }

    int i = 0, ii = -5, t = 300;
    uint32_t sp = 0;
    int PWM = 0;
    char ploep[100];

    //Kc = 2;

    setPWM(0);
    while(1)
    {
        trigger(); // Trigger routine creates a 10 usec trigger pulse for the ultrasonic sensor.
        PWM = controller(distance, sp);
        if(i < 30)
        {
            setPWM(0);
        }
        else
        {
            sp = 400;
            setPWM(PWM);
        }

        sprintf(&ploep, "%d,%d,%d,%i\n", i, distance, sp, PWM);
        sendStringUART(ploep);

        i++;
        while(tick == false){}
        tick = false;
    }

    return 0;
}