#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  = 97,
            CLOSE_POSITION = 10
        };

        enum current_tresholds
        {
            OFF_TRESHOLD = 200,
            ON_TRESHOLD  = 500
        };

        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
        };

        enum definitions
        {
            SAMPLES_COUNT = 20
        };

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

    //Variables
    private:
        volatile uint16_t samples[SAMPLES_COUNT];
        volatile uint8_t samples_item;

    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()
        {
            samples[samples_item++] = ADC;

            if (samples_item >= SAMPLES_COUNT)
            {
                samples_item = 0;
                uint32_t sum = 0;
                for (uint8_t i=0; i<SAMPLES_COUNT; i++)
                {
                    sum += samples[i];
                }

                uint16_t mean = sum/(uint32_t)SAMPLES_COUNT;

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

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

                    default: channel = 0;
                        break;
                }
            }

            else adc.start_conversion();
        }

        void gate_handler();

        void print_debug();
};

extern Controller controller;