// Include the servo library:
#include <Servo.h>
// Create a new servo objects
Servo topArm;
Servo wheel;
Servo rightArm;
Servo bottomArm;
Servo leftArm;
// Define the servo pin:
#define servo1Pin 22
#define servo2Pin 19
#define servo3Pin 18
#define servo4Pin 17
#define servo5Pin 16
#define pageLeft 8
#define pageRight 9

#define rx1pin 7 // receive pin
#define tx1pin 6 // transmit pin
#define rx2pin 4 // receive pin
#define tx2pin 2 // transmit pin
#define settle 1000 // settle time

// Define Commands
#define INVALID -1
#define SET 0
#define MOVE 1

// For tracking touches
const int numReadings = 10;
int rightTouchReadings[numReadings];
int leftTouchReadings[numReadings];
int readIndex = 0;
int totalRight = 0;
int totalLeft = 0;
float averageRight = 0.0; 
float averageLeft = 0.0;
float threshold = 0.10;
int counter = 0;

// Define touches
int PAGE_RIGHT = 1;
int PAGE_LEFT = 2;
int NO_TOUCH = 0;

// Create a variable to store the servo position:
int angle = 0;
void setup() {
  // Attach the Servo variable to a pin:
  topArm.attach(servo3Pin);
  wheel.attach(servo4Pin);
  rightArm.attach(servo5Pin);
  leftArm.attach(servo1Pin);
  bottomArm.attach(servo2Pin);
  Serial.begin(9600); 
  pinMode(pageLeft, INPUT_PULLUP);
  pinMode(pageRight, INPUT_PULLUP);

  // initialize all the readings to 0:
  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
    rightTouchReadings[thisReading] = 0;
    leftTouchReadings[thisReading] = 0;
  }

  pinMode(tx1pin,OUTPUT); // set transmit pin to output
  pinMode(tx2pin,OUTPUT);
}

int delay_time = 200;
int prevButtonState = 1;
int currButtonState = 1;
int currentPageDirection = -1; // 1 means right, 0 means left, -1 mean init
int firstTime = 0;

int topArmRightBaseAngle = 22;
int topArmLeftBaseAngle = 180 - topArmRightBaseAngle + 13;

int numTurnsRight = 0;
int numTurnsLeft = 0;

int detect_touch() {
  uint32_t up1,down1, up2, down2;
  up1 = down1 = 0;
  up2 = down2 = 0;
  noInterrupts(); // disable interrupts while measuring
  digitalWrite(tx1pin,HIGH); // charge up
  up1 += analogRead(rx1pin); // read
  delayMicroseconds(settle); //settle
  digitalWrite(tx1pin,LOW); // charge down
  down1 += analogRead(rx1pin); // read
  delayMicroseconds(settle); // settle

  
  digitalWrite(tx2pin,HIGH); // charge up
  up2 += analogRead(rx2pin); // read
  delayMicroseconds(settle); //settle
  digitalWrite(tx2pin,LOW); // charge down
  down2 += analogRead(rx2pin); // read
  delayMicroseconds(settle); // settle
  
  interrupts(); // enable interrupts after measuring

  int rightReading = up1 - down1;
  int leftReading = up2 - down2;

  if (counter < 10) {
    counter += 1;
  } else {
    float fracDiffRight = (averageRight - (float)rightReading) / averageRight;
    float fracDiffLeft = (averageLeft - (float)leftReading) / averageLeft;

    if ((fracDiffRight > threshold) && (fracDiffLeft > threshold)) {
      // do nothing
      return NO_TOUCH;
    } else if ((fracDiffRight > threshold)) {
      Serial.println("Right touch!");
      Serial.flush();
//      delay(200);
      Serial.print("Right: ");
      Serial.println(rightReading);
      return PAGE_RIGHT;
    } else if ((fracDiffLeft > threshold)) {
      Serial.println("Left touch!");
      Serial.flush();
//      delay(200);
      Serial.print("Left: ");
      Serial.println(leftReading);
      return PAGE_LEFT;
    } else {
      totalRight = totalRight - rightTouchReadings[readIndex];
      rightTouchReadings[readIndex] = rightReading;
      totalRight = totalRight + rightTouchReadings[readIndex];
    
      totalLeft = totalLeft - leftTouchReadings[readIndex];
      leftTouchReadings[readIndex] = leftReading;
      totalLeft = totalLeft + leftTouchReadings[readIndex];
      
      readIndex = readIndex + 1;
    
      if (readIndex >= numReadings) {
        readIndex = 0;
      }
    
      averageRight = (float)totalRight / (float)numReadings;
      averageLeft = (float)totalLeft / (float)numReadings;
      return NO_TOUCH;
      Serial.print("Average Right: ");
      Serial.println(averageRight);
      Serial.print("Average Left: ");
      Serial.println(averageLeft);
    }
    
  }
  return NO_TOUCH;
}

