// MOTOR
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor = AFMS.getMotor(3);
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(4);

  // SPEAKER
#include <SPI.h>
#define NOTE_C4  262 //If I send the value from matlab, I don't need to define it here!
#define NOTE_C6  1047
int Note = 0; //get the value from matlab
int NoteDuration = 0;
int DigiPotValue = 0;
byte address = 0x11;
int CS= 53; //10;
int i=0;

  //  LED STRIPS
#include <Adafruit_NeoPixel.h>
#define LED_COUNT 8
Adafruit_NeoPixel stripFood = Adafruit_NeoPixel(LED_COUNT, 7, NEO_GRB + NEO_KHZ800); //Food Magazine Light
Adafruit_NeoPixel stripHouse = Adafruit_NeoPixel(LED_COUNT, 28, NEO_GRB + NEO_KHZ800); //House Light

int rc; //defining different variables for connection Matlab+Arduino
boolean recvInProgress = false;
int startMarker = 60; //for sending data
int endMarker = 62;
int startMarkerMotor = 40; //for commanding the motor
int endMarkerMotor = 42;
int numChars = 32;
int receivedChars[32];
int index = 0;
int Used = 0;
int NumPellet = 0;
int rewardType = 0;
int motorNum = 0;
boolean newData = false;
int beam1 = 22; //defining the sensors input
int beam2 = 26;
int beam3 = 30;
int beam4 = 33;
int beam5 = 34;
int sensorState1 = 0, lastState1 = 0;  // variable for reading the current and last status of each beam sensor
int sensorState2 = 0, lastState2 = 0;
int sensorState3 = 0, lastState3 = 0;
int sensorState4 = 0, lastState4 = 0;
int sensorState5 = 0, lastState5 = 0;
int data = 0;
int consecutiveTimeout = 0;
boolean TimeoutTrigger = false;
boolean TimeoutBreak = false;
boolean FirstTrial = true;
boolean FoodLightsOn = false;
boolean HabituationTrigger = false;
int Taster = 40;
int Taster2 = 41;
int Wert;
int Wert2;
int FoodDoor = 39;
int sensorStateFood = 0;
int FoodTrigger = false;

void setup() {
  Serial.begin(115200);
  pinMode(12, OUTPUT); //defining output of led 1
  pinMode(11, OUTPUT); //defining output of led 2
  pinMode(45, OUTPUT);  //defining output of led 3
  pinMode(6, OUTPUT);  //defining output of led 4
  pinMode(3, OUTPUT);  //defining output of led 5
  pinMode(CS, OUTPUT); //defining output for CS (digiPot)
  pinMode(Taster,INPUT); //motor taster 1
  pinMode(Taster2,INPUT); //motor taster 2
  pinMode(FoodDoor, INPUT); //food flap door
  pinMode(beam1, INPUT);     // initialize the sensor pin as an input
  pinMode(beam2, INPUT);
  pinMode(beam3, INPUT);
  pinMode(beam4, INPUT);
  pinMode(beam5, INPUT);
  digitalWrite(beam1, HIGH); // turn on the pullup
  digitalWrite(beam2, HIGH);
  digitalWrite(beam3, HIGH);
  digitalWrite(beam4, HIGH);
  digitalWrite(beam5, HIGH);
  digitalWrite(Taster, HIGH);
  digitalWrite(Taster2, HIGH);
  digitalWrite(FoodDoor, HIGH); 

  SPI.begin();
  digitalPotWrite(0x00); // adjust Highest Resistance of digital Potentiometer
  delay(1000);
  digitalPotWrite(0x80); // adjust  wiper in the  Mid point of digiPot
  delay(1000);
  digitalPotWrite(0xFF); // adjust Lowest Resistance of digiPot
  delay(1000);
   
  AFMS.begin();
  myMotor->setSpeed(250); //set motor speed between 0 and 255
  myMotor2->setSpeed(250); //set motor speed between 0 and 255
  
  stripHouse.begin();           // INITIALIZE NeoPixel strip object
  stripHouse.setBrightness(50); // Set BRIGHTNESS to about 1/5 (max = 255)
  stripFood.begin();           // INITIALIZE NeoPixel strip object
  stripFood.setBrightness(50); // Set BRIGHTNESS to about 1/5 (max = 255)
  for(int i=0; i<8; i++) { // For each pixel in strip...
    stripFood.setPixelColor(i,0,0,0); //turned off
    stripHouse.setPixelColor(i,255,0,0); //red color, full brightness
  }
  stripFood.show();
  stripHouse.show();

  digitalWrite(CS,LOW);
  //  send in the address and value via SPI:
  SPI.transfer(address);
  // write out a value
  SPI.transfer(254);
  // take the CS pin high to de-select the chip:
  digitalWrite(CS,HIGH);

  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
}

