//**************************************************************//
//  Name    : ShiftOutLorca_1                                   //
//  Author  : D. Cuartielles, A. Posada, Miguel                 //
//  Date    : 20 Nov, 2007                                      //
//  Version : 3.9.1                                             //
//  Notes   : Code for using a 74HC595 Shift Register           //
//          : to count from 0 to 255                            //
//****************************************************************

/* Revision 3.9
 *
 * What is new?
 * - now we have a funcion called moveMotor(motor, distance) that will
move the motor to a certain location
 * - the estimated maximum distance a motor can move is 5200 units
starting from the back
 * - there is a reset function (code ASCII 40) that will move all the
motors to the back
 * - there is a report of "all motors on the back", consists in
sending 3 times 'r'
 * - if the digital input 'theInputPin' happens to be activated (use
circuit with pull up resistor) it will command a reset
 * - commands 31-36 will move the motors to the center of the stage
independently of where they are
 * - the array motorLocation[] stores the actual position of the motor
 * - the array motorLocationNext[] stores the next position for the motor
 * - MAX_LOCATION is the variable containing the maximum distance a
motor can move
 */

//Pin connected to RESET at MUX Board
int resetPin = 10;
//Pin connected to OUT of MUX Board
int switchIn = 12;
//Pin connected to ST_CP of 74HC595
int latchPin = 10;
//Pin connected to SH_CP of 74HC595
int clockPin = 9;
////Pin connected to DS of 74HC595
int dataPin = 8;

////Address pins for the Analog input
int add[] = {
 7, 6, 5, 4, 3, 2 };

////Reading pin for analog mux
int analIn = 0;

///status for the 32 input pins
///by default they are at '1'
///because they have pull-up resistors
int inPins[] = {
 1, 1, 1, 1, 1, 1, 1, 1,
 1, 1, 1, 1, 1, 1, 1, 1,
 1, 1, 1, 1, 1, 1, 1, 1,
 1, 1, 1, 1, 1, 1, 1, 1
};

////status for the 32 output pins
int pins[] = {
 0, 0, 0, 0, 0,   // motor1 and lamp
 0, 0, 0, 0, 0,   // motor2 and lamp
 0, 0, 0, 0, 0,   // motor3 and lamp
 0,
 0, 0, 0, 0, 0,   // motor4 and lamp
 0, 0, 0, 0, 0,   // motor5 and lamp
 0, 0, 0, 0, 0,   // motor6 and lamp
 0
};

////localisation of the input pins physically
////ALEX: CAMBIA AQUI LAS DIRECCIONES DE LOS PINES DE ENTRADA
////      EN EL ORDEN EN QUE LOS HABEIS MONTADO
////      TODO EL CODIGO ESTA LISTO PARA QUE LO NUMERES DE 1 EN ADELANTE
////      (y no 0, como pasa con los vectores)
int switchesFront[] = {
 12,   // motor1
 13,   // motor2
 14,   // motor3
 15,   // motor4
 16,   // motor5
 17   // motor6
};

int switchesBack[] = {
 31,   // motor1
 32,   // motor2
 1,    // motor3
 26,   // motor4
 27,   // motor5
 28    // motor6
};

int switchesHang[] = {
 30,      // hanging insect 1
 29       // hanging insect 2
};

////positions of the motors in order: A B C D
//// for the lorca project this means: black, brown, yellow, orange
int pins_motors[] = {
 29, 30, 31, 32,   // motor 1
 24, 25, 26, 27,  // motor 2
 18, 19, 20, 22,  // motor 3 *** 22 in 21 because 21 is out
 13, 14 ,15, 16, // motor 4
 8, 9, 10, 11, // motor 5
 3, 4, 5, 6 };   // motor 6

///positions for the lamps
int pins_lamps[] = {
 28, 23, 17, 12, 7, 1
};

////variables for the motors
int MAX_MOTORS = 6;
// 0 means no action, 1 means move forward, 2 means move backward,
// 3 means stopped when going forward, 4 means stopped when going backwards,
// 5 means moving back after stopping in front,
// 6 means moving forward after stopping in back
// 9 means resetting the system
int flags[] = {
 0, 0, 0, 0, 0, 0
};
// flags hang are for the motors taking the hanging insects back and forth
// hanging buttons state
// remember that '1' means not pressed
// we use this to send the command of the button pressed just once
int flagsHang_old [] = {
 0, 0
};
int motor[] = {
 0, 0, 0, 0, 0, 0
};    // counter for the 4 bit number representing the motor state at each time
int motorLocation[] = {
 0, 0, 0, 0, 0, 0
};    // how far each insect is from the front of the scene
int motorLocationNext[] = {
 0, 0, 0, 0, 0, 0
};    // destination for the motor
int motorIndex[] = {
 0, 0, 0, 0, 0, 0
};    // where in the sequence you are for each motor
int MAX_STEP = 4;  // modify this to add more steps to your sequence
int stepSequence[] = {
 12, 6, 3, 9 };

