#include "lidar.h"

void print(const char* message);
void print(int x);

void quickSortX(Point *array, uint8_t low, uint8_t high);
void quickSortY(Point *array, uint8_t low, uint8_t high);


#define signum(x) ((x < 0) ? -1 : ((x == 0) ? 0 : 1))
#define pow2s(x) (abs(x)*x)
#define min(a, b) ((a < b) ? a : b)
#define max(a, b) ((a > b) ? a : b)

#define COS45 0.707106781186547

double mask[6] = {3, 3, 3, 3, 3};

Lidar :: Lidar(Servos &servos) : m_servos(servos)
{
    VL53L0X_Error status = Laser::Init();
    m_servos.s3(90);
}

void Lidar :: buildHough()
{
	print("\n\r");
	print("startlidar\n\r");
	m_servos.s3(0);
	for(uint8_t i=0; i<8; i++)
	{
		for(uint8_t j=0; j<DIST_VALUES_CNT; j++)
		{
			m_houghSpace[i][j] = 0;
		}
	}
	time_service::delayms(500);
	uint32_t time = time_service::millis();
	for(int16_t i=0; i<180; i++)
	{
		m_servos.s3(i);
		while(time_service::millis() - time < 23);
		time = time_service::millis();
		uint16_t laser_distance = Laser::read();
		m_points[i] = laser_distance;
	}
	for(int16_t i=179; i>=0; i--)
	{
		m_servos.s3(i);
		while(time_service::millis() - time < 23);
		time = time_service::millis();
		uint16_t laser_distance = Laser::read();
		m_points[i] += laser_distance;
		m_points[i] /= 2;
	}
	for(int16_t i=0; i<180; i++)
	{
		m_servos.s3(i);
		while(time_service::millis() - time < 23);
		time = time_service::millis();
		uint16_t laser_distance = Laser::read();
		m_points[i] += laser_distance;
		m_points[i] /= 2;
	}
	for(int16_t i=179; i>=0; i--)
	{
		m_servos.s3(i);
		while(time_service::millis() - time < 23);
		time = time_service::millis();
		uint16_t laser_distance = Laser::read();
		m_points[i] += laser_distance;
		m_points[i] /= 2;
	}
	for(int16_t i=0; i<180; i++)
	{
		m_servos.s3(i);
		while(time_service::millis() - time < 23);
		time = time_service::millis();
		uint16_t laser_distance = Laser::read();
		m_points[i] += laser_distance;
		m_points[i] /= 2;
	}
	for(int16_t i=179; i>=0; i--)
	{
		m_servos.s3(i);
		while(time_service::millis() - time < 23);
		time = time_service::millis();
		uint16_t laser_distance = Laser::read();
		m_points[i] += laser_distance;
		m_points[i] /= 2;
	}
	for(int16_t i=0; i<180; i++)
	{
		m_servos.s3(i);
		while(time_service::millis() - time < 23);
		time = time_service::millis();
		uint16_t laser_distance = Laser::read();
		m_points[i] += laser_distance;
		m_points[i] /= 2;
	}
	for(int16_t i=179; i>=0; i--)
	{
		m_servos.s3(i);
		while(time_service::millis() - time < 23);
		time = time_service::millis();
		uint16_t laser_distance = Laser::read();
		m_points[i] += laser_distance;
		m_points[i] /= 2;
		if (m_points[i] > VAL_ERROR_LIDAR)
			m_points[i] = 0;
		m_cartesian[i].x = (int16_t) (((float)m_points[i]) * cos((float)i*DEG_TO_RAD));
		m_cartesian[i].y = (int16_t) (((float)m_points[i]) * sin((float)i*DEG_TO_RAD));
		if (m_points[i] == 0)
			continue;
		print(i);
		print(" ");
		print(m_points[i]);
		print("\n\r");
	}
	blur();
	blur();
	
	for(uint16_t i=0; i<180; i++)
	{
		for(uint8_t j=0; j<8; j++)
		{
			double angle = i - j*45; //angle between servo and perpendicular to line in hough space
			if(abs(angle) > 90) continue;
			float r = m_points[i] * cos(angle*DEG_TO_RAD); //perpendicular length
			m_houghSpace[j][(int)constrain(r/20, 0, DIST_VALUES_CNT)]++;
			if(max_votes < m_houghSpace[j][(int)constrain(r/20, 0, DIST_VALUES_CNT)])
			{				
				max_votes = m_houghSpace[j][(int)constrain(r/20, 0, DIST_VALUES_CNT)];
			}
		}
	}
	for(uint16_t i=179; i>0; i--)
	{
		for(uint8_t j=0; j<8; j++)
		{
			double angle = i - j*45; //angle between servo and perpendicular to line in hough space
			if(abs(angle) > 90) continue;
			float r = m_points[i] * cos(angle*DEG_TO_RAD); //perpendicular length
			m_houghSpace[j][(int)constrain(r/20, 0, DIST_VALUES_CNT)]++;
			if(max_votes < m_houghSpace[j][(int)constrain(r/20, 0, DIST_VALUES_CNT)])
			{				
				max_votes = m_houghSpace[j][(int)constrain(r/20, 0, DIST_VALUES_CNT)];
			}
		}
	}
	
	print("stoplidar\n\r");
	print("\n\r");
}

