#include <Servo.h>


const int trigPin = A4;
const int echoPin = A5;

unsigned long previousMillis;
unsigned long interval = 500;

int MoveForward = 0;
int MoveBackward = 1;
int MoveLeft = 3;
int MoveRight = 4;
int distanceTraveled = 0;
bool lastState;
Servo myservo;

void setup() {
  Serial.begin(9600);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(4, INPUT);
  pinMode(5, INPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  previousMillis = millis();
  myservo.attach(A1);
  pinMode(A0, INPUT);
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
}

bool targetReatched = false;
void loop() {
  String str = Serial.readString();
  ParseInput(str);
}

float EchoDistance()
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  float duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  float distance = duration*0.034 / 2;
  return distance;
}

void ParseInput(String str)
{
  if (str.length() <= 0)
    return;

  char func = str[0];
  int arg = str.substring(2).toInt();

  if (str[0] == '~')
  {
    Stop();
  }
  else if (str[0] == 'm')
  {
    GoStraightDistance(arg);
  }
  else if (str[0] == 'r')
  {
    TurnDistance(arg);
  }
  else if (str[0] == 'b')
  {
    Serial.println(EchoDistance());
  }
  else if (str[0] == 'p')
  {
    RotateServo(arg);
  }
  else if (str[0] == 'c')
  {
    Serial.println(ReadDistanceFromInfraRed());
  }
  else if (str[0] == 'w')
  {
    RotateServo(arg);
    Serial.println(ReadDistanceFromInfraRed());
  }
  else if (str[0] == 's')
  {
    LoopRotateAndCheckDistance();
  }
}

float ReadDistanceFromInfraRed()
{
  float v = analogRead(A0);
  float u = (v / 1023.0f) * 3.30f;
  if (u < 0.50f)
  {
    u = 0.50f;
  }

  return (70.0f / u) - 6.50f;
}

int currentRot = 0;
void LoopRotateAndCheckDistance()
{
  if(currentRot <= 90)
  {
    for (currentRot = 0; currentRot <= 180; currentRot += 1) {
      myservo.write(currentRot);
      PrintRotDistance(currentRot, ReadDistanceFromInfraRed());
      delay(15);
    }
  }
  else
  {
    for (currentRot = 180; currentRot >= 0; currentRot -= 1) {
      myservo.write(currentRot);
      PrintRotDistance(currentRot, ReadDistanceFromInfraRed());
      delay(15);
    }
  }

  currentRot = 90;
  myservo.write(currentRot);
}

void PrintRotDistance(int rot, float distance)
{
  Serial.print("rot: ");
  Serial.print(rot);
  Serial.print(", dist:");
  Serial.println(distance);
}


void RotateServo(int angle)
{
  currentRot = angle;
  myservo.write(currentRot);
}

void GoStraightDistance(int distanceToTravel)
{
  distanceTraveled = 0;
  int dir = MoveForward;
  if (distanceToTravel < 0)
  {
    dir = MoveBackward;
    distanceToTravel = distanceToTravel * -1;
    Serial.print(distanceToTravel);
  }

  while (distanceTraveled < distanceToTravel)
  {
    if (dir == MoveForward)
    {
      Front();
    }
    else if (dir == MoveBackward)
    {
      Back();
    }
    CalcDistanceTraveled();
  }
  Stop();
}

int AngleToTics(int angle){
  return roundf(angle/6);
}

void TurnDistance(int angle)
{
  distanceTraveled = 0;
  int distanceToTravel = AngleToTics(angle);
  int dir = MoveLeft;
  if (distanceToTravel < 0)
  {
    dir = MoveRight;
    distanceToTravel = distanceToTravel * -1;
  }
  while (distanceTraveled < distanceToTravel)
  {
    if (dir == MoveLeft)
    {
      LeftFront();
    }
    else if (dir == MoveRight)
    {
      RightFront();
    }
    CalcDistanceTraveled();
  }
  Stop();
}


void CalcDistanceTraveled() {
  int state = digitalRead(A3);
  if (state != lastState)
  {
    distanceTraveled++;
    lastState = state;
  }
}


void RightFront()
{
  digitalWrite(7, HIGH);
  digitalWrite(8, LOW);
  digitalWrite(10, HIGH);

  digitalWrite(12, LOW);
  digitalWrite(13, HIGH);
  digitalWrite(11, HIGH);
}

void LeftFront()
{
  digitalWrite(7, LOW);
  digitalWrite(8, HIGH);
  digitalWrite(10, HIGH);

  digitalWrite(12, HIGH);
  digitalWrite(13, LOW);
  digitalWrite(11, HIGH);
}


void Front()
{
  digitalWrite(7, HIGH);
  digitalWrite(8, LOW);
  digitalWrite(10, HIGH);

  digitalWrite(12, HIGH);
  digitalWrite(13, LOW);
  digitalWrite(11, HIGH);
}

void Back()
{
  digitalWrite(7, LOW);
  digitalWrite(8, HIGH);
  digitalWrite(10, HIGH);

  digitalWrite(12, LOW);
  digitalWrite(13, HIGH);
  digitalWrite(11, HIGH);
}


void Stop()
{
  digitalWrite(10, 0);
  digitalWrite(11, 0);
}