int flagsLamps[] = {
 0, 0, 0, 0, 0, 0
};    // 0 means off, 1 means on, 2 means fade in. 3 means fade out
int lamp[] = {
 0, 0, 0, 0, 0, 0
};    // intensity of light for each lamp

//basic calibration values
int MAX_LOCATION = 5500; //HACKME: change this value for the distance between back and front

/// FUNCTIONS

/// Shiftout32

void shiftOut32() {
 //ground latchPin and hold low for as long as you are transmitting
 digitalWrite(latchPin, LOW);
 for (int j = 0; j < 4; j++) {
   int theByte = 0;
   for (int i = 0; i < 8; i++)
     theByte = theByte*2 + pins[i+j*8];
   shiftOut(dataPin, clockPin, MSBFIRST, theByte);
   //Serial.print(theByte);
   //Serial.print(" ");

   //delayMicroseconds(40);
 }
 //Serial.println();
 //return the latch pin high to signal chip that it
 //no longer needs to listen for information
 digitalWrite(latchPin, HIGH);
 //delayMicroseconds(1000);
}
/// END Shiftout32

///Address the multiplexer
void addressMUX(int address) {
 // assuming the multiplexer is addressed with pins 2-7
 PORTD &= ~(B11111100);  // erase pins
 PORTD |= address<<2;  // write the address
}
///END Address

/// Process Switches
void processSwitches() {
 for (int i = 0; i < 6; i++) {
   if (inPins[switchesFront[i] - 1] == 0 && flags[i] != 5 && flags[i] != 9)
{
     flags[i] = 3;
   }
   if (inPins[switchesBack[i] - 1] == 0 && flags[i] != 6) {
     flags[i] = 4;
   }
 }
 //for (int i = 0; i < 2; i++) {
 //  if (inPins[switchesHang[i] - 1] == 0 && flagsHang_old[i] == 1) {
 //    Serial.print('a' + i);
 //    Serial.print('a' + i);
 //    Serial.print('a' + i);
 //  }
 //  flagsHang_old[i] = inPins[switchesHang[i] - 1];
 //}
}
/// END Process Switches

/// Read all inputs
void readInputs() {
 for (int i = 0; i < 32; i++) {
   //HACKME: add addresses
   addressMUX(i);
   inPins[i] = analogRead(analIn);
   //delay(1);
 }
}
///END Read all inputs

/// Read all inputs as Digital
void readInputsD() {
 for (int i = 0; i < 32; i++) {
   //HACKME: add addresses
   inPins[i] = digitalRead(switchIn);
   addressMUX(i);
   //inPins[i] = analogRead(analIn);
   //if (inPins[i] > 512) inPins[i] = 1;
   //else inPins[i] = 0;
   //Serial.print(inPins[i]);
   delayMicroseconds(50);
 }
 //Serial.println();
}
//END Read all inputs as Digital

///sendInputs as bytes to the serial port
///ends with a CR
void sendInputs() {
 for (int j = 0; j < 4; j++) {
   int theByte = 0;
   for (int i = 0; i < 8; i++)
     theByte = theByte*2 + inPins[i+j*8];
   Serial.print(theByte);
 }
 Serial.println();
 //delay(1);
}
///END sendInputs

///reporting data about motors
void reportMotor(int theMotor, int dir){
 switch (dir) {
 case 0:
   Serial.print("Stopping motor: ");
   break;
 case 1:
   Serial.print("Move forward motor: ");
   break;
 case 2:
   Serial.print("Move backward motor: ");
   break;
 case 3:
   Serial.print("Stop motor in front: ");
   break;
 case 4:
   Serial.print("Stop motor in back: ");
   break;
 case 5:
   Serial.print("Move backward motor: ");
   break;
 case 6:
   Serial.print("Move forward motor: ");
   break;
 default:
   break;
 }
 Serial.println(theMotor + 1);
 Serial.print("-- this motor is connected on: ");
 for (int i = 0; i <4; i++) {
   Serial.print(pins_motors[theMotor*4 + i]);
   Serial.print("  ");
 }
 Serial.println();
}

///END reporting data about motors

