/*
 * LCD_HD44780.c
 *
 * Created: 20.01.2018 12:51:11
 *  Author: Ulrich
 */ 

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "lcd.h"
#include "i2c.h"
#include "modbus.h"
#include "menu.h"
#include "adc.h"
#include "pid.h"

#define COIL1 1<<1
#define COIL2 1<<0
#define COIL3 1<<2
#define COIL4 1<<3

volatile uint16_t holdingRegisters[10];

volatile uint16_t temp_values[4];
volatile uint16_t temp_setpoints[4];

volatile uint16_t output;
volatile struct pid controller;

volatile uint32_t duty[3];

void modbusGet(void) {
	if (modbusGetBusState() & (1<<ReceiveCompleted))
	{
        switch(rxbuffer[1]) {
            case fcReadHoldingRegisters:
                holdingRegisters[0] = duty[0];
                holdingRegisters[1] = duty[1];
                holdingRegisters[2] = duty[2];

                holdingRegisters[3] = temp_values[0];
                holdingRegisters[4] = temp_values[1];
                holdingRegisters[5] = temp_values[2];

                modbusExchangeRegisters(holdingRegisters,0,10);
                break;
            case fcPresetSingleRegister:
            case fcPresetMultipleRegisters:
                modbusExchangeRegisters(holdingRegisters,0,4);
                menu_state = holdingRegisters[0];
                break;
            default:
                modbusSendException(ecIllegalFunction);
                break;
		}
	}
}

void read_buttons(){
    i2c_start(0x71);
    uint8_t buttons = i2c_readNak();
    i2c_stop();
    //TODO error handling

    if(buttons & (1 << 0))
        set_item(1);
    else if(buttons & (1 << 1))
        set_item(2);
    else if(buttons & (1 << 2))
        set_item(3);
    else if(buttons & (1 << 3))
        set_item(4);
    else if(buttons & (1 << 4))
        set_item(5);
    else if((buttons & (1 << 5)) == 0) // inverted logic on this one
        set_item(0);

}

int main(void)
{
    DDRB |= 0x0F;       // output coils
    DDRD |= (1 << 4);   // board LED

	i2c_init();
	lcd_init();
    adc_init();
    enc_init();

    init_pid(&controller, 1, 0.05, 0);

    modbusSetAddress(1);
    modbusInit();

    //modbus timer
    TCCR0B|=(1<<CS01);
	TIMSK0|=(1<<TOIE0);

    // timer for regulator
    TCCR1B|=(1<<CS12) | (1<<CS10);
	TIMSK1|=(1<<TOIE1)|(1<<OCIE1A);

    sei();

	lcd_clear();
    draw_menu();


    uint16_t x = 0;
	while(1){
        modbusGet();
        read_buttons();

        //blink cursor
        if(x==0){
        //}
        ////write values
        //if(x % 2048 == 0){
            lcd_cursor(0);
            write_temps();
            write_setpoints();
            write_motor();
            update_cursor();
        }
        x++;
        if(x >= 1024)
            x=0;
	}
}

ISR(PCINT2_vect){
    encoder_isr();
}

ISR(TIMER0_OVF_vect) { //this ISR is called 9765.625 times per second
	modbusTickTimer();
}

ISR(TIMER1_COMPA_vect) {
    /* turn off outputs */

    if(OCR1A >= duty[0]){
        PORTB &= ~(COIL1);
        PORTD &= ~(1 << 4);
    }
    if(OCR1A >= duty[1])
        PORTB &= ~(COIL2);
    if(OCR1A >= duty[2])
        PORTB &= ~(COIL3);

    uint16_t new_ocr = 0xFFFE;
    for(uint8_t i = 0; i < 3; i++){
        if((duty[i] > OCR1A) && (duty[i] < new_ocr))
            new_ocr = duty[i];
    }
    OCR1A = new_ocr;

    //uint16_t tmp1 = duty[0];
    //uint16_t tmp2;
    //for(uint8_t i = 0; i < 3; i++){
    //    if(duty[i] < tmp1 && duty[i] > OCR1A)
    //        tmp2 = duty[i];
    //}
    //OCR1A = tmp2;
}

ISR(TIMER1_OVF_vect) {
    int16_t tmp_pid = pid_step(&controller, 1, temp_setpoints[0] - temp_values[0]);
    
    if(tmp_pid>=0)
        output = tmp_pid;
    else
        output = 0;


    if(output >= 128){
        output = 128;
        duty[0]=0xFFFE;
    }
    else if(output >= 1){
        uint32_t tmp = output * 512;
        duty[0] = tmp;
    }
    else
        duty[0]=0;


    duty[1] = duty[0] / 100 * temp_setpoints[1];
    duty[2] = duty[0] / 100 * temp_setpoints[2];

    for(uint8_t i = 0; i < 3; i++){
        if(duty[i] >= 0xFFFE)
            duty[i] = 0xFFFE;
    }

    OCR1A = 0xFFFE;
    for(uint8_t i = 0; i < 3; i++){
        if(duty[i] < OCR1A && duty[i] >= (uint16_t)1000)
            OCR1A = duty[i];
    }

    if(duty[0] >= 1000){
        PORTB |= COIL1;
        PORTD |= 1<<4;
    }
    if(duty[1] >= 1000)
        PORTB |= COIL2;

    if(duty[2] >= 1000)
        PORTB |= COIL3;


}