//
// Created by martin.vitek on 10.7.16.
//

#include "Controller.h"

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

#include "servo.h"

Controller controller;

static servo_data blue_servo_data
{
        220,	//0°
        2200,	//180°

        5000,	//ICR_value
        17366	//Pulse_duration
};

static usart_regs uart_regs
{
    &UCSRA,
    &UCSRB,
    &UCSRC,
    &UDR,
    &UBRRL,
    &UBRRH
};

Controller::Controller(): servo(&DDRB, PB1, &OCR1A, blue_servo_data),
                          adc(),
                          uart(uart_regs)
{
    //Servo ini
    TCCR1A |= (1<<COM1A1) | (1<<COM1B1) | (1<<WGM11);	//Fast PWM non-inverted
    TCCR1B |= (1<<WGM13)  | (1<<WGM12)  | (1<<CS11) | (1<<CS10);	//Prescaler 64

    ICR1 = 4000;	//Top value

    //UART ini
    uart.set_operating_mode(uart.ASYNCHRONOUS_NORMAL);
    uart.set_baudrate(115200);
    uart.set_data_bits(8);
    uart.set_stop_bits(1);
    uart.enable_reciver();
    uart.enable_trasmitter();

    //ADC ini
    DDRC = 0;
    PORTC = 0;
    ADMUX  = (1<<REFS1) | (1<<REFS0);
    ADCSRA = (1<<ADEN) | (1<<ADIE) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);
    //adc.set_reference_voltage(adc.INTERNAL_REFERENCE);  //2.56V
    //adc.set_prescaler(adc.PRESCALER_128);               //115kHz
    //adc.enable_interrupt();
    //adc.set_channel(adc.ADC0);
    //adc.enable_adc();

    //TCC0 ini
    TCCR0 = (1<<CS02);  //225Hz
    TIMSK |= (1<<TOIE0);

    sei();
}

void Controller::print_debug()
{
    uart.transmit_string("P: ");
    uart.transmit_dec32(current_positive);
    uart.transmit_string("\tN: ");
    uart.transmit_dec32(current_negative);
    uart.transmit_new_line();
}