/*
 * ATmega32L_ADC_test.c
 *
 * Created: 03.12.2011 18:53:45
 *  Author: Sabesto
 */

#define F_CPU 8000000UL
#define USART_BAUDRATE 9600
#define BAUD_PRESCALE (((F_CPU / (USART_BAUDRATE * 16UL))) - 1)
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

volatile uint8_t newSampleReady = 0;
volatile int8_t channel = 6;
volatile uint8_t ad[8];
volatile uint8_t at[8] = {0,20,20,20,100,200,120,255};
uint32_t teller = 0;
uint32_t nevner = 0;
uint8_t posisjon = 0;
uint16_t time = 0;
uint16_t lastTime = 0;
int16_t lastErr = 0;
volatile uint64_t millis = 0;
int8_t Setpoint = 50;
int8_t kp, ki, kd;
int16_t output = 0;
int32_t errSum = 0;
int32_t error = 0;

void timer1init(void){
    TCCR1B |= (1<<CS10);
    //TCCR1B |= (1<<CS11);  // prescaler 1024
    TIMSK |= (1<<TOIE1);    // timer1 overflow interrupt enabled
}
void uartinit (void)
{
    UCSRB |= (1 << RXEN) | (1 << TXEN);
    UCSRC |= (1 << UCSZ0) | (1 << UCSZ1);
    UBRRH = (BAUD_PRESCALE >> 8);
    UBRRL = BAUD_PRESCALE;
}
void USART1_Transmit(unsigned char data)
{
   while (!(UCSRA & (1<<UDRE)));
   UDR = data;
}
void USART1_Transmituint16(uint16_t data)
{
    uint8_t low = data;
    uint8_t high = (data>>8);

   while (!(UCSRA & (1<<UDRE)));
   UDR = high;
   while (!(UCSRA & (1<<UDRE)));
   UDR = low;
}
void USART1_TransmitString (const char *str) {

    while (*str)
    {
        USART1_Transmit(*str);
        str++;
    }
}
void SetTunings(uint32_t Kp, uint32_t Ki, uint32_t Kd)
{
   kp = Kp;
   ki = Ki;
   kd = Kd;
}
void adcinit (void){
    ADCSRA |= (1 << ADPS2) | (1 << ADPS1); // | (1 << ADPS0); // Set ADC prescalar to 64(128) - 125KHz sample rate @ 16MHz
    ADMUX = 0b01100000;
    //ADMUX |= (1 << REFS0); // Set ADC reference to AVCC
    //ADMUX |= (1 << ADLAR); // Left adjust ADC result to allow easy 8 bit reading
    ADCSRA |= (1 << ADATE);  // Set ADC to Free-Running Mode
    ADCSRA |= (1 << ADEN);  // Enable ADC
    ADCSRA |= (1 << ADIE);  // enable adc interrupt
    ADCSRA |= (1 << ADSC);  // Start A2D Conversions

}
int main (void)
{
    SetTunings(2,0,100);
    timer1init();
    DDRC = 0xff;
    DDRD = 0b10000000;
    uartinit();
    adcinit();
    sei();
    while(1)
    {
        if (newSampleReady == 1)
        {
            PORTD |= (1<<PD7);
            teller=0;
            nevner=0;
            posisjon=0;
            for (uint16_t x=1;x<8;x++)
            {
                if(ad[x]<30){ad[x]=0;}
                else {ad[x] -= 30;}
                teller += ((x-1)<<4)*(ad[x]);
            }
            for (uint8_t x=1;x<8;x++)
            {
                nevner += ad[x];
            }
            posisjon = (teller/nevner);
            //PID--------------------------
            uint32_t now = millis;
            uint32_t timeChange = (uint32_t)(now - lastTime);
            error = Setpoint - posisjon;
            errSum += (error * timeChange);
            uint32_t dErr = (error - lastErr) / timeChange;
            output = kp * error + ki * errSum + kd * dErr;
            lastErr = error;
            lastTime = now;
            //PID------------------------------
            /*
            if(posisjon < 14){PORTC = 0b00000001;}
            if(posisjon > 14 && posisjon < 28){PORTC =  0b00000010;}
            if(posisjon > 28 && posisjon < 42){PORTC =  0b00000100;}
            if(posisjon > 42 && posisjon < 56){PORTC =  0b00001000;}
            if(posisjon > 56 && posisjon < 70){PORTC =  0b00010000;}
            if(posisjon > 70 && posisjon < 82){PORTC =  0b00100000;}
            if(posisjon > 82 && posisjon < 100){PORTC = 0b01000000;}
            */
            USART1_Transmituint16(output>>3);

            newSampleReady = 0;
            ADCSRA |= (1<<ADIE);
            PORTD &= ~(1<<PD7);
        }

    }
}

ISR(ADC_vect)
{
    ad[channel] = ADCH; // Store the result (8-bits, discard the 3 LSB)
    // ADCSRA &= ~(1 << ADEN); // Disable the ADC
    channel ++;


    if (channel == 8)
    {
        ADCSRA &= ~(1<<ADIE);
        // disable interrupts for now
        newSampleReady = 1; // 7 channels sampled
        channel = 0; // Reset to channel 0
    }
    ADMUX &= 0xE0;//Update the ADMUX register
    ADMUX |= channel;
}
SIGNAL(TIMER1_OVF_vect)
{
   TCNT1 = 0xF0FF;
   millis++;
}