#include <Wire.h>
#include <EEPROM.h>
#define SLAVE_ADDRESS 0x08


// pin number declaration

// pins limitswitches
const int limitswitchPin[] = {27,28,29,30,31,32,33,34};
// pins infrared sensor
const int infraredsensorPin[] = {A0,A1};
// pins motor
const int motorDrivingOutPin = 35;
const int motorDrivingPWMPin = 7;

const int motorRotationOutPin = 36;
const int motorRotationPWMPin = 8;

const int motorElevatorOutPin = 37;
const int motorElevatorPWMPin = 9;

const int motorXaxisOutPin = 38;
const int motorXaxisPWMPin = 10;

const int servoSteeringPin = 39;

const int relaiGripperInPin = 40;
const int relaiGripperOutPin = 41;

const int elevatorEncoderDTPin = 3;
const int elevatorEncoderCLKPin = 4;

const int xaxisEncoderDTPin = 5;
const int xaxisEncoderCLKPin = 6;

// define global variables
int distance[5];
bool infrared[2] = {false,false};
int leeway = 20;
bool angled;
String front;
String back;
int currentcase = 0;
const int casesorder[] = {0,10,1,2,3,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,3,2,4,2,3,2,5,6,7,1,8,2,3,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,8,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,8,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,8,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,8,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,3,2,9,7,1,4,2,3,2,5,2,3,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,3,2,4,2,3,2,5,2,3,2,0};
const int elevatorheight[] = {60,50,60,40,80,70,80,40,100,90,100,40,120,110,120,40,140,130,140,40,60,50,60,40,50,40,60,50,60,50,60,40,50,40,60,50,60,50,60,40,50,40,60,50,60,50,60,40,50,40,60,50,60,50,60,40,50,40,60,50,60,40,60,50,60,40,80,70,80,40,100,90,100,40,120,110,120,40,140,130,140};
const int posxaxis[] = {0,1,0,2,0,3,0,4,0,5,0,6,5,0,5,4,0,4,3,0,3,2,0,2,1,0,6,0,5,0,4,0,3,0,2,0,1};//0 is blikjes pakken buiten robot 1-6 zijn opslagplekken 1t/m 6
int nElevatorcase = 0;
int nXaxiscase = 0;
int casescounter = 0;
int elevatorcounter = 0;
int xaxiscounter = 0;
bool stopdriving;
int encoderval;
bool stopprogram = false;
int forwardDriveTime = 3000;//drive time in ms for case 6 (possibly make variable for different time the case is used)
int backwardsDriveTime = 1750;//drive time in ms for case 9 (possibly make variable for different time the case is used)
int infraredThresh1 = 0.4; //als onder deze waarde is valt is de afstand zeker meer dan 30 cm * of minder dan 0.1)

float elevatorlowestpoint = 40;// the lowest point in the elevator without pressing the limitswitch in cm
float elevatordistance = 100; //cm die de lift tussen zijn limitswitches heeft zitten
int targetsXaxis[] = {800,350,300,250,200,150,100}; // encoder values required to aim for when going towards position

int xaxisTotalSteps = 0; // encoder step available in the xaxis
int elevatorTotalSteps = 0; // encoder steps available in the elevetor
// bool rotationstate = false; // mogelijk gebruiken om aan te geven welke stand bastion gedraaid is