void Lidar :: blur()
{
	for(uint8_t i = 2; i < 178; i++)
	{
		double koef = 0;
		double sum = 0;
		for(int8_t j = -2; j <= 2; j++)
		{
			if(m_points[i+j] < 1200)
			{
				sum += double(m_points[i+j]) * mask[2 + j]; 
				koef += mask[2 + j];
			}
		}
		m_points_blured[i] = uint16_t(sum / koef);
	}
	
	for(uint8_t i = 0; i < 180; i++)
	{
		m_points[i] = m_points_blured[i];
	}
}

void Lidar :: findLines()
{
	// Miha`s teacher idea (find of lines)
	quickSortX(m_cartesian, 0, 179);
	left_wall_s = 0;
	right_wall_s = 0;
	forward_wall_s = 0;
#define DIFF_X_LOW 10
	for (uint8_t i = 10; i < 10+DIFF_X_LOW; i++) {
		right_wall_s+=m_cartesian[i].x;
	}
	right_wall_s /= -DIFF_X_LOW;
#define DIFF_X_HIGH 10
	for (uint8_t i = 160; i < 160+DIFF_X_HIGH; i++) {
		left_wall_s+=m_cartesian[i].x;
	}
	left_wall_s /= DIFF_X_HIGH;
	quickSortY(m_cartesian, 0, 179);
#define DIFF_Y_HIGH 10
	for (uint8_t i = 160; i < 160+DIFF_Y_HIGH; i++) {
		forward_wall_s+=m_cartesian[i].y;
	}
	forward_wall_s /= DIFF_Y_HIGH;
	print("\n\r");
	print("startline\n\r");
	print("d 0 ");
	print(forward_wall_s);
	print("\n\r");
	print("d Infinity ");
	print(left_wall_s);
	print("\n\r");
	print("d -Infinity ");
	print(right_wall_s);
	print("\n\r");
	// Discrete Radon algorithm
	for(uint8_t i=0; i<LINES_FIND_NUMBER; i++)
	{
		m_lines[i].angle = 0;
		m_lines[i].distance = 0;
	}
	uint8_t count = 0;
	for(uint8_t i=0; i<8; i++)
	{
		uint8_t max_votes_in_line = 0;
		uint8_t idx = 0;
		for(uint8_t j=1; j<DIST_VALUES_CNT; j++)
		{
			if(m_houghSpace[i][j] > max_votes_in_line && m_houghSpace[i][j] > max_votes/8)
			{
				idx = j;
				max_votes_in_line = m_houghSpace[i][j];
			}
		}
		m_lines[count].angle = i;
		m_lines[count].distance = idx*2;
		count++;

		print("p ");
		print(i*45);
		print(" ");
		print(idx*20);
		print("\n\r");
	}
	left_wall = m_lines[0].distance;
	forward_wall = m_lines[2].distance;
	right_wall = m_lines[4].distance;
	print("stoplidar\n\r");
	print("\n\r");
	// Zahar ball detection (finding local minimum algorithm)
	for (uint8_t i = 0; i < BALLS_FIND_NUMBER; i++) {
		m_balls[i].id = 255;
		m_balls[i].thresh = 0;
	}
	for (uint8_t i = 10; i <= 170; i += 4) {
		uint8_t small_left = 0, small_right = 0;
		for (uint8_t j = i-10; j < i; j++)
			if ((m_points[j]/10) > (m_points[i]/10)) small_left++;
		for (uint8_t j = i+1; j <= i+10; j++)
			if ((m_points[j]/10) > (m_points[i]/10)) small_right++;
		if (small_left > 3 && small_right > 3) {
			uint8_t factor = small_left+small_right;
			uint8_t min_id = 0;
			for (uint8_t k = 0; k < BALLS_FIND_NUMBER; k++) if (m_balls[min_id].thresh > m_balls[k].thresh) min_id = k;
			bool not_added = true;
			for(uint8_t k = 0; k < BALLS_FIND_NUMBER; k++)
			{
				if(abs(m_balls[k].id - i) < 10) not_added = false;
			}
			
			if(i < atan((double)forward_wall/left_wall)/DEG_TO_RAD)
			{
				if(abs(left_wall - m_points[i]*cos(i*DEG_TO_RAD)) < NEAR_WALL) not_added = false;
			}
			else if(i < atan2((double)forward_wall, -(double)right_wall)/DEG_TO_RAD)
			{
				if(abs(forward_wall - m_points[i]*sin(i*DEG_TO_RAD)) < NEAR_WALL) not_added = false;
			}
			else
			{
				if(abs(right_wall + m_points[i]*cos(i*DEG_TO_RAD)) < NEAR_WALL) not_added = false;
			}
			
			if (m_balls[min_id].thresh < factor && not_added) {
				m_balls[min_id].thresh = factor;
				m_balls[min_id].id = i;
			}
		}
	}
	/*for (uint8_t i = 0; i < BALLS_FIND_NUMBER; i++) {
		m_balls[i].id = 255;
		m_balls[i].thresh = 0;
	}
#define ZONE_BLOCK 5
#define ZONE_NUM 4
	for (uint8_t i = ZONE_BLOCK; i < 180-ZONE_BLOCK; i += ZONE_BLOCK) {
		uint32_t avgt = 0;
		for (uint8_t t = i; t < (i + ZONE_BLOCK); t++) {
			avgt += m_points[t];
		}
		uint8_t matches = 0;
		avgt /= ZONE_BLOCK;
		for (uint8_t d = max(0, i - ZONE_NUM*ZONE_BLOCK); d < i; d += ZONE_BLOCK) {
			uint32_t avg = 0;
			for (uint8_t t = d; t < (d + ZONE_BLOCK); t++) {
				avg += m_points[t];
			}
			avg /= ZONE_BLOCK;
			if (avgt < avg) matches++;
		}
		for (uint8_t d = i; d < min(180, i + ZONE_NUM*ZONE_BLOCK); d += ZONE_BLOCK) {
			uint32_t avg = 0;
			for (uint8_t t = d; t < (d + ZONE_BLOCK); t++) {
				avg += m_points[t];
			}
			avg /= ZONE_BLOCK;
			if (avgt < avg) matches++;
		}
		if (matches > 3) {
			uint8_t min_id = 0;
			for (uint8_t k = 0; k < BALLS_FIND_NUMBER; k++) if (m_balls[min_id].thresh > m_balls[k].thresh) min_id = k;
			bool not_added = true;
			for(uint8_t k = 0; k < BALLS_FIND_NUMBER; k++)
			{
				if(abs(m_balls[k].id - i) < 10) not_added = false;
			}
			if (m_balls[min_id].thresh < matches && not_added) {
				m_balls[min_id].thresh = matches;
				m_balls[min_id].id = i;
			}
		}
	}
#undef ZONE_BLOCK
#undef ZONE_NUM*/
}