///Moving motors
void stopMotor(int theMotor) {
 //reportMotor(theMotor,0);
 flags[theMotor] = 0;
 motor[theMotor] = 0;
}

void stopMotor(int theMotor, int flag) {
 //reportMotor(theMotor,0);
 flags[theMotor] = flag;
 motor[theMotor] = 0;
}

void startMotor(int theMotor, int dir) {
 //possible directions: 1 = forward, 2 = backward
 //depending on the motor's previous state (flag), we will have to
 //choose which is the next one
 if (flags[theMotor] == 0 || flags[theMotor] == 1 || flags[theMotor] == 2)
   flags[theMotor] = dir;  // if the motor was stopped, we will move it first time according to the request
 else if (flags[theMotor] == 3 && dir == 2)
   flags[theMotor] = 5;  // if the motor was stopped in the front, we will move it back only if the directionb is right
 else if (flags[theMotor] == 4 && dir == 1)
   flags[theMotor] = 6;  // if the motor was stopped in the back, we will move it forward only if the direction is right
 //reportMotor(theMotor, flags[theMotor]);
 Serial.print(flags[theMotor]);
 motor[theMotor] = stepSequence[motorIndex[theMotor]];
}
///END moving motors

///Activate Message
void activateMsg(int command){
 switch (command) {
 case 'I':
   //will read all the inputs from the multiplexer
   //and send it back over the serial port as a
   //4-bytes package ending with CR+LF
   readInputsD();
   sendInputs();
   break;
 case 'F':
   //move forward, if followed by a number it will make
   //one of the steppers move forward, if followed by 'A'
   //ir will move all the steppers at once
   command = Serial.read();
   switch (command) {
   case '1':
     moveMotor(0, MAX_LOCATION);
     break;
   case '2':
     moveMotor(1, MAX_LOCATION);
     break;
   case '3':
     moveMotor(2, MAX_LOCATION);
     break;
   case '4':
     moveMotor(3, MAX_LOCATION);
     break;
   case '5':
     moveMotor(4, MAX_LOCATION);
     break;
   case '6':
     moveMotor(5, MAX_LOCATION);
     break;
   case 'A':
     for(int i = 0; i < 6; i++){
       moveMotor(i, MAX_LOCATION);
     }
     break;
   default:
     break;
   }
   break;
 case 'B':
   //move backward, if followed by a number it will make
   //one of the steppers move backward, if followed by 'A'
   //ir will move all the steppers at once
   command = Serial.read();
   switch (command) {
   case '1':
     moveMotor(0, 0);
     break;
   case '2':
     moveMotor(1, 0);
     break;
   case '3':
     moveMotor(2, 0);
     break;
   case '4':
     moveMotor(3, 0);
     break;
   case '5':
     moveMotor(4, 0);
     break;
   case '6':
     moveMotor(5, 0);
     break;
   case 'A':
     for(int i = 0; i < 6; i++) {
       moveMotor(i,0);
     }
     break;
   default:
     break;
   }
   break;
 case 'L':
   //turn on lights, if followed by a number it will make
   //one of the lamps light up, if followed by 'A'
   //ir will activate all the lamps at full blast at once
   command = Serial.read();
   switch (command) {
   case '1':
     flagsLamps[0] = 1;
     break;
   case '2':
     flagsLamps[1] = 1;
     break;
   case '3':
     flagsLamps[2] = 1;
     break;
   case '4':
     flagsLamps[3] = 1;
     break;
   case '5':
     flagsLamps[4] = 1;
     break;
   case '6':
     flagsLamps[5] = 1;
     break;
   case 'A':
     for(int i = 0; i < 6; i++)
       flagsLamps[i] = 1;
     break;
   default:
     break;
   }
   break;
 case 'O':
   //turn off lights, if followed by a number it will make
   //one of the lamps light off, if followed by 'A'
   //ir will turn all the lights off once
   command = Serial.read();
   switch (command) {
   case '1':
     flagsLamps[0] = 0;
     break;
   case '2':
     flagsLamps[1] = 0;
     break;
   case '3':
     flagsLamps[2] = 0;
     break;
   case '4':
     flagsLamps[3] = 0;
     break;
   case '5':
     flagsLamps[4] = 0;
     break;
   case '6':
     flagsLamps[5] = 0;
     break;
   case 'A':
     for(int i = 0; i < 6; i++)
       flagsLamps[i] = 0;
     break;
   default:
     break;
   }
   break;
 case 'S':
   //stop every motor, independently of where they are
   for(int i = 0; i < 6; i++)
     stopMotor(i);
   break;
 case 'C':
   //lets do the calibration using e.g. motor 6
   //calibration(6);
   Serial.println(MAX_LOCATION);
   break;
 case 'M':
   // moving the objects to half way only
   command = Serial.read();
   switch (command) {
   case '1':
     break;
   case '2':
     break;
   case '3':
     break;
   case '4':
     break;
   case '5':
     break;
   case '6':
     break;
   case 'A':
     for(int i = 0; i < 6; i++)
       break;
   default:
     break;
   }
   break;
 default:
   break;
 };
}
///END Activate Message

