#pragma once
#include "vl53l0x.h"
#include "motors.h"
#include "time_service.h"
#include "math.h"

#define DIST_VALUES_CNT 70 //one count two santimeters
#define LINES_FIND_NUMBER 8
#define BALLS_FIND_NUMBER 8

#define EVACUATION_SIZE 25 //centimeters
#define BALL_SCAN_TRESH 10 // angles
#define BALL_DELETE_TRESH 5 // angles
#define VAL_ERROR_LIDAR 1200 // mm
#define E_THRESH 6

#define NEAR_WALL 10

struct Line
{
	uint8_t angle; //every 45 degrees
	uint16_t distance;
};

struct Point
{
	int16_t x;
	int16_t y;
};

struct Ball {
	uint8_t thresh;
	uint8_t id;
};

class Lidar
{
    public:
        Lidar(Servos &servos);
		void buildHough();
		void blur();
		void findLines();
		uint8_t findEvacuationZone();
	
		int16_t left_wall, forward_wall, right_wall;
		int16_t left_wall_s, forward_wall_s, right_wall_s;
		Ball m_balls[BALLS_FIND_NUMBER];
		int16_t m_points[180];
		int16_t m_points_blured[180];
    private:
		Servos m_servos;
		Point m_cartesian[180];
		uint8_t m_houghSpace[8][DIST_VALUES_CNT];
		uint8_t max_votes;
		Line m_lines[LINES_FIND_NUMBER];
};

enum ZoneState { //robot states enumeration
	F_SCAN_START,
	F_SCAN_ZONE,
	TRANSITITION,
	ZONE_MOVE,
	ZONE_BALLS,
	ZONE_CATCH,
	ZONE_DROP,
	DO_EXIT_ZONE
};

class MovementController
{
	public:
        MovementController(Motors &motors, Servos &servos, Lidar &lidar);
		bool doTick();
		void deinit();
	private:
		ZoneState m_state;
		Servos m_servos;
		Motors m_motors;
		Lidar m_lidar;
		
};
