#include <TimerFour.h>//check
#include <TimerOne.h>//check
#include <TimerTwo.h>//check
#include <TimerThree.h>//check

#include <communication.h>//check
#include <stepper.h>//check
#include <vexdcmodule.h>//check
#include <Servo.h>//check
#include <irsensor.h>//check

communication comChannel;

// good for any motor
unsigned forward = 1;
unsigned backward = 0;

// all motor initializations
double lipDisplacement = 10;
double center = 93.6875;

unsigned xMotorPin = 3, xDirectionPin = 30, xOnPin = 31, xZeroButtonPin = 0;//32 if used // 2 digital pins, 1 analog pin
double xButtonDistance = 0;
stepper xMotor(xOnPin,xDirectionPin,xMotorPin,xZeroButtonPin,xButtonDistance);
double xFreq = 180;
double xFreqFast = 180;
double defaultX = 93.6875;

unsigned yMotorPin = 5, yDirectionPin = 28, yOnPin = 29, yZeroButtonPin = 27; // 2 digital pins, 1 analog pin
double yFreq=100;
double yFreqFast=150;
double yButtonDistance =  18.5; //18.5 inches
stepper yMotor(yOnPin, yDirectionPin, yMotorPin, yZeroButtonPin,yButtonDistance);
double defaultY = 497.6;

unsigned zMotorPin = 12, zTopButton = 36, zBottomButton = 37 ; // 3 digital pins, 1 analog pin
Servo zServo;
vexdcmodule zMotor(zMotorPin, &zServo);
double zFreq = 60.0;

unsigned rotationMotorPin = 9, rotationIrSensorPin = 42, stripLength = 219, whiteStrips = 32, rotationDistance = 109, direct = 1; // 2 digital pins
irsensor rotationIR(rotationIrSensorPin,stripLength,whiteStrips);
Servo rotationServo;
vexdcmodule rotationMotor(rotationMotorPin, &rotationServo);

/*
unsigned tiltMotorPinForward = 2, tiltMotorPinBackward = 3, tiltTopButtonPin = 24, tiltBottomButtonPin = 25; // 1 digital pin, 2 analog pins
 vexdcmotormodule tiltMotor(tiltMotorPinForward,tiltMotorPinBackward,tiltTopButtonPin,tiltBottomButtonPin);
 //*/
/*
unsigned servoPolePin = 10, dDefault = 0;
 Servo servoPole; // 1 digital pin (eventually);
 //*/
// unsigned rf = 2; // 2 digital pins // use Serial2 for rf

unsigned rfidEnablePin = 22; // use Serial3 rx to read the data
char code[10];
char oldCode[10];

int bytesread = 0;
int val = 0;

// communication variables used to store the data and for comparison for the motors
unsigned xCoordinate1 = 0, yCoordinate1 = 0, location = 0, error1 = 0, theta1 = 0;
unsigned rotationAngle = 0, rotationDirection = 0;
boolean xValid1 = false, yValid1 = false, lValid = false, errorValid = false, thetaValid1 = false,rotationAngleValid = false;
boolean xyMoving = false, cValid = false;
boolean startProgram = true;

// for timing:
unsigned long s;

boolean xInterrupted = false, yInterrupted = false;

void setup() {
  // put your setup code here, to run once:
  //  noInterrupts();
  Serial.begin(9600);
  //Serial.println("Start");
  pinMode(rfidEnablePin,OUTPUT); // rfid enable
  pinMode(yMotorPin,OUTPUT);
  pinMode(yDirectionPin,OUTPUT);
  // Serial.println("Initializing Stepper Motor");
  yMotor.setStepLocation(0);
  yMotor.setLocation(0);
  yMotor.setTimeout(100000);
  yMotor.setDegreesPerStep(7.5);
  yMotor.setDistancePerStep(0.296377);
  yMotor.setDirection(forward);
  yMotor.setFreq(yFreq);
  yMotor.setMaxDistance(580);
  yMotor.setMinDistance(0);
  xMotor.setFreq(xFreq);
  xMotor.setStepLocation(0);
  xMotor.setLocation(0);
  xMotor.setTimeout(100000);
  xMotor.setDegreesPerStep(1.8);
  xMotor.setDistancePerStep(0.21); 
  xMotor.setDirection(forward);
  xMotor.setMaxDistance(157);
  xMotor.setMinDistance(13);
  //Serial.println("Initializing rotation motor");
  rotationMotor.initializeIr(&rotationIR,22,stripLength);
  zMotor.initializeButtons(zTopButton,zBottomButton);

  //  servoPole.attach(servoPolePin);
  //  interrupts();
  Serial.println("A");
}

