#include "Controller.h"

#include <stdint.h>

#include "usart.h"
#include "servo.h"


Controller controller;

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

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

        10000,	//ICR_value
        20000	//Pulse_duration
};

Controller::Controller():   potenciometr(),
                            servoL(&DDRB, PB1, &OCR1A, blue_servo_data),
                            servoR(&DDRB, PB2, &OCR1B, blue_servo_data),
                            uart(registers)
{
    uart.set_operating_mode(uart.ASYNCHRONOUS_NORMAL);
    uart.set_baudrate(115200);
    uart.set_data_bits(8);
    uart.set_stop_bits(1);
    uart.set_parity(uart.PARITY_NONE);
    uart.enable_trasmitter();
    //uart.enable_RX_complete_interrupt();

    uart.transmit_string("\nServo headlight control\n");

    Ldata = {163, 820, 85, 113};
    Rdata = {163, 820, 85, 113};

    L_k = (Ldata.pot_max-Ldata.pot_min)/(Ldata.angle_max-Ldata.angle_min);
    R_k = (Rdata.pot_max-Rdata.pot_min)/(Rdata.angle_max-Rdata.angle_min);

    servo_ini();
}

void Controller::servo_ini()
{
    TCCR1A |= (1<<COM1A1) | (1<<COM1B1) | (1<<WGM11);	//Fast PWM non-inverted
    TCCR1B |= (1<<WGM13)  | (1<<WGM12)  | (1<<CS11);	//Prescaler 8 -> 7.03Hz

    ICR1 = 10000;	//Top value -> 46.04Hz

    servoL.set_angle(113);
    servoR.set_angle(113);

}

void Controller::L_handler()
{
    uint8_t angle = ((potenciometr.adc_value+1963)/L_k)-30;
    servoL.set_angle(angle);
}

void Controller::R_handler()
{
    uint8_t angle = ((potenciometr.adc_value+1963)/R_k)-7;
    servoR.set_angle(angle);
}

void Controller::send_debug()
{
    uart.transmit_string("Pot:\t");
    uart.transmit_dec32(potenciometr.adc_value);
    uart.transmit_string("\t\t");
    uart.transmit_string("L:\t");
    uart.transmit_dec32(servoL.get_angle());
    uart.transmit_string("\t\t");
    uart.transmit_string("R:\t");
    uart.transmit_dec32(servoR.get_angle());
    uart.transmit_new_line();
}