/******************************************************************************/
/* PSD.C: Implementace kompaktniho PSD regulatoru                             */
/******************************************************************************/
 
#include "chip.h"
#include "board.h"
#include "common_utils.h"
#include "display.h"
#include "system_SAM3S.h"
#include "AD_DA.h"
#include "EEPROM.h"
#include <math.h>

static float PSD(float e);
void encoder_dir(int32_t *_i, int32_t min, int32_t max);
uint8_t set_numbers (uint8_t *array, uint8_t minus);
void float2array(uint8_t *array, float number, uint8_t minus);
void uint2array(uint8_t *array, uint16_t number);
uint8_t set_numbers_not_decimal(uint8_t *array);
void set_PIO_IRQ(void);
void PIO_Init(void);
void Spi_TWI_Init(void);
void TC0_Init(uint32_t freqHz);
void Change_Samplig_Freq(uint32_t freqHz);

static const Pin DI[] = {PINS_DI};
static const Pin DO[] = {PINS_DO};
static const Pin A = PIN_PA7_A;
static const Pin B = PIN_PA29_B;
static const Pin NO = PIN_PA30_NO;
static const Pin TWI_pins[] = {PIN_TWI_TWD, PIN_TWI_TWCK};

uint8_t semafor1 = 1;
volatile uint8_t NO_Button = 0;

/* PSD constants and variables */
static volatile float Isum = 0, dsum = 0;
static float K, Ti, Td, N, w, y[2];
static uint16_t Ts; /* Ts is in ms */

/* SPI - clock */
static uint32_t spiClock = 10000000;

/** TWI clock frequency in Hz */
#define TWCK            400000
 
/*-----------------------------PSD---------------------------------------------*/
static float PSD(float e)
{
    float U, Tsample;

    Tsample = (float)Ts/1000;

    if (Ti > 0 && Td > 0)
    {
        U = (K*e+Isum+N*(K*e+((dsum*exp((-Tsample*N)/Td)))-dsum));
	    Isum += ((Tsample/Ti)*e*K);
	    dsum = K*e+(dsum*exp((-Tsample*N)/Td));
    }

    if(U > 10)
		U = 10;
	if(U < -10)
		U = -10;

    if(Isum > 10)
		Isum = 10;
	if(Isum < -10)
		Isum = -10;

    return U;
}
/*-----------------------------------------------------------------------------------------------*/
/*---------------- IRQ HANDERS ------------------------------------------------*/
/*static void BUSY_ADC_Handler(const Pin *pin)
{
    ADC_is_BUSY = 0;    
} */

int count = 0;
int count_prev = 0; 
static void A_Handler(const Pin * pin)
{
    if(PIO_Get(&B))
        count--;
    else
        count++;
}

static void NO_Handler(const Pin * pin)
{
    NO_Button = 1;
}

void TC0_IrqHandler(void)
{
    volatile uint8_t dummy;
    /* Clear status bit to acknowledge interrupt */
    dummy = TC0->TC_CHANNEL[0].TC_SR;
    if (PIO_Get(&DO[0]))
        PIO_Clear(&DO[0]);
    else
        PIO_Set(&DO[0]);

    ADC_ReadVoltage(y);
    DAC_Set_Voltage(DAC_A, PSD(w-y[0]));
}

/*-----------------------------------------------------------------------------*/