///Activate Message 1 byte
void activateMsg1Byte(int command){
 switch (command) {
 case 0:
   //will read all the inputs from the multiplexer
   //and send it back over the serial port as a
   //4-bytes package ending with CR+LF
   readInputsD();
   //sendInputs();
   break;
 case 1:
   //stop every motor, independently of where they are
   for(int i = 0; i < 6; i++)
     stopMotor(i);
   break;
 case 2:
     moveMotor(0, MAX_LOCATION);
   break;
 case 3:
     moveMotor(1, MAX_LOCATION);
   break;
 case 4:
     moveMotor(2, MAX_LOCATION);
   break;
 case 5:
     moveMotor(3, MAX_LOCATION);
   break;
 case 6:
     moveMotor(4, MAX_LOCATION);
   break;
 case 7:
     moveMotor(5, MAX_LOCATION);
   break;
 case 8:
   for(int i = 0; i < 6; i++) {
     moveMotor(i, MAX_LOCATION);
   }
   break;
 case 9:
   moveMotor(0, 0);
   break;
 case 10:
     moveMotor(1, 0);
   break;
 case 11:
     moveMotor(2, 0);
   break;
 case 12:
     moveMotor(3, 0);
   break;
 case 13:
     moveMotor(4,0);
   break;
 case 14:
     moveMotor(5, 0);
   break;
 case 15:
   for(int i = 0; i < 6; i++) {
     moveMotor(i, 0);
 }
   break;
 case 16:
   flagsLamps[0] = 1;
   break;
 case 17:
   flagsLamps[1] = 1;
   break;
 case 18:
   flagsLamps[2] = 1;
   break;
 case 19:
   flagsLamps[3] = 1;
   break;
 case 20:
   flagsLamps[4] = 1;
   break;
 case 21:
   flagsLamps[5] = 1;
   break;
 case 22:
   for(int i = 0; i < 6; i++)
     flagsLamps[i] = 1;
   break;
 case 23:
   flagsLamps[0] = 0;
   break;
 case 24:
   flagsLamps[1] = 0;
   break;
 case 25:
   flagsLamps[2] = 0;
   break;
 case 26:
   flagsLamps[3] = 0;
   break;
 case 27:
   flagsLamps[4] = 0;
   break;
 case 28:
   flagsLamps[5] = 0;
   break;
 case 29:
   for(int i = 0; i < 6; i++)
     flagsLamps[i] = 0;
   break;
 case 30:
   //lets do the calibration using e.g. motor 6
   //MAX_LOCATION = calibration(6);
   Serial.println(MAX_LOCATION);
   break;
 case 31:
   //move motor 1 to the center from the back
   moveMotor(0,MAX_LOCATION/2);
   break;
 case 32:
   //move motor 2 to the center from the back
   moveMotor(1,MAX_LOCATION/2);
   break;
 case 33:
   //move motor 3 to the center from the back
   moveMotor(2,MAX_LOCATION/2);
   break;
 case 34:
   //move motor 4 to the center from the back
   moveMotor(3,MAX_LOCATION/2);
   break;
 case 35:
   //move motor 5 to the center from the back
   moveMotor(4,MAX_LOCATION/2);
   break;
 case 36:
   //move motor 6 to the center from the back
   moveMotor(5,MAX_LOCATION/2);
   break;
 case 37:
   for(int i = 0; i < 6; i++)
     moveMotor(i,MAX_LOCATION/2);
   break;
 case 40:
   //reset the system
   reset();
 default:
   break;
 };
}
///END Activate Message

///package data
///puts data in the right configuration before sending it through the port
void packageData() {
 for (int j = 0; j < MAX_MOTORS; j++) {
   pins[32 - pins_motors[j*4]] = (motor[j]) & B1;
   pins[32 - pins_motors[j*4+1]] = (motor[j]/2) & B1;
   pins[32 - pins_motors[j*4+2]] = (motor[j]/4) & B1;
   pins[32 - pins_motors[j*4+3]] = (motor[j]/8) & B1;
   //HACKME: correct for the correct functionality of fades
   if (lamp[j] > 512)
     pins[32 - pins_lamps[j]] = 1;
   else
     pins[32 - pins_lamps[j]] = 0;
 }
}
///END package data

