#include <Servo.h>
#include <Arduino_LSM9DS1.h>

const int upDownServoPin = 8;
const int leftRightServoPin = 7;
const int buttonPin = 2;

Servo upDownServo;
Servo leftRightServo;

float x, y, z;

int threshold = 0.04;

int buttonState = 0;

int middleUD = 90;
int middleLR = 90;
float xInitPos, yInitPos, zInitPos = 0;

int offset = 30;
int currentPosUD;
int currentPosLR;

int d = 3;
void setup() {
  // put your setup code here, to run once:
  
  SerialBegin();
  IMUSetup();
  ServoSetup();
  pinMode(buttonPin, INPUT);

  getInitialPos(400);

}

void loop() {

  readAccel();

}

bool GetButtonState(){
  buttonState = digitalRead(buttonPin);
  if (buttonState == HIGH) {

    //String msg = "Button is pressed!";
    return true;

  } else {

    //String msg = "Button is not pressed!";
    return false;
    
  }

}


void readAccel(){

  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(x, y, z);

    //Serial.print(x);
    //Serial.print(" m/s2");
    //Serial.print('\t');
    //Serial.print(y);
    //Serial.print(" m/s2");
    //Serial.print('\t');
    //Serial.print(z);
    //Serial.println(" m/s2");
    delay(100);

    if (checkIfNotMoving()){
      GoStraight();
      //Serial.println("not moving");
    }

    else{
      if (!GetButtonState()){
        getDominantAxis(x,y,z);
      }
      
    }
    
    
  }

}

bool checkIfNotMoving(){

  if (abs(zInitPos - z) < 0.08 && abs(yInitPos - y) < 0.08){
    
    return true;
  }else{
    return false;
  }

}

void getDominantAxis(float x, float y, float z){

  
  if (abs(z) > abs(y)){
  // fork is up or backwards
    if (z > 0){
    //Serial.println("Up");
    GoLimp(0,1);
    }
    else if (z < 0){
    //Serial.println("Down");
    GoLimp(0,-1);
    }
  }
  else if (abs(y) > abs(z)){
  // fork is tilted right or left
    if (y < 0){
      //Serial.println("Right");
      GoLimp(1,1);
    }
    else if (y > 0){
      //Serial.println("Left");
      GoLimp(1,-1);
    }
  }
  



}

void GoLimp(int servo, int dir){

  if (servo == 0 && dir == 1){
    Rotate(0, middleUD + offset);
    Rotate(1, middleLR);
  }
  else if(servo == 0 && dir == -1){
    Rotate(0, middleUD - offset);
    Rotate(1, middleLR);
  }
  else if(servo == 1 && dir == 1){
    Rotate(1, middleLR - offset);
    Rotate(0, middleUD);
    
  }
  else if(servo == 1 && dir == -1){

    Rotate(1, middleLR + offset);
    Rotate(0, middleUD);

  }

}

void Rotate(int index, int angle){
  if (index == 0){ //UD moving
    if (currentPosUD < angle){ //pos 45, targetAngle 90

      for (int i = currentPosUD; i < angle; i++){
        upDownServo.write(i);

        delay(d);
      }
      currentPosUD = angle;

    }
    else{ //pos 90, targetAngle 45

      for (int i = currentPosUD; i > angle; i--){
        upDownServo.write(i);

      delay(d);
      }
      currentPosUD = angle;
    }
    
  }else{

    if (currentPosLR < angle){ //pos 45, targetAngle 90

      for (int i = currentPosLR; i < angle; i++){
        leftRightServo.write(i);

        delay(d);
      }
      currentPosLR = angle;

    }
    else{ //pos 90, targetAngle 45

      for (int i = currentPosLR; i > angle; i--){
        leftRightServo.write(i);

      delay(d);
      }
      currentPosLR = angle;
    }


  }




}

void GoStraight(){
  
  upDownServo.write(middleUD);
  leftRightServo.write(middleLR);
  currentPosUD = middleUD;
  currentPosLR = middleLR;
}


//------------setup------------
void SerialBegin(){

  Serial.begin(9600);

  while (!Serial);
  //Serial.println("Serial Port Initiated");
}

void ServoSetup(){
  upDownServo.attach(upDownServoPin);
  leftRightServo.attach(leftRightServoPin);
  GoStraight();
}

void IMUSetup(){

  if (!IMU.begin()) {

    //Serial.println("Failed to initialize IMU!");

    while (1);

  }

  //Serial.print("Accelerometer sample rate = ");
  //Serial.print(IMU.accelerationSampleRate());
  //Serial.println("Hz");

}

void getInitialPos(int sampleSize){
  
  float x1,y1,z1;
  

  for(int i = 0; i < sampleSize; i++){
  IMU.readAcceleration(x1, y1, z1);
  xInitPos += x1;
  yInitPos += y1;
  zInitPos += z1;
  }

  xInitPos = xInitPos / sampleSize;
  yInitPos = yInitPos / sampleSize;
  zInitPos = zInitPos / sampleSize;


}