void loop() {
 // Serial.println("Start of Loop");
  Serial3.begin(2400); // for rfid
  if(startProgram == true){
    yMotor.setFreq(yFreqFast);
    xMotor.setFreq(xFreqFast);
    while(1==1){
      comChannel.getCommunication();
      if(comChannel._S == true){
        comChannel._S = false;
        break;
      } 
    }
  }

  //Serial.println("Moving away");
  xyDefault();
  if(startProgram == false){
    comChannel.stageBDone();
    moveZUp(0);  
  }
  rotationMotor.rotateAngle(0,0);
  
  yMotor.setFreq(yFreqFast);
  xMotor.setFreq(xFreqFast);

  if(startProgram = true){
    startProgram = false;
  }
 // Serial.println("rotating");
//  rotationMotor.rotateDistance(0);

  comChannel.sendBookNotInB();

  // Serial.println("Wait for D");
  comChannel._newCommand = false;
  while(0==0){
    comChannel.getCommunication();
    if(comChannel._D == true){
      comChannel._D = false;
      break;
    }
  }
//  Serial.println("Reading RFID");

  int i;
  rfidInit();
  boolean gotRFID = false;
  while(gotRFID == false){
    gotRFID = readRfid(); 
  }
  
  // 2.  /* communication code: add functionality for full coordinate system
  comChannel._newCommand = false;
  //Serial.println("Looking for C");
  while(0==0){
    comChannel.getCommunication(); // need to have the getData functions reset valids
    if(comChannel._cValid == true){//&& comChannel._lValid == true) // has coordinates
      comChannel._cValid == false; 
      // comChannel._lValid = false;
      break;
    } 
  }
  //Serial.println("Moving over book");
  moveXY1();
  // 4. move rotation motor to correct angle from comm data
  
  if(comChannel._direction == 1){
    comChannel._angle1=(comChannel._angle1-180);
  }
  rotationMotor.rotateAngle(comChannel._angle1,0); // 0 = slow speed, 1 = medium speed

  ///*
  //Serial.println("Moving Down");
  yMotor.setFreq(yFreq);
  xMotor.setFreq(xFreq);

  moveZDown(0);

  // 7 move book away from lip  
  /*
  Serial.print("Angle received: ");
  Serial.println(comChannel._rotationAngle);
  */
  int endRotation = 0;
  if(comChannel._direction == 1){
    //Serial.println("Adding rotaion angle, direction = 1");
    endRotation =  rotationMotor.getLocation()*360/stripLength+comChannel._rotationAngle;
  }
  else{
    //Serial.println("Subtracting rotaion angle, direction = 0");
    endRotation =  rotationMotor.getLocation()*360/stripLength-comChannel._rotationAngle;    
  }
  rotationMotor.rotateAngle(endRotation,1); // 1 is medium speed
  // 10 move to one side of book to push off
  
  moveZUp(1);
  yMotor.setFreq(yFreqFast);
  xMotor.setFreq(xFreqFast);

  xySideOfBook();
  /*
  Serial.print("Angle 2: ");
  Serial.println(comChannel._angle2);
  */
  rotationMotor.rotateAngle(comChannel._angle2,0);
  
  yMotor.setFreq(yFreq);
  xMotor.setFreq(xFreq);

  moveZDown(1);

}

void rfidInit(){
  //Serial.println("initializing RFID");
  pinMode(rfidEnablePin,OUTPUT); // Set digital pin 2 as OUTPUT to connect it to the RFID /ENABLE pin 
  digitalWrite(rfidEnablePin,LOW); // Activate the RFID reader 
}

