// це реалізація студентів, подивитись потім, що тут і як. Є проблеми з реалізацією поворотів
#include <Stepper.h>
#include <Servo.h>
#include "pair.h"

#define PI_VALUE 3.14159

#define LEFT_MOTOR_ENABLE_STATE_DPIN 9
#define LEFT_MOTOR_STEP_DPIN 8
#define LEFT_MOTOR_DIRECTION_DPIN 7

#define RIGHT_MOTOR_ENABLE_STATE_DPIN 6
#define RIGHT_MOTOR_STEP_DPIN 5
#define RIGHT_MOTOR_DIRECTION_DPIN 4

#define SCAN_SERVO_MOTOR_DPIN 10

#define SENSOR_TRIG_DPIN 3
#define SENSOR_ECHO_DPIN 2


const int min_distance = 30; // in cm(value may change)

const int wheel_radius = 4; // in cm(value may change)
const int wheels_axis_width = 15; // in cm(value may change)
const int scan_angle_deviation = 45;

const int motor_steps_for_rotate = 200;
const float step_one_degree_value = (float)motor_steps_for_rotate / 360;

Stepper left_stepper(motor_steps_for_rotate, LEFT_MOTOR_ENABLE_STATE_DPIN, LEFT_MOTOR_STEP_DPIN);
Stepper right_stepper(motor_steps_for_rotate, RIGHT_MOTOR_ENABLE_STATE_DPIN, RIGHT_MOTOR_STEP_DPIN);

Servo scanservo;

int current_servo_position = 90;


std::pair<int, int> scanPlace();

void setup()
{
  Serial.begin(115200);
  pinMode(SENSOR_TRIG_DPIN, OUTPUT);
  pinMode(SENSOR_ECHO_DPIN, INPUT);

  pinMode(LEFT_MOTOR_DIRECTION_DPIN, OUTPUT);
  pinMode(RIGHT_MOTOR_DIRECTION_DPIN, OUTPUT);

  scanservo.attach(SCAN_SERVO_MOTOR_DPIN);
  scanservo.write(90);

  digitalWrite(LEFT_MOTOR_DIRECTION_DPIN, HIGH);
  digitalWrite(RIGHT_MOTOR_DIRECTION_DPIN, HIGH);

  delay(1000);
}

void loop() 
{
  int distance = makeMeasurement();
  if(distance <= min_distance)
  {
    std::pair<int, int> result = scanPlace();
    Serial.print("Optimal angle: ");
    Serial.println(result.first);
    if(result.second <= distance + 5)
    {
      moveOnDistance(-30); 
    }
    else
    {
      make_wheel_rotate(result.first);
    }
  }
  else
  {
    left_stepper.step(5);
    right_stepper.step(5);
  }
}

void moveOnDistance(const int distance) // if value < 0 - move back. Distance in cm
{
  if(distance < 0)
  {
    digitalWrite(LEFT_MOTOR_DIRECTION_DPIN, LOW);
    digitalWrite(RIGHT_MOTOR_DIRECTION_DPIN, LOW);
  }
  float q_rotates = (float)distance / (2 * PI_VALUE * wheel_radius);
  //Serial.print("q_rotates: ");
  //Serial.println(q_rotates);
  int i_step = 2 * pow(-1, distance < 0);
  for(int i = 1; i <= abs(motor_steps_for_rotate * q_rotates * 4); i += abs(i_step))
  {
    left_stepper.step(i_step);
    right_stepper.step(i_step);
    delay(1);
  }
  //left_stepper.step(motor_steps_for_rotate * q_rotates * 4);
  //right_stepper.step(motor_steps_for_rotate * q_rotates * 4);
  delay(100);
  if(distance < 0)
  {
    digitalWrite(LEFT_MOTOR_DIRECTION_DPIN, HIGH);
    digitalWrite(RIGHT_MOTOR_DIRECTION_DPIN, HIGH);
  }
}

int make_wheel_rotate(const int angle)
{ 
  if(angle > 0) // left wheel rotate
  {
    float q_rotates = getQRotates(angle);
    int i_step = 2;
    for(int i = 1; i <= motor_steps_for_rotate * q_rotates * 4; i += i_step)
    {
      left_stepper.step(i_step);
      delay(1);
    }
    // Serial.print("q_rotates: ");
    // Serial.println(q_rotates);
    // Serial.print("q_steps: ");
    // Serial.println(motor_steps_for_rotate * q_rotates * 4);
    //left_stepper.step(motor_steps_for_rotate * q_rotates * 4);
    delay(100);
  }
  else if(angle < 0) // right wheel rotate
  {
    float q_rotates = getQRotates(abs(angle));
    // Serial.print("q_rotates: ");
    // Serial.println(q_rotates);
    // Serial.print("q_steps: ");
    // Serial.println(motor_steps_for_rotate * q_rotates * 4);
    int i_step = 2;
    for(int i = 1; i <= motor_steps_for_rotate * q_rotates * 4; i += i_step)
    {
      right_stepper.step(i_step);
      delay(1);
    }
    //right_stepper.step(motor_steps_for_rotate * q_rotates * 4);
    delay(100);
  }
}

int makeMeasurement() // make one measurement and return result
{
  digitalWrite(SENSOR_TRIG_DPIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(SENSOR_TRIG_DPIN, LOW);

  int duration = pulseIn(SENSOR_ECHO_DPIN, HIGH) / 58;
  return duration;
}

std::pair<int, int> scanPlace() // scan the place, and return optimal angle(left: value < 0; right: value > 0)
{

  int measurment_delay = 10;
  std::pair<int, int> result;
  int prev_optimal_distance = 0;
  int optimal_distance = 0;
  int optimal_angle = 0;
  for(;current_servo_position >= 90 - scan_angle_deviation; current_servo_position--)
  {
    prev_optimal_distance = optimal_distance;
    optimal_distance = makeMeasurement();
    if(prev_optimal_distance > optimal_distance)
    {
      optimal_distance = prev_optimal_distance;
    }
    else
    {
      optimal_angle = current_servo_position - 90;
      result = { optimal_angle, optimal_distance };
    }
    scanservo.write(current_servo_position);
    delay(measurment_delay);
  }

  current_servo_position = 90;
  scanservo.write(90);
  delay(200);
  
  for(;current_servo_position <= 90 + scan_angle_deviation; current_servo_position++)
  {
    prev_optimal_distance = optimal_distance;
    optimal_distance = makeMeasurement();
    if(prev_optimal_distance > optimal_distance)
    {
      optimal_distance = prev_optimal_distance;
    }
    else
    {
      optimal_angle = current_servo_position - 90;
      result = { optimal_angle, optimal_distance };
    }
    scanservo.write(current_servo_position);
    delay(measurment_delay);
  }

  current_servo_position = 90;
  scanservo.write(90);
  delay(200);

  return result;
}

float getQRotates(const int angle) // return quantity of wheel rotates by angle
{
  return (float)(wheels_axis_width * angle) / (360 * wheel_radius);
}