#pragma once
#include "project_config.h"
#include "math.h"


// mode
#define ZONE_DEBUG true
// WALL - simple, SCAN - new
#define ZONE_TYPE SCAN // SCAN

#define VEL 210 //line forward velocity
#define CIRCLE_RADIUS 140
#define CIRCLE_TIME 9000
#define TURN_TIME 1000 //green marker turn time
#define TURN_RADIUS 50

#define ZONE_VEL 170

#define OBSTACLE_FILTER 0.7

#define STORAGE_OPEN 0
#define STORAGE_CLOSE 82

#define sq(x) (x)*(x)
#define max(x, y) (((x) > (y)) ? (x) : (y))
#define min(x, y) (((x) < (y)) ? (x) : (y))

enum ROBOT_STATE { //robot states enumeration
	IDLE,
	LINE,
	OBSTACLE,
	TURN_RIGHT,
	TURN_LEFT,
	TURN_ROUND,
	ALIGNMENT,
	TURN_ZONE,
	SIMPLE_ZONE,
	WALL,
	EVACUATION,
	EXIT,
	SCAN
};

namespace Robot
{    
    void LED_write(uint16_t val) //val: [0;2000]
    {
        TIM_SetCompare1(TIM2, val * 10);
    }
    
    void Init()
    {
        RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
        
        GPIO_InitTypeDef param; //LED pin configuration
        param.GPIO_Mode = GPIO_Mode_AF;
        param.GPIO_Pin = GPIO_Pin_15;
        param.GPIO_PuPd = GPIO_PuPd_NOPULL;
        param.GPIO_OType = GPIO_OType_PP;
        param.GPIO_Speed = GPIO_Speed_100MHz;
        GPIO_Init(GPIOA, &param);
        
        GPIO_ResetBits(GPIOA, GPIO_Pin_15);
        
        TIM_TimeBaseInitTypeDef timer_param; //LED timer configuration
        timer_param.TIM_Period = 20000;
        timer_param.TIM_Prescaler = 16800;
        timer_param.TIM_ClockDivision = TIM_CKD_DIV1;
        timer_param.TIM_CounterMode = TIM_CounterMode_Up;
        timer_param.TIM_RepetitionCounter = 0;
    
        TIM_OCInitTypeDef cann; //LED timer channels configuration
        cann.TIM_OCMode = TIM_OCMode_PWM1;
        cann.TIM_OutputState = TIM_OutputState_Enable;
        cann.TIM_OutputNState = TIM_OutputNState_Disable;
        cann.TIM_Pulse = 0;
        cann.TIM_OCPolarity = TIM_OCPolarity_High;
        cann.TIM_OCNPolarity = TIM_OCNPolarity_High;
        cann.TIM_OCIdleState = TIM_OCIdleState_Set;
        cann.TIM_OCNIdleState = TIM_OCNIdleState_Set;
    
        TIM_TimeBaseInit(TIM2, &timer_param); //timer 2 Init
        TIM_OC1Init(TIM2, &cann);
        GPIO_PinAFConfig(GPIOA, GPIO_PinSource15, GPIO_AF_TIM2); //GPIO alternative function config
        
        TIM_Cmd(TIM2, ENABLE); 
        
        LED_write(0);
		
		RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); //button pin configuration

		param.GPIO_Mode = GPIO_Mode_IN;
		param.GPIO_Pin = GPIO_Pin_13;
		param.GPIO_PuPd = GPIO_PuPd_UP;
		param.GPIO_OType = GPIO_OType_PP;
		param.GPIO_Speed = GPIO_Speed_100MHz;
		GPIO_Init(GPIOC, &param);
		
		RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE); //forward button pin configuration

		param.GPIO_Mode = GPIO_Mode_IN;
		param.GPIO_Pin = GPIO_Pin_2;
		param.GPIO_PuPd = GPIO_PuPd_UP;
		param.GPIO_OType = GPIO_OType_PP;
		param.GPIO_Speed = GPIO_Speed_100MHz;
		GPIO_Init(GPIOD, &param);
        
        time_service::Init(); //main timer initialization 
        time_service::startTime(); //main timer start
        
        Sonar::Init(); //ultrasonic distance sensor initialization
        
        Usart3::Init(); //st-link uart initialization
        
        VL53L0X_Error status = Laser::Init(); //vl53l0x initialization
    }
    
    void calibration(LineSensors *sensors, Motors *motors) //line sensors min-max calibration
    {
		while(GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_13) == 0);
		time_service::delayms(100);
        uint32_t time = time_service::millis();
        //while(time_service::millis() - time < 3600) //360 degrees turn
		while(GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_13) == 1)
        {
            //motors->drive(0, 0, 100);
			motors->m1(3000);
            sensors->sensorsCalibration();
			sensors->colorsCalibration();
            time_service::delayms(1);
        }
        motors->stop();
		time_service::delayms(100);
    }
}

void print(const char* message) //st-link uart print string
{
    int i=0;
    char c = message[i];
    while(c != '\0')
    {
        Usart3::write(c);
        i++;
        c = message[i];
    }
}

void print(int x) //st-link uart print 
{
	if(x == 0) Usart3::write('0');
    if(x < 0)
    {
        Usart3::write('-');
        x = -x;
    }
    int n = (int)log10((double)x) + 1;
    char array[10];
    for(int i=0; i<n; i++)
    {
        array[i] = x % 10;
        x /= 10;
    }
    for(int i=n-1; i>=0; i--)
    {
        Usart3::write(array[i] + 48);
    }
}