uint8_t Lidar :: findEvacuationZone()
{
	/*double delta;
	delta = m_lines[0].distance * COS45 - EVACUATION_SIZE * COS45 - m_lines[7].distance;
	if(abs(delta) < 5) return 1;
	delta = m_lines[4].distance * COS45 - EVACUATION_SIZE * COS45 - m_lines[5].distance;
	if(abs(delta) < 5) return 4;
	delta = (m_lines[0].distance + m_lines[2].distance) * COS45 - EVACUATION_SIZE * COS45 - m_lines[1].distance;
	if(abs(delta) < 5) return 2;
	delta = (m_lines[4].distance + m_lines[2].distance) * COS45 - EVACUATION_SIZE * COS45 - m_lines[3].distance;
	if(abs(delta) < 5) return 3;
	return 0;*/
	m_servos.s3(0);
	uint32_t time = time_service::millis();
	int zone_1 = 0;
	int zone_2 = 0;
	int zone_3 = 0;
	int zone_4 = 0;
	// first way (discrete Radon)
	for(uint16_t i=0; i<180; i++)
	{
		uint16_t laser_distance = m_points[i]/10;
		if(i < atan(25.0/left_wall) / DEG_TO_RAD)
		{
			if(left_wall + 2 - cos(i*DEG_TO_RAD)*double(laser_distance) > E_THRESH)
			{
				zone_1++;
			}
		}
		else if(i > atan((forward_wall - 25.0)/left_wall) / DEG_TO_RAD && i < atan(1.0*forward_wall/left_wall) / DEG_TO_RAD)
		{
			if(left_wall + 2 - cos(i*DEG_TO_RAD)*double(laser_distance) > E_THRESH)
			{
				zone_2++;
			}
		}
		else if(i > atan(1.0*forward_wall/left_wall) / DEG_TO_RAD && i < atan2(1.0*forward_wall, (left_wall - 25.0)) / DEG_TO_RAD)
		{
			if(forward_wall + 2 - sin(i*DEG_TO_RAD)*double(laser_distance) > E_THRESH)
			{
				zone_2++;
			}
		}
		else if(i > atan2(1.0*forward_wall, (25.0 - right_wall)) / DEG_TO_RAD && i < 180 - atan(1.0*forward_wall/right_wall) / DEG_TO_RAD)
		{
			if(forward_wall + 2 - sin(i*DEG_TO_RAD)*double(laser_distance) > E_THRESH)
			{
				zone_3++;
			}
		}
		else if(i > 180 - atan(1.0*forward_wall/right_wall) / DEG_TO_RAD && i < 180 - atan((forward_wall - 25.0)/right_wall) / DEG_TO_RAD)
		{
			if(right_wall + 2 + cos(i*DEG_TO_RAD)*double(laser_distance) > E_THRESH)
			{
				zone_3++;
			}
		}
		else if(i > 180 - atan(25.0/right_wall) / DEG_TO_RAD)
		{
			if(right_wall + 2 + cos(i*DEG_TO_RAD)*double(laser_distance) > E_THRESH)
			{
				zone_4++;
			}
		}
	}
	// second way (Miha teacher`s idea)
	for(uint16_t i=0; i < 180; i++)
	{
		uint16_t laser_distance = m_points[i]/10;
		if(i < atan(25.0/left_wall_s) / DEG_TO_RAD)
		{
			if(abs(left_wall_s - cos(i*DEG_TO_RAD)*double(laser_distance)) > E_THRESH)
			{
				zone_1++;
			}
		}
		else if(i > atan((forward_wall_s - 25.0)/left_wall_s) / DEG_TO_RAD && i < atan(1.0*forward_wall_s/left_wall_s) / DEG_TO_RAD)
		{
			if(abs(left_wall_s - cos(i*DEG_TO_RAD)*double(laser_distance)) > E_THRESH)
			{
				zone_2++;
			}
		}
		else if(i > atan(1.0*forward_wall_s/left_wall_s) / DEG_TO_RAD && i < atan2(1.0*forward_wall_s, (left_wall_s - 25.0)) / DEG_TO_RAD)
		{
			if(abs(forward_wall_s - sin(i*DEG_TO_RAD)*double(laser_distance)) > E_THRESH)
			{
				zone_2++;
			}
		}
		else if(i > atan2(1.0*forward_wall_s, (25.0 - right_wall_s)) / DEG_TO_RAD && i < 180 - atan(1.0*forward_wall_s/right_wall_s) / DEG_TO_RAD)
		{
			if(abs(forward_wall_s - sin(i*DEG_TO_RAD)*double(laser_distance)) > E_THRESH)
			{
				zone_3++;
			}
		}
		else if(i > 180 - atan(1.0*forward_wall_s/right_wall_s) / DEG_TO_RAD && i < 180 - atan((forward_wall_s - 25.0)/right_wall_s) / DEG_TO_RAD)
		{
			if(abs(right_wall_s + cos(i*DEG_TO_RAD)*double(laser_distance)) > E_THRESH)
			{
				zone_3++;
			}
		}
		else if(i > 180 - atan(25.0/right_wall_s) / DEG_TO_RAD)
		{
			if(abs(right_wall_s + cos(i*DEG_TO_RAD)*double(laser_distance)) > E_THRESH)
			{
				zone_4++;
			}
		}
	}
	// may be retest with fix
	//zone_3 *= 2;
	//zone_3 /= 3;
	uint8_t zone_max = max(max(zone_1, zone_2), max(zone_3, zone_4));
	print("zone1 = ");
	print(zone_1);
	print(" zone2 = ");
	print(zone_2);
	print(" zone3 = ");
	print(zone_3);
	print(" zone4 = ");
	print(zone_4);
	print("\n\r");
	
	if(zone_max == zone_1) return 1;
	if(zone_max == zone_2) return 2;
	if(zone_max == zone_3) return 3;
	if(zone_max == zone_4) return 4;
	return 0;
}