void loop() {

  if (firstTime == 0) {
//    basePosition(false);
    initPosition();
    firstTime = 1;
  }

  int touch = detect_touch();

  if(touch == PAGE_LEFT){
    Serial.println("Left page turn");
    if ((currentPageDirection == 1) || (currentPageDirection == -1)) {
      currentPageDirection = 0;
      basePosition(true);
    }  else {
      turnPageSequence();
      numTurnsLeft += 1;
      numTurnsRight -= 1;
      basePosition(false);
    }
    return;
  }

  if(touch == PAGE_RIGHT){
    Serial.println("Right page turn");
    if ((currentPageDirection == 0) || (currentPageDirection == -1)) {
      currentPageDirection = 1;
      basePosition(true);
    }  else {
      turnPageSequence();
      numTurnsRight += 1;
      numTurnsLeft -= 1;
      basePosition(false);
    }
    return;
  }

}
                          
void rotateServo (Servo &servo, int angle, int delayTime) {
 servo.write(angle);
 delay(delayTime);              // Refresh cycle of servo
}

void bottomArmPageTurn() {
  if (currentPageDirection == 1) {
    //rotate bottom arm from right to left
    rotateServo(bottomArm, 180, 0);
  } else {
    //rotate bottom arm from left to right
    rotateServo(bottomArm, 0, 0);
  }
}

void sideArmPageTurn() {
//  if (currentPageDirection == 1) {
//    //lift left arm up
//    rotateServo(leftArm, 90, 2000);
//  } else {
//    //lift right arm up
//    rotateServo(rightArm, 90, 2000);
//  }
}

void wheelPageTurn() {
  if (currentPageDirection == 1) {
    // rotate wheel to 180
    rotateServo(wheel, 0, 0);
  } else {
    // rotate wheel to 0
    rotateServo(wheel, 180, 0);
  }
}

void topArmPageTurn() {
  if (currentPageDirection == 1) {
    // rotate wheel to 180
    rotateServo(topArm, 35, 0);
  } else {
    // rotate wheel to 0
    rotateServo(topArm, 145, 0);
  }
}

void turnPageSequence() {
  wheelPageTurn();
  delay(1000);
  sideArmPageTurn();
  bottomArmPageTurn();
  delay(250);
  topArmPageTurn();
  delay(1000);

  // bottom arm moves
  // wheel lifted basically right after

  
  // left arm goes down

  // bottom arm goes back

  // wheel goes back

  // topArm lowered
}

void bottomArmBasePosition() {
  if (currentPageDirection == 1) {
    //bottom arm should be pointing to right
    rotateServo(bottomArm, 0, 0);
  } else {
    //bottom arm should be pointing to left
    rotateServo(bottomArm, 180, 0);
  }
}

void sideArmBasePosition() {
//  if (currentPageDirection == 1) {
//    //left arm would be lifted up, so needs to be lowered
//    rotateServo(leftArm, 0, 2000);
//  } else {
//    //right arm would be lifter up, so needs to be lowered
//    rotateServo(rightArm, 180, 2000);
//  }
}

void topArmAndWheelBasePosition() {
  if (currentPageDirection == 1) {
    // wheel should go back to 0
    rotateServo(wheel, 180, 0);
    delay(500);
    // topArm should be down on page
    rotateServo(topArm, getTopArmRightBaseAngle(),  0);
  } else {
    // wheel should go back to 0
    rotateServo(wheel, 0, 0);
    delay(500);
    // topArm should be down on page
    rotateServo(topArm, getTopArmLeftBaseAngle(),  0);
  }
}

void topArmAndWheelBasePositionSwitching() {
  if (currentPageDirection == 1) {
    rotateServo(topArm, getTopArmRightBaseAngle(),  0);
    delay(100);
    rotateServo(wheel, 180, 0);    
  } else {
    rotateServo(topArm, getTopArmLeftBaseAngle(),  0);
    delay(100);
    rotateServo(wheel, 0, 0);
  }
}

int getTopArmRightBaseAngle() {
  return topArmRightBaseAngle - (numTurnsRight / 4);
}

int getTopArmLeftBaseAngle() {
  return topArmLeftBaseAngle + (numTurnsLeft / 4);
}

void basePosition(boolean switching) {
  // Default motor position in-between page turning sequences
  sideArmBasePosition();
  bottomArmBasePosition();
  if (switching) {
    topArmAndWheelBasePositionSwitching();
  } else topArmAndWheelBasePosition();
  delay(delay_time);
}

void initPosition() {
  rotateServo(topArm, 90, 0);
  rotateServo(wheel, 90, 0);
  rotateServo(rightArm, 90, 0);
  rotateServo(leftArm, 90, 0);
  rotateServo(bottomArm, 90, 0);
  delay(delay_time);
}