int main (void) 
{                          /* Main Program                    */ 
    uint8_t Enter_Menu = 0, Menu_Level = 0; 
    
    char menu1[5][16] = { "START/STOP", "Velikost zadana", "Parametry PSD", "Vzork. perioda", "Zpet" };
    char menuPSD[5][16] = { "Nastaveni K", "Nastaveni Ti[s]", "Nastaveni Td[s]", "Konst. N", "Zpet" }; 
    int i = 0, j = 0;
    int16_t aux_number;

    uint8_t OK = 0;
    uint8_t menu_num[5]; 
    uint8_t running = 0;
    uint8_t _switch = 0;  

    SystemInit();
    SysTick_Config(SystemFrequency/1000);    /* Generate interrupt each 1 ms    */

    PIO_Init();
    set_PIO_IRQ();

    lcd_init();
    Spi_TWI_Init();                 

    DAC_Init();
    DAC_Set_Voltage(DAC_A, 0.0);
    DAC_Set_Voltage(DAC_B, 0.0);

    PSD_Param_Init(&K, &Ti, &Td, &N, &Ts, &w);
               
    TC0_Init(1/((float)Ts/1000));  /*1/((float)Ts/1000) because TC0_Init need Ts in seconds */

    lcd_string("Zadana:");
    lcd_setline(1);
    lcd_string("Aktualni:");  
    
    lcd_instr(0x0C); /* turn off blinking */ 
                                              
    for (;;) 
    {                               /* Loop forever                    */       
        /*--------------- Dgital Input/Output function demonstration -----------------------*/
        /* When one of DI is set correspond DO will be set */ 
       /* if (PIO_Get(&DI[0]))
            PIO_Set(&DO[0]);
        else if (PIO_Get(&DO[0] == 1)
            PIO_Clear(&DO[0]); */

    /*---------------------------------------------------------------------------------*/
        
        if (one_sec())  /* aprox. every each 1s refresh values on display */
        {            
            if (PIO_Get(&DO[1]))   /*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
                PIO_Clear(&DO[1]);     /*pryc s tim pak */
            else
                PIO_Set(&DO[1]);
            
            if (running)
            {
                lcd_gotoxy(15,0);
                if (_switch)
                {
                    lcd_char('*');
                    _switch = 0;
                }
                else
                {
                    lcd_char(' ');
                    _switch = 1;
                }
            }

            lcd_gotoxy(7,0);       /* wanted */
            float2array(menu_num, w, 1);
            menu_num[0] == 1 ? lcd_char(43) : lcd_char(45);   /* +/- according to ASCII */             
            for (j=1; j<sizeof(menu_num);j++)
            {
                 if (j == 3)
                     lcd_char(',');
                 lcd_char(menu_num[j]+48);
            }
            
            lcd_gotoxy(9,1);    /* actual */
            float2array(menu_num, y[0], 1);
            menu_num[0] == 1 ? lcd_char(43) : lcd_char(45);   /* +/- according to ASCII */             
            for (j=1; j<sizeof(menu_num);j++)
            {
                 if (j == 3)
                     lcd_char(',');
                 lcd_char(menu_num[j]+48);
            }
        }

        if (NO_Button) /* if NO Button is pressed do something */
        {
            NO_Button = 0;
            Enter_Menu = 1;
            lcd_instr(0x0D); /* turn on blinking */
        }

        while(Enter_Menu)
        {
            count_prev = count;
            switch (Menu_Level)
            {
                case 0: NO_Button = 0;  /* menu1 Basic menu */
                        lcd_clrscr();
                        lcd_string("Nastaveni:");
                        lcd_setline(1);
                        i = 0;
                        lcd_string(menu1[i]);
                        while(NO_Button == 0)
                        {
                            if (count != count_prev)
                            {
                                encoder_dir(&i, 0, sizeof(menu1)/16 - 1);
                                lcd_clearline(1);
                                lcd_string(menu1[i]);
                            }
                             
                        }
                        Menu_Level = i+1;
                        break;

               case 1: NO_Button = 0;  /* START/STOP */
                       Isum = 0; 
                       dsum = 0;
                       DAC_Set_Voltage(DAC_A, 0.0);
                       if (!running)
                       {
                           TC_Start(TC0, 0); /*start timer and controller */
                           running = 1;
                       }
                       else
                       {
                           TC_Stop(TC0, 0); /* stop timer and controller */
                           running = 0;
                       }
                       Menu_Level = 5; /* after start EXIT */
                       break;
                       
               case 2: NO_Button = 0; /* menu2 set wanted value */
                       lcd_clrscr();  
                       lcd_string("Nastaveni w:");
                       lcd_setline(1);
                       float2array(menu_num, w, 1);
                       menu_num[0] == 1 ? lcd_char(43) : lcd_char(45);   /* +/- according to ASCII */             
                       for (j=1; j<sizeof(menu_num);j++)
                       {
                            if (j == 3)
                                lcd_char(',');
                            lcd_char(menu_num[j]+48);
                       }
                       lcd_string(" V      OK");
                       lcd_gotoxy(0,1);    
                       OK = set_numbers(menu_num, 1);
                       if (OK)
                       {
                           aux_number = menu_num[1]*1000 + menu_num[2]*100 + menu_num[3]*10 + menu_num[4];
                           aux_number = (menu_num[0] == 1) ? aux_number : aux_number * -1;
                           if ((aux_number > 1000) || (aux_number < -1000))
                           {
                               lcd_clrscr();
                               lcd_string("!! CHYBA !!");
                               lcd_setline(1);
                               lcd_string("Musi byt<-10;10>");
                               while(NO_Button == 0);
                               Menu_Level = 1;
                           }
                           else
                           {
                               w = (float)aux_number/100;
                               EEPROM_Write_Param(w_addr, w);
                               /* asi pak pridat kontrolu, ze se to ulozilo spravne, tzn. vycist z EEPROM a porovnat s RAM */
                               Menu_Level = 0;
                           }
                       }
                       break;

               case 3: NO_Button = 0; /* menu2 set PSD parametres */
                       lcd_clrscr();  
                       lcd_string("Nastaveni reg.:");
                       lcd_setline(1);
                       i = 0;
                       lcd_string(menuPSD[i]);
                       while(NO_Button == 0)
                       {
                           if (count != count_prev)
                           {
                               encoder_dir(&i, 0, sizeof(menuPSD)/16 - 1);
                               lcd_clearline(1);
                               lcd_string(menuPSD[i]);
                           }
                            
                       }
                       if (i == 4)   /* back */ 
                            Menu_Level = 0;
                       else
                            Menu_Level = i+6;
                       break;

               case 4: NO_Button = 0; /* menu2 set sampling time */
                       lcd_clrscr();  
                       lcd_string("Nastaveni Ts[ms]");
                       lcd_setline(1);
                       uint2array(menu_num, Ts);
                       for (j=0; j<sizeof(menu_num)-1;j++)
                       {
                            lcd_char(menu_num[j]+48);
                       }
                       lcd_string(" ms       OK");
                       lcd_gotoxy(0,1);
                       OK = set_numbers_not_decimal(menu_num);
                       if (OK)
                       {
                           Ts = menu_num[0]*1000 + menu_num[1]*100 + menu_num[2]*10 + menu_num[3];
                           EEPROM_Write_Param(Ts_addr, Ts);
                           Change_Samplig_Freq(1/((float)Ts/1000));
                           /* asi pak pridat kontrolu, ze se to ulozilo spravne, tzn. vycist z EEPROM a porovnat s RAM */
                           Menu_Level = 0;                         
                       }
                       break;
               /*------------ EXIT -------------------*/
               case 5: NO_Button = 0; /* menu1 exit */
                       Enter_Menu = 0;
                       Menu_Level = 0;
                       lcd_clrscr();
                       lcd_string("Zadana:");
                       lcd_setline(1);
                       lcd_string("Aktualni:");
                       lcd_instr(0x0C); /* turn off blinking */
                       break;
               /*-------------------------------------*/

               case 6: NO_Button = 0; /* menu3 PSD parametres - K */
                       lcd_clrscr();
                       lcd_string("Proporc. slozka:");
                       lcd_setline(1);
                       float2array(menu_num, K, 0);
                       for (j=0; j<sizeof(menu_num)-1;j++)
                       {
                            if (j == 2)
                                lcd_char(',');
                            lcd_char(menu_num[j]+48);
                       }
                       lcd_string("         OK");
                       lcd_gotoxy(0,1);   
                       OK = set_numbers(menu_num, 0);
                       if (OK)
                       {
                           aux_number = menu_num[0]*1000 + menu_num[1]*100 + menu_num[2]*10 + menu_num[3];
                           K = (float)aux_number/100;
                           EEPROM_Write_Param(K_addr, K);
                           /* asi pak pridat kontrolu, ze se to ulozilo spravne, tzn. vycist z EEPROM a porovnat s RAM */
                           Menu_Level = 3;                         
                       }
                       break;
               case 7: NO_Button = 0; /* menu3 PSD parametres - Ti */
                       lcd_clrscr();
                       lcd_string("Cas. konst. Ti:");
                       lcd_setline(1);
                       float2array(menu_num, Ti, 0);
                       for (j=0; j<sizeof(menu_num)-1;j++)
                       {
                            if (j == 2)
                                lcd_char(',');
                            lcd_char(menu_num[j]+48);
                       }
                       lcd_string(" s       OK");
                       lcd_gotoxy(0,1);   
                       OK = set_numbers(menu_num, 0);
                       if (OK)
                       {
                           aux_number = menu_num[0]*1000 + menu_num[1]*100 + menu_num[2]*10 + menu_num[3];
                           Ti = (float)aux_number/100;
                           EEPROM_Write_Param(Ti_addr, Ti);
                           /* asi pak pridat kontrolu, ze se to ulozilo spravne, tzn. vycist z EEPROM a porovnat s RAM */
                           Menu_Level = 3;                         
                       }
                       break;
               case 8: NO_Button = 0; /* menu3 PSD parametres - Td */
                       lcd_clrscr();
                       lcd_string("Cas. konst. Td");
                       lcd_setline(1);
                       float2array(menu_num, Td, 0);
                       for (j=0; j<sizeof(menu_num)-1;j++)
                       {
                            if (j == 2)
                                lcd_char(',');
                            lcd_char(menu_num[j]+48);
                       }
                       lcd_string(" s       OK");
                       lcd_gotoxy(0,1);    
                       OK = set_numbers(menu_num, 0);
                       if (OK)
                       {
                           aux_number = menu_num[0]*1000 + menu_num[1]*100 + menu_num[2]*10 + menu_num[3];
                           Td = (float)aux_number/100;
                           EEPROM_Write_Param(Td_addr, Td);
                           /* asi pak pridat kontrolu, ze se to ulozilo spravne, tzn. vycist z EEPROM a porovnat s RAM */
                           Menu_Level = 3;                         
                       }
                       break;
               case 9: NO_Button = 0; /* menu3 PSD parametres - N */
                       lcd_clrscr();
                       lcd_string("Konst. N");
                       lcd_setline(1);
                       float2array(menu_num, N, 0);
                       for (j=0; j<sizeof(menu_num)-1;j++)
                       {
                            if (j == 2)
                                lcd_char(',');
                            lcd_char(menu_num[j]+48);
                       }
                       lcd_string("         OK");
                       lcd_gotoxy(0,1);    
                       OK = set_numbers(menu_num, 0);
                       if (OK)
                       {
                           aux_number = menu_num[0]*1000 + menu_num[1]*100 + menu_num[2]*10 + menu_num[3];
                           N = (float)aux_number/100;
                           EEPROM_Write_Param(N_addr, N);
                           /* asi pak pridat kontrolu, ze se to ulozilo spravne, tzn. vycist z EEPROM a porovnat s RAM */
                           Menu_Level = 3;                         
                       }
                       break;                                                                       
               default: break;                
            }
        }
  }
}