void setup()
{
  pinMode(motorDrivingOutPin, OUTPUT);
  pinMode(motorDrivingPWMPin, OUTPUT);
  pinMode(motorRotationOutPin, OUTPUT);
  pinMode(motorRotationPWMPin, OUTPUT);
  pinMode(motorElevatorOutPin, OUTPUT);
  pinMode(motorElevatorPWMPin, OUTPUT);
  pinMode(motorXaxisOutPin, OUTPUT);
  pinMode(motorXaxisPWMPin, OUTPUT);
  pinMode(servoSteeringPin, OUTPUT);
  pinMode(relaiGripperInPin, OUTPUT);
  pinMode(relaiGripperOutPin, OUTPUT);

  digitalWrite(motorDrivingPWMPin,LOW);
  digitalWrite(motorRotationPWMPin,LOW);
  digitalWrite(motorElevatorPWMPin,LOW);
  digitalWrite(motorXaxisPWMPin,LOW);

  pinMode(elevatorEncoderDTPin, INPUT);
  pinMode(elevatorEncoderCLKPin, INPUT);
  pinMode(xaxisEncoderDTPin, INPUT);
  pinMode(xaxisEncoderCLKPin, INPUT);

  //digitalWrite(relaiGripperOutPin,LOW);
  //digitalWrite(relaiGripperInPin,LOW;
  for(int i = 0; i<8;i++)
  {
    pinMode(limitswitchPin[i],INPUT);
  }
  Wire.begin();
  Serial.begin(9600); // Starts the serial communication or bluetooth

  xaxisTotalSteps += EEPROM.read(0) + (EEPROM.read(1) << 8); //read bytes from eeprom and set xaxistotal to value
  elevatorTotalSteps += EEPROM.read(2) + (EEPROM.read(3) << 8);
}



void updateInfrareds()//update infrared values of the robot
{
  for(int i;i<2;i++)
  {
    float irvalue = analogRead(infraredsensorPin[i])*0.0048828125; // voltage wat sensor terug geeft(5/1024)
    if(irvalue < infraredThresh1)
    {
        infrared[i] = true;
    }
    else
    {
      infrared[i] = false;
    }
  }
}



void printvalues()
{
  String distances = "distances: " + (String)distance[0] + " " + (String)distance[1] + " " + (String)distance[2] + " " + (String)distance[3] + " " + (String)distance[4] + "/n";
  String infrareds = "infrareds: " + (String)infrared[0] + " " + (String)infrared[1] + "/n";
  String positioning = "angled: " + (String)angled + " " + front + " " + back + "/n";
  String casesstring = "case: " + (String)currentcase + "/n";
  Serial.println(distances + infrareds + positioning + casesstring);
}

void updateUltrasonics()// updates ultrasonic sensor values from the arduino one chip
{
  Wire.requestFrom(SLAVE_ADDRESS, 10);
  if(Wire.available() >= 10)
  {
    for(int i = 0; i < 5; i++)
    {
      int iRXVal;
      for (int j = 0; j < 2; j++)             // Receive and rebuild the 'int'.
      {
          iRXVal += Wire.read() << (j * 8);   //    "     "     "     "    "
      }
      //  Serial.println(iRXVal);                 // Print the result.
      distance[i] = iRXVal;
    }
  }
}

void steeringforward() //decides how to steer during forward driving
{
  // check if distance is the same for sensors on the same side
  if(distance[0] >= distance[2] - leeway && distance[0] <= distance[2] + leeway && distance[1] >= distance[3] - leeway && distance[1] <= distance[3] + leeway)
  {
    Serial.println("Robot is angled correctly");
    angled = true;
  }
  else
  {
    Serial.println("Robot is angled incorrectly");
    angled = false;
  }

  // check if the front is centerd
  if(distance[0] >= distance[1] - leeway && distance[0] <= distance[1] + leeway)
  {
    front = "centerd";
  }
  else if(distance[0] >= distance[1])
  {
    front = "right side";
  }
  else
  {
    front = "left side";
  }
  //check if the back is sented
      if(distance[2] >= distance[3] - leeway && distance[2] <= distance[3] + leeway)
  {
    back = "centerd";
  }
  else if(distance[2] >= distance[3])
  {
    back = "right side";
  }
  else
  {
    back = "left side";
  }
  //servo aansturen op basis van gegevens
}