void loop() {
  recvData();
  sortData();
  readSensor();
  readFoodSensor();
}

//FUNCTIONS

void colorWipe(uint32_t color) {
  for(int i=0; i<8; i++) { // For each pixel in strip...
    stripHouse.setPixelColor(i, color);         //  Set pixel's color
  }
 stripHouse.show(); 
}

void Habituation() {
  if (HabituationTrigger == false) {
    for(int i=0; i<8; i++) { // For each pixel in strip...
        stripFood.setPixelColor(i,127,127,127);         //  Set pixel's color to white, half brightness
        stripHouse.setPixelColor(i,255,0,0);            //  Set pixel's color to red, full brightness
    }
    stripFood.show();
    stripHouse.show();
    for (index = 1; index < Used; index++) {
          analogWrite(12,int(receivedChars[index]));
          analogWrite(11,int(receivedChars[index]));
          analogWrite(45,int(receivedChars[index]));
          analogWrite(6,int(receivedChars[index]));
          analogWrite(3,int(receivedChars[index]));
    }
    HabituationTrigger = true;
  }
  else if (HabituationTrigger == true) {
    for(int i=0; i<8; i++) { // For each pixel in strip...
        stripFood.setPixelColor(i,0,0,0);      //  Turns pixel's color off
        stripHouse.setPixelColor(i,255,0,0);  //red color, full brightness
    }
    stripFood.show();
    stripHouse.show();
    digitalWrite(12, LOW);
    digitalWrite(11, LOW);
    digitalWrite(45, LOW);
    digitalWrite(6, LOW);
    digitalWrite(3, LOW);
    HabituationTrigger = false;
  }
}
    

void recvData() { //function for receiving Data
  if ((Serial.available() > 0) && (newData == false)) {
    rc = Serial.read();
    if ((recvInProgress == true) && (rc == endMarker)){
      recvInProgress = false;
      newData = true;
    }
    else if(recvInProgress == true) {
      receivedChars[Used] = rc;
      Used++;
      if (Used >= numChars) {
        Used = numChars - 1;
      }
    }
    else if (rc == startMarker) {
      recvInProgress = true;
      Used = 0;
    }
  }
}

void sortData() { //check first character to know which function to call
  if (newData == true){
      switch (receivedChars[0]) {
        case 0:
          Habituation();
          break;
        case 1:
          if (TimeoutTrigger == false) {
            setLed();
          }
          break;
        case 2:
          if (TimeoutTrigger == false) {
            FoodLights();
          }
          break;
        case 3:
          if (TimeoutTrigger == false) {
            setMotor();
            break;
          }
        case 4:
          newData = false;
          TimeoutTrigger = true;
          Timeout(); //timeout start
          break;
        case 5:
          if (TimeoutTrigger == true) {
            TimeoutBreak = true; //end of Timeout
          }
        case 6:
          if (TimeoutTrigger == false) {
            setSpeaker();
          }
          break;
        default:
          break;
      }
  newData = false;
  }
}

void setLed() {
    for (index = 1; index < Used; index++) { 
      switch (index) {
        case 1:
          analogWrite(12,int(receivedChars[index]));
          break;
        case 2:
          analogWrite(11,int(receivedChars[index]));
          break;
        case 3:
          analogWrite(45,int(receivedChars[index]));
          break;
        case 4:
          analogWrite(6,int(receivedChars[index]));
          break;
        case 5:
          analogWrite(3,int(receivedChars[index]));
          break;
        default:
          break;
      }
    } 
}

void readSensor() {
  sensorState1 = digitalRead(beam1);   // read the state of the infrared sensor
  sensorState2 = digitalRead(beam2);
  sensorState3 = digitalRead(beam3);
  sensorState4 = digitalRead(beam4);
  sensorState5 = digitalRead(beam5);
  
  if (!sensorState1 && lastState1) { //if beam1 was connected and then broken, send 1
    Serial.write(1);
  }
  if (!sensorState2 && lastState2) { //for beam2
    Serial.write(2);
  }
  if (!sensorState3 && lastState3) { //for beam3
    Serial.write(3);
  }
  if (!sensorState4 && lastState4) { //for beam4
    Serial.write(4);
  }
  if (!sensorState5 && lastState5) { //for beam5
    Serial.write(5);    
  }
  if (sensorState1 && !lastState1) { //closing of beam 1
    Serial.write(6);
  } 
  if (sensorState2 && !lastState2) { //closing of beam 2
    Serial.write(7);
  } 
  if (sensorState3 && !lastState3) { //closing of beam 3
    Serial.write(8);
  } 
  if (sensorState4 && !lastState4) { //closing of beam 4
    Serial.write(9);
  } 
  if (sensorState5 && !lastState5) { //closing of beam 5
    Serial.write(10);
  } 

  lastState1 = sensorState1; //saves the current state as last state
  lastState2 = sensorState2;
  lastState3 = sensorState3;
  lastState4 = sensorState4;
  lastState5 = sensorState5;
}