void set_PIO_IRQ(void)
{
    PIO_InitializeInterrupts(2); /* Init IRQ and enabled PIO controller clock */
    
//    PIO_ConfigureIt(&BUSY_ADC, BUSY_ADC_Handler);
    PIO_ConfigureIt(&A, A_Handler);
    PIO_ConfigureIt(&NO, NO_Handler);
    
//    NVIC_EnableIRQ((IRQn_Type)BUSY_ADC.id);
    NVIC_EnableIRQ((IRQn_Type)A.id);
    NVIC_EnableIRQ((IRQn_Type)NO.id);

//    PIO_EnableIt(&BUSY_ADC);
    PIO_EnableIt(&A);
    PIO_EnableIt(&NO);

    PIO_SetDebounceFilter(&A, 100); /* Adjust pio debounce filter patameters, uses 100 Hz filter. */
    PIO_SetDebounceFilter(&NO, 100); /* It must be here because PIO controller clock has to be enabled before setting this */
}

void PIO_Init(void)
{    
    PIO_Configure(&A, PIO_LISTSIZE(A));
    PIO_Configure(&B, PIO_LISTSIZE(B));
    PIO_Configure(&NO, PIO_LISTSIZE(NO));
    
    PIO_Configure(DI, PIO_LISTSIZE(DI));
    PIO_Configure(DO, PIO_LISTSIZE(DO));

    PIO_Configure(TWI_pins, PIO_LISTSIZE(TWI_pins));
}