void steeringbackwards() //decides how to steer during forward driving
{
  if(distance[0] >= distance[2] - leeway && distance[0] <= distance[2] + leeway && distance[1] >= distance[3] - leeway && distance[1] <= distance[3] + leeway)
  {
    Serial.println("Robot is angled correctly");
    angled = true;
  }
  else
  {
    Serial.println("Robot is angled incorrectly");
    angled = false;
  }

  // check if the front is centerd
  if(distance[0] >= distance[1] - leeway && distance[0] <= distance[1] + leeway)
  {
    front = "centerd";
  }
  else if(distance[0] >= distance[1])
  {
    front = "right side";
  }
  else
  {
    front = "left side";
  }
  //check if the back is sented
      if(distance[2] >= distance[3] - leeway && distance[2] <= distance[3] + leeway)
  {
    back = "centerd";
  }
  else if(distance[2] >= distance[3])
  {
    back = "right side";
  }
  else
  {
    back = "left side";
  }
  //servo aansturen op basis van gegevens
}



void stop(int directionpin,int speedpin)                    //Stop
{
  digitalWrite(speedpin,0);
  digitalWrite(directionpin,LOW);
}
void advance(int directionpin,int speedpin,int speedmotor)          //Move forward
{
  analogWrite (speedpin,speedmotor);      //PWM Speed Control
  digitalWrite(directionpin,HIGH);
}
void back_off(int directionpin,int speedpin,int speedmotor)          //Move backward
{
  analogWrite (speedpin,speedmotor);      //PWM Speed Control
  digitalWrite(directionpin,LOW);
}
void movemotor(int directionpin,int speedpin,int direct,int speedmotor)          //Move forward
{
  analogWrite (speedpin,speedmotor);      //PWM Speed Control
  digitalWrite(directionpin,direct);
}

/*
bool limitswitchCheck(int switchpin)
{
  if(digitalRead(switchpin) == 1)
  {
    //Serial.print("limitswitch met pin ");Serial.print(switchpin);Serial.println(" is ingedrukt");
    return true;
  }
  return false;
}*/


int upcurrentcase() //set robot to next case on the case order
{
  if(casesorder[casescounter] == 2)
  {
    nElevatorcase++;
  }
  else if(casesorder[casescounter] == 3)
  {
    nXaxiscase++;
  }
  casescounter++;
  return casesorder[casescounter-1];
}

void setcasezero() //set robot to case 0 and stops program
{
  casescounter = 0;
  currentcase = 0;
  nElevatorcase = 0;
  nXaxiscase = 0;
}




void case0()//code for running case 0 (wait for button)
{
  delay(1);
  //wait for button press
  //check if robot is in the correct state (elevator low, turned the right way and xaxis in robot)
}





void case1()//code for running case 1: drive backward to the wall
{
  unsigned long startMillis = millis();
  unsigned long threshold1 = startMillis + 500;
  unsigned long threshold2 = startMillis + 1000;
  unsigned long currentMillis;
  int currentspeed;
  int setspeed = 100;
  while(1)
  {
    updateUltrasonics();
    Serial.println(String(distance[0]) + " " + String(distance[1]) + " " + String(distance[2]) + " " + String(distance[3]));
    steeringforward();
    //naar voren rijden door motor aan te zetten (slower == true)
    //bijsturen op basis van ultrasoon sensoren

    currentMillis = millis();

    if(currentMillis>= startMillis + forwardDriveTime)
    {
      break;
    }
    if(currentMillis >= threshold1)
    {
      if(currentMillis >= threshold2)
      {
        setspeed = 255;
      }
      else
      {
        setspeed = 175;
      }
    }
    else
    {
      setspeed = 100;
    }

    if(setspeed != currentspeed)
    {
      back_off(motorDrivingOutPin,motorDrivingPWMPin,setspeed);
      currentspeed = setspeed;
    }
    // encoder uitlezen
    // als encoder bepaalde waarde berijkt aangeven of trager moet of uit moet
    // if(stopdriving == true) {break;}
  }
  stop(motorDrivingOutPin,motorDrivingPWMPin);
}





