#include "io.h"




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


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






#define N 3


void *mainThread(void *arg0)
{
    const float TSYS = 42.15;
    const float KSYS = 1.51;
    const float TETA = 1.75;

    const float TS =  0.1;

    const float SET =  50.0;

    const float KC = ((1.2*TSYS)/(KSYS*TETA));
    const float TI = (2.0*TETA);
    const float TD = (0.5*TETA); //0


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


    if (initUART() == false)
    {
        // initUart() failed
        while(1);
    }
    // Samplefrequency = 10 Hz
    if (initTimer(10) == false)
    {
        sendStringUART("initTimer() failed");
        while(1);
    }
    if (initADC() == false)
    {
        sendStringUART("initADC() failed");
        while(1);
    }
    if (initPWM() == false)
    {
        sendStringUART("initPWM() failed");
        while(1);
    }
    int ticks = 0; // counter for ticks

    float dutyCycle = 0.0;
    float r = 200.0;
    float y0 = 0.0;
    float y1 = 0.0;
    float y2 = 0.0;
    float u = 0.0;
    float u_oud = 0.0;

    while(1)
    {
        y0 = readADC();

        float e = (r-y1);

        //dutyCycle = dutyCycleOud + KC*(Q0*y[0]+Q1*y[1]+Q2*y[2]+Q3*(r-y[1]));

        u = u_oud + KC*(Q0*(y0)+Q1*(y1)+Q2*(y2)+Q3*(e));

        if(u > 1000)
        {
            dutyCycle = 1000;
        }
        else if(u < 0)
        {
            dutyCycle = 0;
        }
        else
        {
            dutyCycle = u;
        }
        u_oud = u;
        setPWM((int)dutyCycle);

        y2 = y1;
        y1 = y0;


        if (y0 == INT16_MIN)
        {
            sendStringUART("readADC failed!");
        }
        else
        {
            sendIntIntUART((int)dutyCycle / 10, y0);
        }

        if (tick == true)
        {
            sendStringUART("The calculation is not fast enough");
        }
        while (tick == false)
        {
            // wait for next timer tick
        }
        tick = false;
    }

    return 0;
}