/*-----------------------------------------------------------------------------------------------*/
void Spi_TWI_Init(void)
{
    SPI_Configure(SPI, ID_SPI, SPI_MR_MSTR | SPI_MR_PS | SPI_MR_MODFDIS | SPI_MR_WDRBT);/*Master mode, volitelny periph. select, Mode fault detect. vyp,   */    
    SPI_ConfigureNPCS(SPI, 1, SPI_DLYBS(30, SystemFrequency) | SPI_SCBR( spiClock, SystemFrequency) | SPI_CSR_CSAAT ) ;
    SPI_ConfigureNPCS(SPI, 0, SPI_DLYBS(0, SystemFrequency) | SPI_SCBR( spiClock, SystemFrequency) | SPI_CSR_CSAAT | SPI_CSR_BITS_14_BIT | SPI_CSR_NCPHA) ;  /*DLYBS = Delay Before SPCK */

    SPI_Enable(SPI);    /* Enables a SPI peripheral. */
        
    PMC->PMC_PCER0 = 1 << ID_TWI0;  /* Enable TWI peripheral clock */
    /* Configure TWI */
    TWI_ConfigureMaster(TWI0, TWCK, SystemFrequency);
    Twi_inst_init();
}

/*----------------------------------- TIMER 0 for sampling --------------------------------------*/