// Sorting! May be rewrite!
void quickSortX(Point *array, uint8_t low, uint8_t high)
{
    uint8_t i = low;
    uint8_t j = high;
    int16_t pivot = array[(i + j) / 2].x;
    Point temp;

    while (i <= j)
    {
        while (array[i].x < pivot)
            i++;
        while (array[j].x > pivot)
            j--;
        if (i <= j)
        {
            temp = array[i];
            array[i] = array[j];
            array[j] = temp;
            i++;
            j--;
        }
    }
    if (j > low)
        quickSortX(array, low, j);
    if (i < high)
        quickSortX(array, i, high);
}

void quickSortY(Point *array, uint8_t low, uint8_t high)
{
    uint8_t i = low;
    uint8_t j = high;
    int16_t pivot = array[(i + j) / 2].y;
    Point temp;

    while (i <= j)
    {
        while (array[i].y < pivot)
            i++;
        while (array[j].y > pivot)
            j--;
        if (i <= j)
        {
            temp = array[i];
            array[i] = array[j];
            array[j] = temp;
            i++;
            j--;
        }
    }
    if (j > low)
        quickSortY(array, low, j);
    if (i < high)
        quickSortY(array, i, high);
}

void bubbleSort(Ball* arr, uint8_t size, bool id)
{
	Ball tmp;
    for(uint8_t i = 0; i < size - 1; ++i)
    {            
        for(uint8_t j = 0; j < size - 1; ++j)
        {     
            if (id ? (arr[j + 1].id < arr[j].id) : (arr[j + 1].thresh > arr[j].thresh)) 
            {
                tmp = arr[j + 1]; 
                arr[j + 1] = arr[j]; 
                arr[j] = tmp;
            }
        }
    }
}

