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


MeGyro gyro;
MeDCMotor motor1(9);
MeDCMotor motor2(10);
int points[4][2]={{0,0},{0,25},{25,25},{25,0}};

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

}

void loop() {
  
  for(int i = 0;i<4;i++){
    goToPoint(i);
  }
}

void goToPoint(int i){
  bool state = true;
  while(state){
    turn90();
    runToPoint(i);
  }
}

void runToPoint(int i){
  float v0 = 0.0;
  bool state = true;
  float distance = 0;
  while(state){
    motor1.run(-100);
    motor2.run(100);
    delay(25*100);
    state = false;
    // gyro.update();
    // float currentVelocity = gyro.getAcclY()*0.005+v0;
    // distance += v0*0.05 + (1/2)*gyro.getAcclY()*0.05;
    // v0 = currentVelocity;
    // if (distance*100>max(point[1],point[0])){
    //   state = false;
    //   motor1.stop();
    //   motor2.stop();
    // }
  }
}


void turn90(){
  bool state = true;
  gyro.update();
  float z_initial = gyro.getAngleZ();
  float z_stop = z_initial - 90;
  if(z_stop<-180){
    float step = -180-z_stop;
    z_stop = 180-step;
  }
  while(state){
    gyro.update();
    motor1.run(-100);
    motor2.run(-100);
    Serial.println(String(round(gyro.getAngleZ()))+" and "+String(round(z_stop)));
    if (round(gyro.getAngleZ())==round(z_stop)){
      state = false;
      motor1.stop();
      motor2.stop();
    }
  }
}