void case2()//code for running case 2: change elevator position
{
  unsigned long checkMillis = millis();
  unsigned long startMillis = millis();
  unsigned long currentMillis;

  int target = elevatorheight[nElevatorcase];
  int targetcounter = (target-elevatorlowestpoint)/elevatordistance*elevatorTotalSteps;
  int motordirection;
  int encoderval;
  bool startingUp = true;
  int encoderpinlastval = digitalRead(elevatorEncoderCLKPin);
  if (targetcounter > elevatorcounter)
  {
    motordirection = 1;
  }
  else
  {
    motordirection = 0;
  }
  int setspeed = 100;
  int currentspeed = 0;
  while(1)
  {
    currentMillis = millis();
    if(currentMillis >= checkMillis + 100) // elke 100 milisec snelheid update
    {
      if(targetcounter - 5 < elevatorcounter && targetcounter + 5 > elevatorcounter)
      {
        break;
      }
      else if (targetcounter - 50 < elevatorcounter && targetcounter + 50 > elevatorcounter)
      {
        setspeed = 100;
        startingUp = false
      }
      else if (targetcounter - 100 < elevatorcounter && targetcounter + 100 > elevatorcounter)
      {
        if (startingUp = false)
        {
          setspeed = 175;
        }
      }
      else
      {
        /*
        if (startingUp = false)
        {
          setspeed = 255;
        }
        else*/

       if(currentMillis >= startMillis + 1000)
       {
          setspeed = 255;

       }
       else if (currentMillis >= startMillis + 500)
       {
         setspeed = 175;
         startingUp = false;
       }
       else
       {
         setspeed = 100;
       }
      }
      if(setspeed != currentspeed)
      {
        movemotor(motorElevatorOutPin ,motorElevatorPWMPin ,motordirection ,setspeed);
        currentspeed = setspeed;
      }
    }

    encoderval = digitalRead(elevatorEncoderCLKPin);
    if (encoderval != encoderpinlastval)
    {
      if (digitalRead(elevatorEncoderDTPin) != encoderval)
      {
        // Means pin A Changed first - We're Rotating Clockwise
        elevatorcounter ++;
        encoderpinlastval = encoderval;
      }
      else
      {
        // Otherwise B changed first and we're moving counter clock wise
        elevatorcounter--;
        encoderpinlastval = encoderval;
      }
    }
  }
  stop(motorElevatorOutPin ,motorElevatorPWMPin);
}





void case3()//code for running case 3: move Xaxis to predetermined position
{
  delay(10);
  posxaxis[nXaxiscase];
}





void case4()//code for running case 4: close gripper
{
  while(1)
  {
    digitalWrite(relaiGripperInPin,HIGH);

    if(digitalRead(limitswitchPin[6]) == 1)
    {
      digitalWrite(relaiGripperInPin,LOW);
      break;
    }
  }
}





void case5()//code for running case 5: open gripper
{
  while(1)
  {
    digitalWrite(relaiGripperOutPin,HIGH);

    if(digitalRead(limitswitchPin[7]) == 1)
    {
      digitalWrite(relaiGripperOutPin,LOW);
      break;
    }
  }
}

void case6()//code for running case 6: move elevator to predetermined position
{
  unsigned long startMillis = millis();
  unsigned long threshold1 = startMillis + 500;
  unsigned long threshold2 = startMillis + 1000;
  unsigned long currentMillis;
  int currentspeed;
  int setspeed = 100;
  while(1)
  {
    updateUltrasonics();
    Serial.println(String(distance[0]) + " " + String(distance[1]) + " " + String(distance[2]) + " " + String(distance[3]));
    steeringforward();
    //naar voren rijden door motor aan te zetten (slower == true)
    //bijsturen op basis van ultrasoon sensoren

    currentMillis = millis();

    if(currentMillis>= startMillis + forwardDriveTime)
    {
      break;
    }
    if(currentMillis >= threshold1)
    {
      if(currentMillis >= threshold2)
      {
        setspeed = 255;
      }
      else
      {
        setspeed = 175;
      }
    }
    else
    {
      setspeed = 100;
    }

    if(setspeed != currentspeed)
    {
      advance(motorDrivingOutPin,motorDrivingPWMPin,setspeed);
      currentspeed = setspeed;
    }
    // encoder uitlezen
    // als encoder bepaalde waarde berijkt aangeven of trager moet of uit moet
    // if(stopdriving == true) {break;}
  }
  stop(motorDrivingOutPin,motorDrivingPWMPin);
}