MovementController::MovementController(Motors &motors, Servos &servos, Lidar &lidar) : m_servos(servos), m_motors(motors), m_lidar(lidar), m_state(F_SCAN_START)
{
}

#define sq(x) ((x)*(x))

bool MovementController::doTick()
{
	static uint8_t zone = 0, balls = 0, balls_count = 0;
	static double angle = 0, distance = 0, zone_distance = 0, zone_angle = 0;
	static uint32_t drive_time = 0;
	static uint32_t turn_time = 0;
	static uint32_t time = 0;
	static ZoneState next_state1 = DO_EXIT_ZONE, next_state2 = DO_EXIT_ZONE;
	static bool reverse = false;
	switch(m_state) {
		case F_SCAN_START:
			// TODO make it finite automate
			drive_time = 0;
			zone_distance = 0;
			zone_angle = 0;
			angle = 0;
			distance = 0;
			balls = 0;
			balls_count = 0;
			time = time_service::millis();
			m_lidar.buildHough();
			m_lidar.blur();
			m_state = F_SCAN_ZONE;
			next_state1 = DO_EXIT_ZONE;
			next_state2 = DO_EXIT_ZONE;
			reverse = false;
		break;
		case F_SCAN_ZONE:
			m_lidar.findLines();
			zone = m_lidar.findEvacuationZone();
			print(zone);
			print("\n\r");
			if(zone == 1)
			{
				zone_angle = -90;
				zone_distance = sqrt((double)sq(5) + (double)sq(m_lidar.left_wall - 25));
			}
			else if(zone == 2)
			{
				zone_distance = sqrt((double)sq(m_lidar.forward_wall - 25 - 30) + (double)sq(m_lidar.left_wall - 25));
				zone_angle = -atan2(double(m_lidar.forward_wall - 30), double(m_lidar.left_wall))/DEG_TO_RAD;
			}
			else if(zone == 3)
			{
				zone_distance = sqrt((double)sq(m_lidar.forward_wall - 25 - 30) + (double)sq(m_lidar.right_wall - 25));
				zone_angle = atan2(double(m_lidar.forward_wall - 30), double(m_lidar.right_wall))/DEG_TO_RAD;
			}
			else if(zone == 4)
			{
				zone_distance = sqrt((double)sq(5) + (double)sq(m_lidar.right_wall - 25));
				zone_angle = 90;
			}
			
			if (zone != 0) {
				/*for (uint8_t i = 0; i < BALLS_FIND_NUMBER; i++) {
					if (m_lidar.m_balls[i].id == 255) {
						m_lidar.m_balls[i].thresh = 0;
						continue;
					}
					if (abs(m_lidar.m_balls[i].id - zone_angle - 90) < BALL_SCAN_TRESH) {
						m_lidar.m_balls[i].id = 255;
						m_lidar.m_balls[i].thresh = 0;
					}
				}
				bubbleSort(m_lidar.m_balls, BALLS_FIND_NUMBER, true);
				for (uint8_t i = 1; i < BALLS_FIND_NUMBER-1; i++) {
					if (m_lidar.m_balls[i].id == 255 || m_lidar.m_balls[i+1].id == 255) {
						continue;
					}
					if (abs(m_lidar.m_balls[i].id - m_lidar.m_balls[i-1].id) < BALL_DELETE_TRESH &&
							abs(m_lidar.m_balls[i].id - m_lidar.m_balls[i+1].id) < BALL_DELETE_TRESH) {
						m_lidar.m_balls[i].id = 255;
						m_lidar.m_balls[i].thresh = 0;
					}
				}*/
				bubbleSort(m_lidar.m_balls, BALLS_FIND_NUMBER, false);
				print("\n\r");
				print("startballs\n\r");
				balls_count = BALLS_FIND_NUMBER;
				for (uint8_t i = 0; i < BALLS_FIND_NUMBER; i++) {
					if (m_lidar.m_balls[i].id == 255) {
						balls_count = i;
						break;
					}
					print(m_lidar.m_balls[i].id);
					print(" ");
					print(m_lidar.m_balls[i].thresh);
					print("\n\r");
				}
				print("stoplidar\n\r");
				print("\n\r");
			}
			m_state = ZONE_MOVE;
			time = time_service::millis();
		break;
		case TRANSITITION:
			if (time + drive_time + turn_time + 500 > time_service::millis()) {
				if (!reverse) {
					if (time + turn_time > time_service::millis()) {
						m_motors.drive(0, 0, signum(angle)*45);
					}
					else if (time + drive_time + turn_time > time_service::millis()) {
						m_motors.drive(-100, 0, 0);
					} 
					else if (time + drive_time + turn_time + 150 > time_service::millis()) {
						//m_motors.driveGyro(0, 0, 0);
						m_motors.stop();
					}
				} else {
					if (time + drive_time > time_service::millis()) {
						m_motors.drive(100, 0, 0);
					}
					else if (time + drive_time + turn_time > time_service::millis()) {
						m_motors.drive(0, 0, (-signum(angle))*45);
					} 
					else if (time + drive_time + turn_time + 150 > time_service::millis()) {
						//m_motors.driveGyro(0, 0, 0);
						m_motors.stop();
					}
				}
			}
			else if (time + drive_time + turn_time + 150 < time_service::millis()) {
				if (reverse) {
					m_state = next_state2;
					time = time_service::millis();
				} else {
					reverse = true;
					m_state = next_state1;
					time = time_service::millis();
				}
			}
		break;
		case ZONE_BALLS:
			if (balls < balls_count) {
				Ball b = m_lidar.m_balls[balls++];
				if (b.id == 255)
					break;
				double a = abs(b.id-90)*DEG_TO_RAD;
				//zone_distance = sqrt(90000.0+sq(double(m_lidar.m_points[b.id]))-600.0*double(m_lidar.m_points[b.id])*cos(abs(b.id-90)*DEG_TO_RAD));
				distance = sqrt(sq(m_lidar.m_points[b.id]*sin(a)) + sq(m_lidar.m_points[b.id]*cos(a) - 300.0));
				angle = signum(b.id-90)*acos((double(m_lidar.m_points[b.id])*cos(a) - 300.0)/distance)/DEG_TO_RAD;
				print("dist= ");
				print(distance);
				print(" angle= ");
				print(angle);
				print("\n\r");
				distance -= 150;
				drive_time = distance * 10;
				turn_time = (abs(angle) * 1000) / 45;
				m_state = TRANSITITION;
				reverse = false;
				next_state1 = ZONE_CATCH;
				next_state2 = ZONE_BALLS;
				time = time_service::millis();
			} else {
				distance = zone_distance;
				angle = zone_angle;
				drive_time = distance * 100;
				turn_time = abs(angle * 1000) / 45;
				m_state = TRANSITITION;
				reverse = false;
				next_state1 = ZONE_DROP;
				next_state2 = DO_EXIT_ZONE;
				time = time_service::millis();
			}
		break;
		case ZONE_MOVE:
				if (time + 3000 > time_service::millis()) {
					m_motors.drive(-100, 0, 0);
				} else {
					m_state = ZONE_BALLS;
					time = time_service::millis();
				}
		break;
		case ZONE_CATCH:
			if(time + 1000 > time_service::millis())
			{
				m_servos.s4(90*(time + 1000 - time_service::millis())/1000);
			}
			else if(time + 2000 > time_service::millis())
			{
				m_servos.s2(145.0*(time + 2000 - time_service::millis())/1000);
			}
			else if(time + 3000 > time_service::millis())
			{
				m_servos.s4(90*(time_service::millis() - time - 1000)/1000);
			}
			else if(time + 4000 > time_service::millis())
			{
				m_servos.s2(145.0*(time_service::millis() - time - 3000)/1000);
			}  
			else 
			{
				time = time_service::millis();
				m_state = TRANSITITION;
			}
    break;
		case ZONE_DROP:
				time = time_service::millis();
				m_state = TRANSITITION;
		break;
		case DO_EXIT_ZONE:
			m_motors.stop();
		break;
	}
	return true;
}

void MovementController::deinit()
{
    m_state = F_SCAN_START;
}
