#include <SoftwareSerial.h>
#include "DFRobot_BNO055.h"
#include "Wire.h"

typedef DFRobot_BNO055_IIC BNO;


BNO bno(&Wire, 0x28);
SoftwareSerial mySerial(8, 7);  // RX, TX
String bt;
int pitch, roll, yaw;
int busy;
void setup() {
  // Open serial communications and wait for port to open:
  Serial.begin(115200);
  mySerial.begin(38400);


  // BNO STARTUP
  bno.reset();
  while (bno.begin() != BNO::eStatusOK) {
    Serial.println("bno begin failed");
    delay(2000);
  }
  Serial.println("bno begin success");

  mySerial.begin(38400);
  pinMode(6, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(4, OUTPUT);
}

void loop() {  // run over and over


  bt = mySerial.readString();
  Serial.println(busy);
  bt.trim();

  if (bt.startsWith("front") && busy == 0) {
    busy = 1;
  } else if (bt.startsWith("front") && busy == 1) {
    busy = 0;
  }

  if (bt.startsWith("back") && busy == 0) {
    busy = 2;
  } else if (bt.startsWith("back") && busy == 2) {
    busy = 0;
  }

  if (bt.startsWith("left") && busy == 0) {
    busy = 3;
  } else if (bt.startsWith("left") && busy == 3) {
    busy = 0;
  }

  if (bt.startsWith("right") && busy == 0) {
    busy = 4;
  } else if (bt.startsWith("right") && busy == 4) {
    busy = 0;
  }

  if (bt.startsWith("head") && busy == 0) {
    busy = 5;
  } else if (bt.startsWith("head") && busy == 5) {
    busy = 0;
  }

  if (busy == 1) {
    analogWrite(5, 90);
    digitalWrite(6, LOW);

  } else if (busy == 2) {
    analogWrite(6, 90);
    digitalWrite(5, LOW);

  } else if (busy == 3) {
    analogWrite(10, 90);
    digitalWrite(9, LOW);
  } else if (busy == 4) {
    analogWrite(10, 90);
    digitalWrite(9, LOW);
  } else if (busy == 5) {
    analogWrite(4, 130);
  } else {
    digitalWrite(6, LOW);
    digitalWrite(5, LOW);
    digitalWrite(9, LOW);
    digitalWrite(10, LOW);
    analogWrite(4, 0);
  }


  // BNO::sEulAnalog_t gyro;
  // gyro = bno.getEul();
  // yaw = gyro.head;
  // roll = gyro.pitch;
  // pitch = gyro.roll;

  // mySerial.print("Pitch:");
  // mySerial.print(pitch);
  // mySerial.print("Roll:");
  // mySerial.print(roll);
  // mySerial.print("Yaw:");
  // mySerial.print(yaw);
  // delay(10);
}