int digitalPotWrite(int value)
{
  digitalWrite(CS, LOW);
  delay(10);
  SPI.transfer(address);
  SPI.transfer(value);
  delay(10);
  digitalWrite(CS, HIGH);
}

void setSpeaker() {
  for (index = 1; index < Used; index++) {
    switch (index) {
      case 1:
        DigiPotValue = int(receivedChars[index]); //get the resistance value from matlab
        break;
      case 2:
        Note = int(receivedChars[index]); //get the chosen note from Matlab
        break;
      case 3:
        NoteDuration = int(receivedChars[index]); //e.g. 1000/10 = 100ms
        break;
      default:
        break;
    }
  }
  if (Note == 1) {
    digitalPotWrite(DigiPotValue); //set the value for the DigiPot
    tone(10, NOTE_C4, NoteDuration);
    delay(1000);
    noTone(10);
  }
  else if (Note == 2) {
    digitalPotWrite(DigiPotValue); //set the value for the DigiPot
    tone(10, NOTE_C6, NoteDuration);
    delay(1000);
    noTone(10);
  }
}
  
void setMotor() {
 for (index = 1; index < Used; index++) {
  switch (index) {
      case 1:
        NumPellet = int(receivedChars[index]); //get the wanted number of pellets
        break;
      case 2:
        motorNum = int(receivedChars[index]); //get number of motor
        break;
      default:
        break;
  }
 }
  if (motorNum == 1){ //for motor 1
    RunMotor1();
  }
  else if (motorNum == 2){ //for motor 2
    RunMotor2();
  }
}

void readFoodSensor(){
  sensorStateFood = digitalRead(FoodDoor);
  if ((sensorStateFood == LOW) && (FoodTrigger == false)) {
    FoodTrigger = true;
    Serial.write(11); //tell matlab the panel was pushed opened
    delay(50); //delay a bit to avoid bouncing!
  }
  if ((sensorStateFood == HIGH) && (FoodTrigger == true)) {
    Serial.write(12); //tell matlab the panel was closed after being opened
    FoodTrigger = false;
    delay(50); //delay a bit to avoid bouncing!
  }
}

void Timeout(){
  while (TimeoutTrigger == true) {
    colorWipe(stripHouse.Color(63,0,0)); //brightness of Houselight reduced to 63 out of 255
    recvData();
    sortData();
    readSensor(); // important for consecutive Timeouts!
    readFoodSensor();
    if (TimeoutBreak == true) {
      colorWipe(stripHouse.Color(255, 0, 0)); // red color, full brightness
      TimeoutTrigger = false;
      TimeoutBreak = false;
      break;
    }
  }
}

void FoodLights() {
  if (FoodLightsOn == true) {
    for(int i=0; i<8; i++) { // For each pixel in strip...
      stripFood.setPixelColor(i,0,0,0);
      stripFood.show(); 
    }
    FoodLightsOn = false; 
  }
  else if (FoodLightsOn == false) {
    for(int i=0; i<8; i++) { // For each pixel in strip...
      stripFood.setPixelColor(i,255,255,255);
      stripFood.show();
    }
    FoodLightsOn = true;  
  }
}

void RunMotor1() {
  int a = 0; //variable for count numbers of delivered pellets
  boolean tr = false; //to make sure both ifs intercalate!
  while (a < NumPellet) {
    Wert = digitalRead(Taster); //read sensor
    myMotor->run(FORWARD); //run motor
    if ((Wert == LOW) && (tr == false)) { //if closed, no pellet
      tr = true;
    }
    else if ((Wert == HIGH) && (tr == true)) { //if open, pellet
      a++; //sum number of pellets given
      tr = false;
    }
  }
  while (a == NumPellet) { //if number of pellets match the wanted number
    Wert = digitalRead(Taster); //read sensor
    if (Wert == LOW) { //wait until sensor is closed again
      myMotor->run(RELEASE); //stop motor
      a = 0; //stop loop
    }
  }
}

void RunMotor2() {
  int a2 = 0;
  boolean tr2 = false;
  while (a2 < NumPellet) {
    Wert2 = digitalRead(Taster2);
    myMotor2->run(FORWARD);
    if ((Wert2 == LOW) && (tr2 == false)) {
      tr2 = true;
    }
    else if ((Wert2 == HIGH) && (tr2 == true)) {
      a2++;
      tr2 = false;
    }
  }
  while (a2 == NumPellet) {
    Wert2 = digitalRead(Taster2);
    if (Wert2 == LOW) {
      myMotor2->run(RELEASE);
      a2 = 0;
    }
  }
}