//#include "MeOrion.h"
#include <MeMCore.h>
#include <Wire.h>

MeGyro gyro;
MeDCMotor motor3(M1);
MeDCMotor motor4(M2);

uint8_t motorSpeed = 200;
MeUltrasonicSensor ultraSensor(PORT_3);

double rWheel = 6.5;
double dWheel = 11.5;

double Pos[4][1];

double angularError = 10;
double posError = 10;

double angle1;
double angle2;
double dist;

void setup()
{
  Serial.begin(115200);
  gyro.begin();

  Pos[0][0] = 0;
  Pos[0][1] = 0;
  Pos[1][0] = 25;
  Pos[1][1] = 0;
  Pos[2][0] = 25;
  Pos[2][1] = 25;
  Pos[3][0] = 0;
  Pos[3][1] = 25;
}

void loop()
{
  Serial.println("Start Loop");
  int i;
  int j;
  Serial.read();
  for(i=0;i<4;i++){
  Serial.print("Beggin angular adjustment for i: ");
  Serial.println(i);
  //angle adjustment
    gyro.update();
    
    angle1=gyro.getAngleZ();
    angle2=atan( (Pos[i+1][1]-Pos[i][1])/ (Pos[i+1][0]-Pos[i][0]) );
    motorSpeed = 70;
    Serial.print("angle1: ");
    Serial.print(angle1);
    Serial.print(" angle2: ");
    Serial.println(angle2);
    
    while(angle1-angle2+360 > angularError) {
      gyro.update();
      angle1=gyro.getAngleZ();
      angle2=atan( (Pos[i+1][0]-Pos[i][0])/ (Pos[i+1][1]-Pos[i][1]) );
      Serial.println("i: ");
      Serial.println(i);
      Serial.print(" angle1: ");
      Serial.print(angle1);
      Serial.print(" angle2: ");
      Serial.println(angle2);
      if(angle2+180<angle1+180){
        motor3.run(-motorSpeed);
        motor4.run(-motorSpeed);
      } else {
        motor3.run(motorSpeed);
        motor4.run(motorSpeed);
      }
      delay(200);
    }
    motor3.stop();
    motor4.stop();
    //translation
    dist=sqrt( pow(Pos[i+1][0]-Pos[i][0],2)+pow(Pos[i+1][1]-Pos[i][1],2) );
    motorSpeed = 0.2/rWheel;
    for(j = 0;j<5;j++) {
      motor3.run(motorSpeed);
      motor4.run(-motorSpeed);
      delay(1000);
    }

    motor3.stop();
    motor4.stop();
    delay(1000);
  }
  delay(100);
}