#include "init.h"

int main()
{
    Robot::Init();
    
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
    
    GPIO_InitTypeDef paramC;
    paramC.GPIO_Mode = GPIO_Mode_OUT;
    paramC.GPIO_Pin = GPIO_Pin_12;
    paramC.GPIO_PuPd = GPIO_PuPd_NOPULL;
    paramC.GPIO_OType = GPIO_OType_PP;
    paramC.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_Init(GPIOC, &paramC);
    
	time_service::delayms(2000);
	
	static MPU9250_ mpu;
	uint32_t time = time_service::millis(); //universal time varaible 
    while(!mpu.setup(0x68) && time_service::millis() - time < 1000); //accel gyro initialization
    volatile bool ok = true;
	volatile double angle = 0;
    
    mpu.calibrateAccelGyro();
    ok = false;
	/*motors.m1(10000);
	motors.m4(10000);
	motors.m2(-10000);
	motors.m3(-10000);
    mpu.calibrateMag();
	motors.stop();*/
	
	static LineSensors line_sensors; //line sensors initialization
    line_sensors.start();
	static Motors motors(&mpu); //motors initialization
	static Servos servo;
	
	servo.s3(45);
	//servo.s4(0);
    
    if (!ZONE_DEBUG) 
	{
		Robot::calibration(&line_sensors, &motors);
	}
    ROBOT_STATE state = IDLE;
    volatile int64_t u;
	double obstacle_dist = 200;
	uint8_t obstacle_cnt = 0; //how many times we see obstacle
	bool button_old = 1; //old button state
	bool button_flag = 0; //button switch flag
    
	int16_t sensors[6];
	
	//servo.s4(125);
	static Lidar lidar(servo);
	static MovementController zone_controller(motors, servo, lidar);
	uint8_t zone = 0;
	double zone_distance = 0;;
	uint16_t zone_time = 0;
	uint8_t reg_count = 0;
	
	uint8_t zone_silver_count = 0; //number of entrances to the zone
	
	double wall_i = 0;
	bool zone_flag = true; // zone has two corners
	bool exit = false; //is found green line
	bool kit_brought = false;
	uint8_t corner_exit = 0;
	uint32_t color_time;
	
	servo.s3(150);
	
    while(1)
    {
		//servo.s3(180);
        //motors.drive(0, 0, 0);
        switch(state)
        {
            case LINE:
				ok = mpu.update(); //500-680 microseconds long gyro update
				if(mpu.isConnected()) angle = (175 - abs(mpu.getRoll())); //calculation inclination value
				else angle = 0;
				if(GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_2) == 0)
				{
					state = OBSTACLE;
					obstacle_cnt = 0;
					motors.stop();
					time = time_service::millis();
				}
				//obstacle detection every 60 milliseconds
				/*if(time_service::millis() - time > 60)
				{
					Sonar::sendPulse();
					time_service::delayus(20);
					time = time_service::millis();
					if(Sonar::getValue() > 20)
					{
						obstacle_dist = OBSTACLE_FILTER*obstacle_dist + (1 - OBSTACLE_FILTER)*Sonar::getValue();
					}
					else
					{
						obstacle_dist = 200;
					}
					if(obstacle_dist < 50 && angle < 20) 
					{
						obstacle_cnt++;
						if(obstacle_cnt > 0)
						{
							state = OBSTACLE;
							obstacle_cnt = 0;
							motors.stop();
						}
					}
					else
					{
						if(obstacle_cnt > 0) obstacle_cnt--;
					}
				}*/
				reg_count++;
				if(reg_count > 1)
				{
					u = line_reg(&line_sensors); //center of mass line regulator
					//print(u);
					//print("\n\r");
					reg_count = 0;
				}
				/*for(int i=0; i<6; i++) //sensors values to debug
				{
					sensors[i] = line_sensors.getValue(i);
				}*/
				if(line_sensors.left > 15 && angle < 15) //green markers check
				{
					if(line_sensors.right < 1) state = TURN_LEFT;
					else state = TURN_ROUND;
					time = time_service::millis();
					line_sensors.left = -50;
					line_sensors.right = -50;
				}
				if(line_sensors.right > 15 && angle < 15) //green markers check
				{
					if(line_sensors.left < 1) state = TURN_RIGHT;
					else state = TURN_ROUND;
					time = time_service::millis();
					line_sensors.left = -50;
					line_sensors.right = -50;
				}
				if(line_sensors.silver > 3 && angle < 10) //check silver line
				{
					line_sensors.silver = 0;
					state = IDLE;
					zone_silver_count++;
					if(zone_silver_count >= 200)
					{
						state = SIMPLE_ZONE;
					}
					time = time_service::millis();
				}
				if(line_sensors.hill > 10 && angle < 15)
				{
					angle = 50;
				}
				double velocity = VEL;
				if(abs(u) > 0) velocity = constrain(VEL - abs(u)*0.4 - line_sensors.isCross() * 70, 0, VEL); //forward driving velocity
				if(angle > 15) velocity += angle*50; //add velocity on slope
				if(angle < 15) 
					motors.drive(velocity, -1.3*u, u); //forward velocity, side velocity, angular velocity
				else motors.driveGyro(velocity, 1.5*u, 0.5*u);
                break;
            case OBSTACLE:
				if(time_service::millis() - time < 500)
				{
					motors.drive(-40, 0, 0);
				}
				else if(time_service::millis() - time < 1500)
				{
					motors.driveGyro(0, 0, 85);
					ok = mpu.update();
				}
				else if(line_sensors.getValue(0) < 400)
				{
					motors.driveGyro(PI*CIRCLE_RADIUS*1000/CIRCLE_TIME, 0, -180.0*1000/CIRCLE_TIME);
					ok = mpu.update();
				}
				else 
				{
					motors.stop();
					motors.drive(0, 0, 0);
					time_service::delayms(1);
					motors.drive(0, 0, 0);
					state = LINE;
					time = time_service::millis();
				}
                break;
			case TURN_LEFT: //turn on green marker
				if(time_service::millis() - time <= TURN_TIME)
				{
					motors.driveGyro(0, PI*TURN_RADIUS*500/TURN_TIME, -90.0*1000/TURN_TIME);
					ok = mpu.update();
				}
				else if(time_service::millis() - time <= TURN_TIME + 1000)
				{
					motors.drive(-10, 90, 0);
					ok = mpu.update();
				}
				else
				{
					motors.stop();
					state = LINE;
				}
				break;
			case TURN_RIGHT: //turn on green marker
				if(time_service::millis() - time <= TURN_TIME)
				{
					motors.driveGyro(0, -PI*TURN_RADIUS*500/TURN_TIME, 90*1000/TURN_TIME);
					ok = mpu.update();
				}
				else if(time_service::millis() - time <= TURN_TIME + 1000)
				{
					motors.drive(-10, -90, 0);
					ok = mpu.update();
				}
				else
				{
					motors.stop();
					state = LINE;
				}
				break;
			case TURN_ROUND: //reverse turn on green marker
				if(time_service::millis() - time <= 2*TURN_TIME)
				{
					motors.driveGyro(0, 0, -180.0*500/TURN_TIME);
					ok = mpu.update();
				}
				else if(time_service::millis() - time <= 2*TURN_TIME + 1000)
				{
					motors.drive(-70, 0, 0);
					ok = mpu.update();
				}
				else
				{
					motors.stop();
					state = LINE;
				}
				break;
			case ALIGNMENT:
				if(abs(silver_reg(&line_sensors)) > 10)
				{
					motors.drive(0, 0, silver_reg(&line_sensors));
					mpu.update();
				}
				else
				{
					motors.stop();
					state = ZONE_TYPE == SCAN ? TURN_ZONE : SIMPLE_ZONE;
					time = time_service::millis();
				}
				break;
			case TURN_ZONE:
				if(time_service::millis() - time <= 1000)
				{
					motors.drive(-70, 0, 0);
					ok = mpu.update();
				}
				if(time_service::millis() - time <= 2*TURN_TIME + 1000)
				{
					motors.driveGyro(0, 0, -180.0*500/TURN_TIME);
					ok = mpu.update();
				}
				else if(time_service::millis() - time <= 2*TURN_TIME + 2000)
				{
					motors.drive(-100, 0, 0);
					ok = mpu.update();
				}
				else
				{
					motors.stop();
					state = SCAN;
				}
				break;
			case SCAN:
				if (!zone_controller.doTick())
				{
					state = LINE;
				}
				break;
			case SIMPLE_ZONE:
				if(time_service::millis() - time < 2000)
				{
					motors.drive(90, 0, 0);
				}
				else if(time_service::millis() - time < 4000)
				{
					motors.driveGyro(0, 0, -45);
				}
				else if(time_service::millis() - time < 5000)
				{
					motors.drive(-80, 0, 0);
				}
				else 
				{
					motors.stop();
					state = WALL;
				}
				break;
			case WALL:
				servo.s3(150);
				print("wall_reg");
				print("\n\r");
				bool isWall = true;
				int16_t u = wall_reg(&line_sensors, &isWall);
				if(!isWall)
				{
					motors.stop();
					state = EXIT;
					time = time_service::millis();
					corner_exit = 0;
				}
				ok = mpu.update();
			
				double g = mpu.getGyroZ();
				if(abs(g) > 10) wall_i += g*0.015;
				if(abs(wall_i) < 60 && abs(wall_i) > 20 && abs(u) < 20)
				{
					wall_i = 0;
					if(zone_flag) state = EVACUATION;
					zone_flag = !zone_flag;
					time = time_service::millis();
				}
				if(abs(wall_i) >= 60 && abs(u) < 20)
				{
					wall_i = 0;
				}
				if(abs(wall_i) <= 20 && abs(u) < 20)
				{
					wall_i = 0;
				}
				
				double dist = line_sensors.readSHARP();
			
				velocity = -120;
				motors.drive(velocity, 0.3*u, u);
				break;
			case EVACUATION:
				if(time_service::millis() - time < 1000)
				{
					motors.drive(-150, 0, 0);
				}
				else if(time_service::millis() - time < 3000)
				{
					motors.driveGyro(0, 1.6*55, -55);
				}
				else if(time_service::millis() - time < 4000)
				{
					motors.drive(150, 0, 0);
				}
				else if(time_service::millis() - time < 7000)
				{
					motors.stop();
					motors.drive(0, 0, 0);
					servo.s2(90);
					if((time_service::millis() - time - 4000) % 1500 < 750)
					{
						servo.s1(STORAGE_OPEN);
					}
					else
					{
						servo.s1(STORAGE_CLOSE);
					}
				}
				else if(time_service::millis() - time < 8000)
				{
					motors.drive(-50, 0, 0);
				}
				else if(time_service::millis() - time < 9000)
				{
					servo.s2(145);
					motors.driveGyro(0, -1.6*55, 55);
				}
				else
				{
					servo.s1_stop();
					servo.s2_stop();
					print("rescued");
					print("\n\r");
					motors.stop();
					state = WALL;
					kit_brought = true;
				}
				break;
			case EXIT:
				if(time_service::millis() - time < 1000)
				{
					servo.s3(0);
					motors.drive(100, 0, 0);
				}
				else if(time_service::millis() - time < 2000)
				{
					motors.stop();
					motors.drive(0, 0, 0);
					uint16_t laser_distance = Laser::read();
					if(laser_distance < 250)
					{
						corner_exit++;
					}
					servo.s3(map(time_service::millis() - time, 1000, 2000, 0, 40));
				}
				if(time_service::millis() - time < 4000)
				{
					if(zone_flag)
					{
						servo.s3(150);
						if(corner_exit < 3)
						{
							motors.drive(-150, 0, -10);
						}
						else
						{
							motors.driveGyro(-70, -20, -45);
						}
					}
					else
					{
						motors.driveGyro(-160, 0, -20);
					}
				}
				else if(!kit_brought)
				{
					motors.stop();
					state = WALL;
				}
				else if(time_service::millis() - time < 6000)
				{
					motors.driveGyro(0, 0, -45);
				}
				else if(time_service::millis() - time < 7000)
				{
					motors.drive(100, 0, 0); 
					if(time_service::millis() - color_time > ((256 - TCS34725_INTEGRATIONTIME_2_4MS) * 12 / 5 + 1))
					{
						if(line_sensors.isLeftOnGreen() || line_sensors.isRightOnGreen())
						{
							exit = true;
						}
						color_time = time_service::millis();
					}
				}
				else if(exit)
				{
					motors.stop();
					state = LINE;
				}
				else if(time_service::millis() - time < 8000)
				{
					motors.drive(-100, 0, 0);
				}
				else if(time_service::millis() - time < 10000)
				{
					motors.driveGyro(0, 0, 45);
				}
				else
				{
					print("exit");
					print("\n\r");
					motors.stop();
					state = WALL;
				}
				break;
            case IDLE: //idle mode
				motors.stop();
                motors.drive(0, 0, 0);
				zone_controller.deinit();
				ok = mpu.update();
				//servo.s3(180);
                break;
        }
		if(GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_13) == 1) //button debounce
		{
			if(button_old == 0)
			{
				time_service::delayms(5);
				if(GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_13) == 1)
				{
					button_flag = !button_flag;
					if(state == IDLE) state = ZONE_DEBUG ? ZONE_TYPE : LINE;
					else state = IDLE;
				}
			}
		}
		button_old = GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_13);
        time_service::delayus(900);
    }    
}