void TC0_Init(uint32_t freqHz) /* input freq is in Hz */
{
/* Timer Counter Clock Assignment*/
/*  TIMER_CLOCK1 - MCK/2
    TIMER_CLOCK2 - MCK/8
    TIMER_CLOCK3 - MCK/32
    TIMER_CLOCK4 - MCK/128
    TIMER_CLOCK5(1) - SLCK  
(1) Please see doc.    */
    uint32_t div;
    uint32_t tcclks;

    /** Enable peripheral clock. */
    PMC->PMC_PCER0 = 1 << ID_TC0;
    
    /** Configure TC for a ?kHz frequency and trigger on RC compare. */
    TC_FindMckDivisor(freqHz, SystemFrequency, &div, &tcclks, SystemFrequency);

    TC_Configure(TC0, 0, tcclks | TC_CMR_CPCTRG);
    TC0->TC_CHANNEL[0].TC_RC = (SystemFrequency / div) / freqHz;   /* on RC makes interrupt */
    /* Configure and enable interrupt on RC compare */
    NVIC_SetPriority((IRQn_Type)ID_TC0, 0); /* set the highest priority */
    NVIC_EnableIRQ((IRQn_Type)ID_TC0);
    TC0->TC_CHANNEL[0].TC_IER = TC_IER_CPCS;
    
    //TC_Start(TC0, 0);   	    
}

void Change_Samplig_Freq(uint32_t freqHz) /* input freq is in Hz */
{
    uint32_t div;
    uint32_t tcclks;

    TC_Stop(TC0, 0);
    TC_FindMckDivisor(freqHz, SystemFrequency, &div, &tcclks, SystemFrequency);
    TC_Configure(TC0, 0, tcclks | TC_CMR_CPCTRG);
    TC0->TC_CHANNEL[0].TC_RC = (SystemFrequency / div) / freqHz;
    TC0->TC_CHANNEL[0].TC_IER = TC_IER_CPCS;
    TC_Start(TC0, 0);
}

/*------------------------------Functions used in menu--------------------------------------- */
/* designates encoder's direction when is rotating*/
void encoder_dir(int32_t *_i, int32_t min, int32_t max)
{
    int32_t i = *_i;
    if (count > count_prev)
    {
        count_prev = count;
        i++;
        if (i > max)
            i = min;                            
    }
    else if (count < count_prev)
    {
        count_prev = count;
        i--;
        if (i < min)
            i = max;
    }
    *_i = i;
}

