#define digitalWriteFast(pin,val) (val ? sio_hw->gpio_set = (1 << pin) : sio_hw->gpio_clr = (1 << pin))
#define digitalReadFast(pin) ((1 << pin) & sio_hw->gpio_in)

// STEP RESPONSE from squeezing
#define Rx D1 // receive 1 pin (D1)
#define Tx D2 // transmit 1 pin (D9)
#define settle 20 // settle time
#define samples 2000 // number of samples to accumulate
#define step_shresh 620000

// SERVO RUNNING TO SHAKE CHUMBLES
#include <Servo.h>
#define delaytime 50 // delay time between steps (ms)
#define servopin0 D0 // servo 0 pin (PA2)
Servo servo; //declare servos
int angle = 0;

// // Accelerameter
// #include <Adafruit_BNO08x.h>

// // For SPI mode, we need a CS pin
// // #define BNO08X_CS 10
// // #define BNO08X_INT 9
// #define BNO08X_RESET -1

// Adafruit_BNO08x  bno08x(BNO08X_RESET);
// sh2_SensorValue_t sensorValue;

// MP3
#include "mp3tf16p.h"

MP3Player mp3(D7, D6);

void setup() {
   Serial.begin(115200);
   pinMode(Tx,OUTPUT);
   servo.attach(servopin0);

  // Serial.println("Adafruit BNO08x test!");

  // // Try to initialize!
  // if (!bno08x.begin_I2C()) {
  // //if (!bno08x.begin_UART(&Serial1)) {  // Requires a device with > 300 byte UART buffer!
  // //if (!bno08x.begin_SPI(BNO08X_CS, BNO08X_INT)) {
  //   Serial.println("Failed to find BNO08x chip");
  //   while (1) { delay(10); }
  // }
  // Serial.println("BNO08x Found!");

  // for (int n = 0; n < bno08x.prodIds.numEntries; n++) {
  //   Serial.print("Part ");
  //   Serial.print(bno08x.prodIds.entry[n].swPartNumber);
  //   Serial.print(": Version :");
  //   Serial.print(bno08x.prodIds.entry[n].swVersionMajor);
  //   Serial.print(".");
  //   Serial.print(bno08x.prodIds.entry[n].swVersionMinor);
  //   Serial.print(".");
  //   Serial.print(bno08x.prodIds.entry[n].swVersionPatch);
  //   Serial.print(" Build ");
  //   Serial.println(bno08x.prodIds.entry[n].swBuildNumber);
  // }

  // setReports();

  // Serial.println("Reading events");
  // delay(100);
   }

// // Here is where you define the sensor outputs you want to receive
// void setReports(void) {
//   Serial.println("Setting desired reports");
//   if (! bno08x.enableReport(SH2_GAME_ROTATION_VECTOR)) {
//     Serial.println("Could not enable game vector");
//   }
// }

void chumble(){
  Serial.println("Servo moving in action!!");
  //  for (angle = 0; angle <= 180; ++angle) { // step servo 0
  int speed = 90;
  servo.write(90+speed);
  delay(2000);
  servo.write(90);
}

void step_response(){
   int32_t up,down;
   up = down = 0;
   for (int i = 0; i < samples; ++i) {
      digitalWriteFast(Tx,HIGH); // charge up
      up += analogRead(Rx); // read
      delayMicroseconds(settle); //settle
      digitalWriteFast(Tx,LOW); // charge down
      down += analogRead(Rx); // read
      delayMicroseconds(settle); // settle
      }
    int diff = up-down;
   Serial.println(diff); // send difference
  //  Serial.flush(); // finish communicating before measuring
   if (diff < step_shresh) {
    Serial.println("Push detected!!");
    Serial.println("Calling chumbles to move!!");
    chumble();
    playmp3();
   }
}

// void acc(){
//   if (bno08x.wasReset()) {
//     Serial.print("sensor was reset ");
//     setReports();
//   }
  
//   if (! bno08x.getSensorEvent(&sensorValue)) {
//     return;
//   }
  
//   switch (sensorValue.sensorId) {
    
//     case SH2_GAME_ROTATION_VECTOR:
//       Serial.print("x: ");
//       Serial.print(sensorValue.un.accelerometer.x*180);
//       Serial.print(" y: ");
//       Serial.print(sensorValue.un.accelerometer.y*180);
//       Serial.print(" z: ");
//       Serial.println(sensorValue.un.accelerometer.z*180);
//       delay(50);
//       break;
//   }
//   if (sensorValue.un.accelerometer.z*180 < 10) {
//     // playmp3();
//   }
// }

void playmp3(){
  mp3.initialize();
  mp3.playTrackNumber(1, 30);
}

void loop() {
  step_response();  
  // acc();
}