///moving motors
void moveMotor(int theMotor, int location) {
 motorLocationNext[theMotor] = location;
 if (motorLocation[theMotor] < motorLocationNext[theMotor]) {
   startMotor(theMotor, 1);
 } else if (motorLocation[theMotor] > motorLocationNext[theMotor]) {
   startMotor(theMotor, 2);
 }
}

void moveForward(int i) {
 // move the motor forward
 motor[i] = stepSequence[motorIndex[i]];
 //Serial.println("motor advancing");
 motorLocation[i]++;
 motorIndex[i]++;
 if (motorIndex[i] >= MAX_STEP) motorIndex[i] = 0;
 if (motorLocation[i] >= MAX_LOCATION) motorLocation[i] = MAX_LOCATION;
 if (motorLocation[i] == motorLocationNext[i]) flags[i] = 0;
}

void moveBackward(int i) {
 //move the motor backward
 motor[i] = stepSequence[motorIndex[i]];
 motorLocation[i]--;
 motorIndex[i]--;
 if (motorIndex[i] < 0) motorIndex[i] = MAX_STEP-1;
 if (motorLocation[i] < 0) motorLocation[i] = 0;
 if (motorLocation[i] == motorLocationNext[i] && flags[i] != 9) flags[i] =
0;
}

void reset() {
 for(int i = 0; i < MAX_MOTORS; i++) {
   flags[i] = 9;
 }
}
///END moving motors

/// END FUNCTIONS

void setup() {
 //set pins to output because they are addressed in the main loop
 pinMode(latchPin, OUTPUT);
 pinMode(clockPin, OUTPUT);
 pinMode(dataPin, OUTPUT);
 pinMode(switchIn, INPUT);

 //declare the address pins for the analog multiplexer
 for (int i = 0; i < 6; i++)
   pinMode(add[i], OUTPUT);

 // initiate Serial port
 Serial.begin(9600);

 // make status report
 // sends ABC so that it is easy to read on MAX
 Serial.print('A');
 delay(500);
 Serial.print('B');
 delay(500);
 Serial.print('C');
}

void loop() {

 //if (digitalRead(theInputPin) == LOW) reset();
 //Serial.println(flags[5]);

 //count up routine
 if (Serial.available() > 0) {
   int msg = Serial.read();
   //Serial.println(msg);
   //activateMsg(msg);
   activateMsg1Byte(msg);
 }

 readInputsD();              /// ReadInputsD takes exactly 1500microseconds,
 /// and therefore we dont need to make the delay by the end of the program
 processSwitches();      /// will go through the inPin array and checks whether one of them
 /// is active to send the right command to the motor later

 /// if the system has been reseted, we send the computer a command indicatintg so
 if (inPins[resetPin-1] == 0) {
   // this means the reset button has been activated
   Serial.print(99,BYTE);
   Serial.print(99,BYTE);
   Serial.print(99,BYTE);
   delay(3000);
//  }
//  if (isReset()) {
//    Serial.print('r');
//    Serial.print('r');
//    Serial.print('r');
  // for (int i = 0; i < MAX_MOTORS; i++) {
  //   motorLocation[i] = 0;
  // }
   reset();
 }

 for(int i = 0; i < MAX_MOTORS; i++) {
   if (flags[i] == 1) {
     moveForward(i);
   }
   else if (flags[i] == 2) {
     moveBackward(i);
   }
   else if (flags[i] == 3) {
     //stop the motor with special flag
     motorLocation[i] = MAX_LOCATION;
     stopMotor(i, 3);
   }
   else if (flags[i] == 4) {
     //stop the motor with special flag
     motorLocation[i] = 0;
     stopMotor(i, 4);
   }
   else if (flags[i] == 5) {
     moveBackward(i);
   }
   else if (flags[i] == 6) {
     moveForward(i);
   }
   else if (flags[i] == 9) {
     motorLocation[i] = MAX_LOCATION;
     moveBackward(i);
   }

   ///take care of the lamps as well
   if (flagsLamps[i] == 1) {
     lamp[i] = 1023;
   }
   else if (flagsLamps[i] == 0) {
     lamp[i] = 0;
   }

 }
 packageData();
 shiftOut32();
 //delayMicroseconds(1400);  ///NOTE: this value is setting the speed of the system!!
 ///      since now we read all the inputs further up in the
 ///      code, we dont need to make this delay, because reading takes
 ///      the same time!!
}