void case7()//code for running case 5: turn Bastion 90 degrees
{
  unsigned long startMillis = millis();
  unsigned long threshold1 = startMillis + 500;
  unsigned long threshold2 = startMillis + 1000;
  unsigned long currentMillis;
  int currentspeed = 0;
  int setspeed = 100;

  if(digitalRead(limitswitchPin[0]) == 1)
  {
    while(1)
    {
      currentMillis = millis();
      if(digitalRead(limitswitchPin[1]) == 1)
      {
        break;
      }
      if(currentMillis >= threshold1)
      {
        if(currentMillis >= threshold2)
        {
          setspeed = 255;
        }
        else
        {
          setspeed = 175;
        }
      }
      else
      {
        setspeed = 100;
      }

      if(setspeed != currentspeed)
      {
        advance(motorRotationOutPin,motorRotationPWMPin,setspeed);
        currentspeed = setspeed;
      }
    }
  }
  else//if(digitalRead(switchpin[1]) == 1)
  {
    while(1)
    {
      currentMillis = millis();
      if(digitalRead(limitswitchPin[0]) == 1)
      {
        break;
      }
      if(currentMillis >= threshold1)
      {
        if(currentMillis >= threshold2)
        {
          setspeed = 255;
        }
        else
        {
          setspeed = 175;
        }
      }
      else
      {
        setspeed = 100;
      }

      if(setspeed != currentspeed)
      {
        back_off(motorRotationOutPin,motorRotationPWMPin,setspeed);
        currentspeed = setspeed;
      }
    }
  }
  stop(motorRotationOutPin,motorRotationPWMPin);
}









void case8()//code for running case 8: drive forward to next robot
{

  unsigned long startMillis = millis();
  unsigned long threshold1 = startMillis + 500;
  unsigned long threshold2 = startMillis + 1000;
  unsigned long currentMillis;
  int currentspeed;
  int setspeed = 100;
  bool driveslow = false;
  while(1)
  {
    updateUltrasonics();
    updateInfrareds();
    Serial.println(String(distance[0]) + " " + String(distance[1]) + " " + String(distance[2]) + " " + String(distance[3]));
    steeringforward();
    //naar voren rijden door motor aan te zetten (slower == true)
    //bijsturen op basis van ultrasoon sensoren
    // infrarood sensoren uitlezen
    // if(stopdriving == true) {break;} (possibly add stop if encoder is far overdue)
    currentMillis = millis();

    if(driveslow)
    {
      setspeed = 100;
      if(infrared[1])
      {
        break;
      }
    }
    else
    {
      if(currentMillis >= threshold1)
      {
        if(currentMillis >= threshold2)
        {
          setspeed = 255;
          // pas kijken om zachter te rijden nadat hij volle snelheid rijdt zodat hij niet zijn beginplek gebruikt
          if(infrared[0])
          {
            driveslow = true;
          }
        }
        else
        {
          setspeed = 175;
        }
      }
      else
      {
        setspeed = 100;
      }
    }
    if(setspeed != currentspeed)
    {
      advance(motorDrivingOutPin,motorDrivingPWMPin,setspeed);
      currentspeed = setspeed;
    }
    // encoder uitlezen
    // als encoder bepaalde waarde berijkt aangeven of trager moet of uit moet
    // if(stopdriving == true) {break;}
  }
  stop(motorDrivingOutPin,motorDrivingPWMPin);
}

void case9()//code for running case 9: drive backwards to given location
{
  delay(10);
}

void loop() {
  switch (currentcase)
  {
    case 0://wait for the user input
      case0();
      break;
    case 1://drive forward to next robot
      case1();
      break;
    case 2://drive forward to predetermined position
      case2();
      break;
    case 3:
      case3();
      break;
    case 4:
      case4();
      break;
    case 5:
      case5();
      break;
    case 6:
      case6();
      break;
    case 7:
      case7();
      break;
    case 8:
      case8();
      break;
    case 9:
      case9();
      break;
  }

  printvalues();

  if(stopprogram)
  {
    setcasezero();
  }
  else
  {
    currentcase = upcurrentcase();
  }
}