#include <Servo.h>
#include <Stepper.h>
#include <Wire.h>
#include <NewPing.h>

#define STEPS 2038  // number of steps in one revolution of motor

Servo tableServo;
Servo robotServo;
Stepper gate(200, 9, 10, 11, 12);  // pins for gate motor

int tableSpeed = 10;  // zde nastavit rychlost servo motoru pro stůl - 1°/ms
int robotSpeed = 10;  // zde nastavit rychlost servo motoru pro robota - 1°/ms

int tableAngle = 0;
int robotAngle = 0;
int randAngle = 0;
int safety = 0;
int tmpGate = 0;

bool tableForward = false;
bool tableBackward = false;

unsigned long tableMillis = 0;
unsigned long robotMillis = 0;
unsigned long sonarMillis = 0;
unsigned long comm_int = 0;

char send[11];
char receive[11];

int dataIn[10] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
int dataOut[10] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

NewPing sonar(7, 8, 200);


void setup() {

  Serial.begin(9600);
  Wire.begin();

  randomSeed(analogRead(0));

  tableServo.attach(6);  // PIN for table data
  robotServo.attach(5);  // PIN for robot data

  robotServo.write(0);  // default set for robot position
  delay(2000);
  tableServo.write(0);  // default set for table position
}


void loop() {

  dataTransfer();
  robot();
  table();
  rollUpDoor();
  lghtscrn();
  signalConnection();

}


//  ******************************************* DATA TRANSFER **********************************************************
void dataTransfer() {
  int a = 0;

  // odešle data každých 50ms a vyžádá data od Slave
  if (millis() - comm_int > 50) {

    for (int i = 0; i < 10; i++) {
      send[i] = (char)dataOut[i] + 1;
    }

    Wire.beginTransmission(10);
    Wire.write(send);
    Wire.endTransmission();

    Wire.requestFrom(10, 11);

    comm_int = millis();
  }

  while (Wire.available() != 0) {                 // pokud něco přišlo
    receive[a] = Wire.read();                     // do pole receive narvi to co je na sběrnici
    if (a < 10) dataIn[a] = (int)receive[a] - 1;  // převod na int
    a++;
  }
}


//  ******************************************* ROBOT **********************************************************
void robot() {

  if (dataIn[1] == 1 && !tableForward && !tableBackward) {
    if (millis() - robotMillis >= robotSpeed) {
      if (robotAngle == randAngle) randAngle = random(1, 57);
      if (robotAngle < randAngle) robotServo.write(++robotAngle);
      if (robotAngle > randAngle) robotServo.write(--robotAngle);

      robotMillis = millis();
    }

  } else {
    if (millis() - robotMillis >= robotSpeed) {
      if (robotAngle > 0) {

        robotServo.write(--robotAngle);
        robotMillis = millis();

      } else {
        robotMillis = 0;
      }
    }
  }
}


//  ******************************************* TABLE ******************************************************
void table() {

  // TABLE FORWARD ROTATION
  if (((dataIn[0] == 1 && tableAngle == 0) || tableForward) && robotAngle == 0 && tmpGate == 0) {
    if (millis() - tableMillis >= tableSpeed && safety) {

      tableForward = true;
      tableServo.write(++tableAngle);
      tableMillis = millis();

      if (tableAngle >= 180) {
        tableForward = false;
        tableMillis = 0;
      }
    }
  }

  // TABLE BACKWARD ROTATION
  if (((dataIn[0] == 0 && tableAngle == 180) || tableBackward) && robotAngle == 0 && tmpGate == 0) {
    if (millis() - tableMillis >= tableSpeed && safety) {

      tableBackward = true;
      tableServo.write(--tableAngle);
      tableMillis = millis();

      if (tableAngle <= 0) {
        tableBackward = false;
        tableMillis = 0;
      }
    }
  }
}


//  ******************************************* GATE **********************************************************
void rollUpDoor() {

  if (!tableForward && !tableBackward) {
    // GATE UP
    if (dataIn[2] == 1 && tmpGate < 17) {
      gate.setSpeed(60);  // 1rpm
      gate.step(-200);     // do 2038 steps in one rotation in one minute
      tmpGate++ ;
    }

    // GATE DOWN
    if (dataIn[2] == 0 && tmpGate > 0) {
      gate.setSpeed(60);  // 1rpm
      gate.step(+200);    // do 2038 steps in one rotation in one minute
      tmpGate-- ;
    }
  }
}

//   ******************************************* SONAR ****************************************************
void lghtscrn() {

  if (millis() - sonarMillis > 50) {  // delay 50ms
    if (sonar.ping_cm() < 7) {       // kdyz bude vzdalenost mensi nez 20
      safety = 0;
    } else {
      safety = 1;
    }
    sonarMillis = millis();
  }
}


//   ******************************************* SIGNAL CONNECTION ****************************************************
void signalConnection() {

  // TABLE
  if (tableAngle == 0) {
    dataOut[0] = 1;
    dataOut[1] = 0;

  } else if (tableAngle == 180) {
    dataOut[0] = 0;
    dataOut[1] = 1;

  } else {
    dataOut[0] = 0;
    dataOut[1] = 0;
  }


  // ROBOT
  if (robotAngle == 0) {
    dataOut[2] = 1;

  } else {
    dataOut[2] = 0;
  }


  // GATE
  if (tmpGate == 17) {
    dataOut[3] = 1;
    dataOut[4] = 0;

  } else if (tmpGate == 0) {
    dataOut[3] = 0;
    dataOut[4] = 1;

  } else {
    dataOut[3] = 0;
    dataOut[4] = 0;
  }


  // SAFETY
  dataOut[5] = safety;
}