boolean readRfid(){
  // Serial.println("Reading RFID");
  boolean done = false;
  if(Serial3.available() > 0) {          // if data available from reader 
    if((val = Serial3.read()) == 10) {   // check for header 
      digitalWrite(rfidEnablePin, LOW);                  // Activate the RFID reader
      bytesread = 0; 
      while(bytesread<10) {              // read 10 digit code 
        if( Serial3.available() > 0) { 
          val = Serial3.read(); 
          if((val == 10)||(val == 13)) { // if header or stop bytes before the 10 digit reading 
            break;                       // stop reading 
          } 
          code[bytesread] = val;         // add the digit           
          bytesread++;                   // ready to read next digit  
        } 
      } 
      if(bytesread == 10) {              // if 10 digit read is complete 
        //        Serial.print("TAG code is: ");   // possibly a good TAG 
        //comChannel.sendRFID(code);            // print the TAG code 
        Serial.print("R");
        Serial.println(code);
        done = true;
      } 
      bytesread = 0; 
      digitalWrite(rfidEnablePin, HIGH);                  // deactivate the RFID reader
    }  
  }
  return done;
}

// horizontal motion
void moveXY1(){
  moveXY(comChannel._x1,comChannel._y1); 
}
void moveXYLip(){
  double x = xMotor.getStepLocation()*xMotor.getDistancePerStep();
  double y = yMotor.getStepLocation()*yMotor.getDistancePerStep(); 
  moveXY( center, y);
}

void xySideOfBook(){
  /*
  Serial.print("Going to x: ");
  Serial.println(comChannel._x2);
  Serial.print("Going to y: ");
  Serial.println(comChannel._y2);
  // */
  int newX=comChannel._x2-15,newY=comChannel._y2-15;
  if(newX < 0){
    newX=0;
  }
  if(newY < 0){
    newY = 0;
  }
 moveXY(newX,newY); 
}

void xyDefault(){
  moveXY(defaultX , defaultY ); 
}
void moveXY(double x, double y){
  /*
  Serial.println("Going to: ");
   Serial.print("Y: ");
   Serial.println(y);
   Serial.print("X: ");
   Serial.println(x);
   //*/
  xyMoving = true;
  //  Timer1.attachInterrupt(yISR);
  // 6: add functionality to attache correct intterupts to correct timers
  Timer3.attachInterrupt(xISR);
  xMotor.rotateDistance(x);
  while(xMotor.getRunning() == 1){
    /*
    Serial.print("X location: ");
     Serial.println(xMotor.getStepLocation());
     //*/
  }
  //Serial.println("X motor stopped");
  Timer3.detachInterrupt();
  Timer3.attachInterrupt(yISR);
  yMotor.rotateDistance(y);
  while(yMotor.getRunning() == 1){
    /*
    Serial.print("Y location: ");
     Serial.println(yMotor.getStepLocation());
     //*/
  }
  //Serial.println("Y motor stopped");
  Timer3.detachInterrupt();
  // moves three stepper motors in the xy plane 
}

void moveZDown(int up){
  zMotor.rotateFull(up);
}

void moveZUp(int up){
  zMotor.rotateFull(up);
} 

// this function checks to see if any data was received
// add functionality for bounds checking
void checkForValidData(){
  if(comChannel._cValid == true){
    if(cValid == false){
      xCoordinate1 = comChannel._x1;
      yCoordinate1 = comChannel._y1;
      theta1 = comChannel._angle1;
      rotationAngle = comChannel._rotationAngle;
      rotationDirection = comChannel._direction;
      setCValids();      
    }
  }
  if(comChannel.getLValid() == true ){
    if(lValid == false){
      location = comChannel.getL();
      lValid = true;
    }
    comChannel.setLValid(false);
  }
  if(comChannel.getErrorValid() == true && errorValid == false){
    error1 = comChannel.getError();
    errorValid = true;
  }
}

void stageBDone(){
  cValid = false;
  comChannel._cValid = false;
}
void setCValids(){
  xValid1=yValid1=thetaValid1=rotationAngleValid=true;
  cValid = true; 
} 

//the y motor is attached to timer 2, so attach it's step tracking function to the timer overflow
void yISR(){
  yInterrupted = true;
  yMotor.goToStepInterrupt(); 
}

void xISR(){
  xInterrupted = true;
  xMotor.goToStepInterrupt();
}