#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(9600);
    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);

    //Ext IRQ ini
    MCUCR |= (1<<ISC11);    //Falling edge
    GICR  |= (1<<INT1);

    //TCC2 ini
    TIMSK |= (1<<TOIE2);

    sei();

    servo.set_angle(CLOSE_POSITION);
}

void Controller::gate_handler()
{
    switch (gate_mode)
    {
        case WAITING_FOR_LOCK_SIGNAL:           waiting_for_lock_signal();
            break;

        case OPENING_PISTON:                    opening_piston();
            break;

        case WAITING_FOR_CLOSING_CURRENT:       waiting_for_closing_current();
            break;

        case WAITING_FOR_CURRENT_REVERSATION:   waiting_for_current_reversation();
            break;

        case WAITING_FOR_CURRENT_DROP:          waiting_for_current_drop();
            break;

        case WAITING_FOR_STABILIZATION:         waiting_for_stabilization();
            break;

        case CLOSING_PISTON:                    closing_piston();
            break;

        default:
            break;
    }
}

void Controller::waiting_for_lock_signal()
{
    if (lock_signal)
    {
        gate_mode = OPENING_PISTON;
    }
}

void Controller::opening_piston()
{
    servo.set_angle(OPEN_POSITION);
    gate_mode = WAITING_FOR_CLOSING_CURRENT;
}

void Controller::waiting_for_closing_current()
{
    if ((current_negative > ON_TRESHOLD) && (current_negative < 1020))
    {
        gate_mode = WAITING_FOR_CURRENT_DROP;
    }
}

void Controller::waiting_for_current_reversation()
{
    if (current_positive > ON_TRESHOLD)
    {
        gate_mode = WAITING_FOR_CURRENT_DROP;
    }
}

void Controller::waiting_for_current_drop()
{
    if ((current_positive < OFF_TRESHOLD) && (current_negative < OFF_TRESHOLD))
    {
        gate_mode = WAITING_FOR_STABILIZATION;
    }
}

void Controller::waiting_for_stabilization()
{
    if (stabilization_delay >= STABILIZATION_TRESHOLD)
    {
        stabilization_delay = 0;
        gate_mode = CLOSING_PISTON;
    }
}

void Controller::closing_piston()
{
    servo.set_angle(CLOSE_POSITION);
    gate_mode = WAITING_FOR_LOCK_SIGNAL;
}

void Controller::print_debug()
{
    uart.transmit_string("P: ");
    uart.transmit_dec32(current_positive);
    uart.transmit_string("\tN: ");
    uart.transmit_dec32(current_negative);
    uart.transmit_string("\tL: ");
    uart.transmit_dec32((char)lock_signal);

    uart.transmit_string("\t M: ");
    switch (gate_mode)
    {
        case WAITING_FOR_LOCK_SIGNAL:           uart.transmit_string("Waiting for lock signal");
            break;

        case OPENING_PISTON:                    uart.transmit_string("Opening piston");
            break;

        case WAITING_FOR_CLOSING_CURRENT:       uart.transmit_string("Waiting for closing current");
            break;

        case WAITING_FOR_CURRENT_REVERSATION:   uart.transmit_string("Waiting for current reversation");
            break;

        case WAITING_FOR_CURRENT_DROP:          uart.transmit_string("Waiting for current drop");
            break;

        case WAITING_FOR_STABILIZATION:         uart.transmit_string("Waiting for stabilization");
            break;

        case CLOSING_PISTON:                    uart.transmit_string("Closing piston");
            break;

        default:
            break;
    }

    uart.transmit_new_line();
}