#pragma once

#include "servo.h"
#include "lib/adc.h"
#include "lib/usart.h"

//Working from 25° to 140°

class Controller
{
    //Enums
    public:
        enum servo_positions
        {
            OPEN_POSITION  = 40,
            CLOSE_POSITION = 120
        };

        enum current_tresholds
        {
            OFF_TRESHOLD = 300,
            ON_TRESHOLD  = 50
        };

        enum stabilization_tresholds
        {
            STABILIZATION_TRESHOLD = 450   //500ms
        };

        enum gate_modes
        {
            WAITING_FOR_LOCK_SIGNAL,
            OPENING_PISTON,
            WAITING_FOR_CLOSING_CURRENT,
            WAITING_FOR_CURRENT_REVERSATION,
            WAITING_FOR_CURRENT_DROP,
            WAITING_FOR_STABILIZATION,
            CLOSING_PISTON
        };

    //Instances
    public:
        servo_class servo;
        adc_class adc;
        usart uart;

    //Variables
    private:
        //uint8_t samples[5];

    public:
        volatile uint16_t current_positive;
        volatile uint16_t current_negative;
        volatile uint8_t  channel;

        volatile bool lock_signal;
        volatile uint8_t lock_signal_delay;

        volatile uint8_t  debug_delay;
        volatile uint16_t stabilization_delay;

        gate_modes gate_mode;

    //Methods
    private:
        void waiting_for_lock_signal();
        void opening_piston();
        void waiting_for_closing_current();
        void waiting_for_current_reversation();
        void waiting_for_current_drop();
        void waiting_for_stabilization();
        void closing_piston();

    public:
        Controller();

        void timer0_handler()
        {
            adc.start_conversion();
            debug_delay++;

            if (gate_mode == WAITING_FOR_STABILIZATION) stabilization_delay++;
        }

        void lock_signal_delay_handler()
        {
            lock_signal_delay++;

            if (lock_signal_delay > 224)    //cca 4s
            {
                if (PIND & (1<<PD3))
                {
                    lock_signal_delay = 0;
                    TCCR2 = 0;
                    TCNT2 = 0;
                    lock_signal = false;
                }

                else lock_signal_delay = 0;

            }
        }

        void lock_signal_handler()
        {
            if (!lock_signal)
            {
                lock_signal = true;
                TCCR2 = (1<<CS22) | (1<<CS21) | (1<<CS20);
            }
        }

        void adc_handler()
        {
            switch (channel)
            {
                case 0: current_positive = ADC;
                        channel++;
                        adc.set_channel(adc.ADC1);
                        adc.start_conversion();
                    break;

                case 1: current_negative = ADC;
                        channel = 0;
                        adc.set_channel(adc.ADC0);

                default: channel = 0;
                    break;
            }
        }

        void gate_handler();

        void print_debug();
};

extern Controller controller;