/* set numbers by encoder direction with decimal point*/
uint8_t set_numbers(uint8_t *array, uint8_t minus)
{                         
    uint8_t _OK = 0;
    int32_t i = 0, aux = 0;
    uint8_t *num = array;
    while(!_OK)
    {
        if (count != count_prev)
        {
            encoder_dir(&i, 0, 15);
            lcd_gotoxy(i,1); /* NOTE: "i" means position on display */                            
        }
    
        if (NO_Button)
        {
            NO_Button = 0;
            if ((i < (5+minus)) && (i != (2+minus))) /* set numbers */
            {
                lcd_instr(0x0E); /* turn off blinking */
                while(NO_Button == 0)
                {                                  
                    if (count != count_prev)
                    {
                        if ((i == 0) && minus)
                        {
                            aux = num[i];
                            encoder_dir(&aux, 1, 2); /* 2 => -; 1 => + */
                            aux == 1 ? lcd_char(aux+42) : lcd_char(aux+43);
                            lcd_gotoxy(i,1);
                            num[i] = aux;
                        }
                        else
                        {
                            if (i < (2+minus))
                                aux = num[i]; 
                            else
                                aux = num[i-1];

                            encoder_dir(&aux, 0, 9);
                            lcd_char(aux+48);
                            lcd_gotoxy(i,1);
                            if (i < (2+minus))
                                num[i] = aux; 
                            else
                                num[i-1] = aux;
                        }            
                    }
                }       /* end of set numbers */
                NO_Button = 0;
                lcd_instr(0x0D); /* turn on blinking */
            }
            else if (i > 13) /* click on OK => exit loop*/
            {
                NO_Button = 0;
                _OK = 1;
            }
        }                                   
    }    
    return _OK;
}    

/*transform float to array */
void float2array(uint8_t *array, float number, uint8_t minus)
{
    int i = 0;
    uint8_t *num = array;
    uint16_t aux, exp = 1000;
    
    if (minus)
    {
        if (number < 0)
        { /* if num<0 -> array[0] = 2 => sign of negative number */
            array[0] = 2;
            number *= -1;
        }
        else
            array[0] = 1;
    }
    aux = number*100;

    for (i = minus; i < sizeof(array)+minus; i++)   /*better: for (i = minus; i < sizeof(num); i++) but not working correctly */
    {
        num[i] = (aux/exp)%10;
        exp /= 10;
    }  
}

/*transform uint to array */
void uint2array(uint8_t *array, uint16_t number)
{
    int i = 0;
    uint8_t *num = array;
    uint16_t exp = 1000;

    for (i = 0; i < sizeof(array); i++)
    {
        num[i] = (number/exp)%10;
        exp /= 10;
    }  
}

/* set numbers by encoder direction with no decimal point*/
uint8_t set_numbers_not_decimal(uint8_t *array)
{                         
    uint8_t _OK = 0;
    int32_t i = 0, aux = 0;
    uint8_t *num = array;
    while(!_OK)
    {
        if (count != count_prev)
        {
            encoder_dir(&i, 0, 15);
            lcd_gotoxy(i,1); /* NOTE: "i" means position on display */                            
        }
    
        if (NO_Button)
        {
            NO_Button = 0;
            if (i < 5) /* set numbers */
            {
                lcd_instr(0x0E); /* turn off blinking */
                while(NO_Button == 0)
                {                                  
                    if (count != count_prev)
                    {
                        aux = num[i];
                        encoder_dir(&aux, 0, 9);
                        lcd_char(aux+48);
                        lcd_gotoxy(i,1);
                        num[i] = aux;            
                    }
                }       /* end of set numbers */
                NO_Button = 0;
                lcd_instr(0x0D); /* turn on blinking */
            }
            else if (i > 13) /* click on OK => exit loop*/
            {
                NO_Button = 0;
                _OK = 1;
            }
        }                                   
    }    
    return _OK;
}    
