#include <Servo.h>
#include <EEPROM.h>
Servo redTrapDoorServo;
Servo greenTrapDoorServo;
Servo blueTrapDoorServo;
String orderCmd;

#define pinColorMeasure_S0 4
#define pinColorMeasure_S1 5
#define pinColorMeasure_S2 6
#define pinColorMeasure_S3 7
#define pinColorMeasure_Out 8
#define pinRedCoinEnteringCartridge 34
#define pinGreenCoinEnteringCartridge 35
#define pinBlueCoinEnteringCartridge 36
#define pinHallSenzor 3
#define saveLastRedGreenDropperPosition 0
#define saveLastBlueDropperPosition 1
#define saveredCoinsInCartridgeCount 2
#define savegreenCoinsInCartridgeCount 3
#define saveblueCoinsInCartridgeCount 4

enum State
{
  CHECK_CMDS,
  WAITING_FOR_COIN_INPUT,
  ACCEPT_COIN,
  HALL_SENSOR_TEST,
  OPEN_TRAP_DOOR,
  WAIT_FOR_COIN_TO_FALL_IN_CARTRIDGE,
  CLOSE_TRAP_DOOR,
  DROP_UKNOWN_COIN,
  ERROR_COIN_STUCK
};

//int redCnt, greenCnt, blueCnt;
bool positionOfDropperRedGreen, positionOfDropperBlue, hall;

const int pinGateStepperCoil_1 =  22;
const int pinGateStepperCoil_2 =  23;
const int pinGateStepperCoil_3 = 24;
const int pinGateStepperCoil_4 = 25;
const int pinDropper1StepperCoil_1 =  26;
const int pinDropper1StepperCoil_2 =  27;
const int pinDropper1StepperCoil_3 = 28;
const int pinDropper1StepperCoil_4 = 29;
const int pinDropper2StepperCoil_1 = 30;
const int pinDropper2StepperCoil_2 = 31;
const int pinDropper2StepperCoil_3 = 32;
const int pinDropper2StepperCoil_4 = 33;

const int velocity = 1;

unsigned long startTime = 0;

int redCoinEnteringCartridge, greenCoinEnteringCartridge, blueCoinEnteringCartridge, hallSenzor;

State state = CHECK_CMDS;

int redCoinsInCartridge = 0;
int greenCoinsInCartridge = 0;
int blueCoinsInCartridge = 0;

enum Color
{
  RED, GREEN, BLUE, NO_COL, UNKNOWN_COIN_COL
};

Color col;

void setup() {
  pinMode(pinColorMeasure_S0, OUTPUT);
  pinMode(pinColorMeasure_S1, OUTPUT);
  pinMode(pinColorMeasure_S2, OUTPUT);
  pinMode(pinColorMeasure_S3, OUTPUT);
  pinMode(pinColorMeasure_Out, INPUT);

  pinMode(pinDropper1StepperCoil_1, OUTPUT);
  pinMode(pinDropper1StepperCoil_2, OUTPUT);
  pinMode(pinDropper1StepperCoil_3, OUTPUT);
  pinMode(pinDropper1StepperCoil_4, OUTPUT);

  pinMode(pinDropper2StepperCoil_1, OUTPUT);
  pinMode(pinDropper2StepperCoil_2, OUTPUT);
  pinMode(pinDropper2StepperCoil_3, OUTPUT);
  pinMode(pinDropper2StepperCoil_4, OUTPUT);

  digitalWrite(pinColorMeasure_S0, HIGH);
  digitalWrite(pinColorMeasure_S1, LOW);

  
  
  Serial.begin(9600);
  
  redTrapDoorServo.attach(9);
  greenTrapDoorServo.attach(10);
  blueTrapDoorServo.attach(11);
  closeRedTrapDoor();
  closeGreenTrapDoor();
  closeBlueTrapDoor();

  positionOfDropperRedGreen = EEPROM.read(saveLastRedGreenDropperPosition);
  positionOfDropperBlue = EEPROM.read(saveLastBlueDropperPosition);
  redCoinsInCartridge = EEPROM.read(saveredCoinsInCartridgeCount);
  greenCoinsInCartridge = EEPROM.read(savegreenCoinsInCartridgeCount);
  blueCoinsInCartridge = EEPROM.read(saveblueCoinsInCartridgeCount);
  
  pinMode(pinRedCoinEnteringCartridge, INPUT);
  pinMode(pinGreenCoinEnteringCartridge, INPUT);
  pinMode(pinBlueCoinEnteringCartridge, INPUT);
  pinMode(hall, INPUT);

  pinMode(pinGateStepperCoil_1, OUTPUT);
  pinMode(pinGateStepperCoil_2, OUTPUT);
  pinMode(pinGateStepperCoil_3, OUTPUT);
  pinMode(pinGateStepperCoil_4, OUTPUT);

  Serial.setTimeout(10);

}
void loop() {
  redCoinEnteringCartridge = digitalRead (pinRedCoinEnteringCartridge);
  greenCoinEnteringCartridge = digitalRead (pinGreenCoinEnteringCartridge);
  blueCoinEnteringCartridge = digitalRead (pinBlueCoinEnteringCartridge);

  switch(state){

    case CHECK_CMDS:
      delay(500); // have enough time to receive whole message + to restrain the count of messages on Serial line
      if(Serial.available()) {
        orderCmd = Serial.readStringUntil('\n');
        executeCommand();
      }
      state = WAITING_FOR_COIN_INPUT;
      break;
    
    
    case WAITING_FOR_COIN_INPUT: // Rozpoznání mince
      col = getCoinColor();
       switch(col)
      {
        case UNKNOWN_COIN_COL:
          state = DROP_UKNOWN_COIN;
          break;

        case NO_COL:
          state = CHECK_CMDS;
          break;

        case GREEN:
          case RED:
          case BLUE:
            state = ACCEPT_COIN;
            break;
      }
      break;
    case ACCEPT_COIN:
      passCoinThroughGate();
      state =HALL_SENSOR_TEST;
      break;

    case HALL_SENSOR_TEST:
        if (startTime ==0){
          startTime = millis();
        }
          if( (millis() - startTime ) > 5000)
          {
            state = WAITING_FOR_COIN_INPUT;
            startTime = 0;
          }
          
          if( coinVerifiedByHallSensor())
          {
            state = OPEN_TRAP_DOOR;
            startTime = 0;
          }
      break;

      case OPEN_TRAP_DOOR:
        openRelevantTrapDoor(col);
        state = WAIT_FOR_COIN_TO_FALL_IN_CARTRIDGE;
        break;

      case WAIT_FOR_COIN_TO_FALL_IN_CARTRIDGE:
        waitForCoinToFall(col);
        state = CLOSE_TRAP_DOOR;
        break;

      case CLOSE_TRAP_DOOR:
        closeRelevantTrapDoor(col);
        state =CHECK_CMDS;
        break;
      case DROP_UKNOWN_COIN:
        passCoinThroughGate();
        delay(5000);
        state =CHECK_CMDS;
        break;

      case ERROR_COIN_STUCK:
        Serial.println("ERROR");
        break;
  }
  EEPROM.write(saveLastRedGreenDropperPosition, positionOfDropperRedGreen);
  EEPROM.write(saveLastBlueDropperPosition, positionOfDropperBlue);
  EEPROM.write(saveredCoinsInCartridgeCount, redCoinsInCartridge);
  EEPROM.write(savegreenCoinsInCartridgeCount, greenCoinsInCartridge);
  EEPROM.write(saveblueCoinsInCartridgeCount, blueCoinsInCartridge);
}
void waitForCoinToFall(Color col)
{
  switch (col)
         {
            case RED:
              while (redCoinEnteringCartridge){
                redCoinEnteringCartridge = digitalRead (pinRedCoinEnteringCartridge);
              }
              redCoinsInCartridge++;
              break;
            case GREEN:
              while (greenCoinEnteringCartridge){
                greenCoinEnteringCartridge = digitalRead (pinGreenCoinEnteringCartridge);
              }
              break;
              greenCoinsInCartridge++;
            case BLUE:
              while (blueCoinEnteringCartridge){
                blueCoinEnteringCartridge = digitalRead (pinBlueCoinEnteringCartridge);
              }
              blueCoinsInCartridge++;
              break;
         }
}

void closeRelevantTrapDoor(Color col)
{
  switch (col)
  {
            case RED:
              closeRedTrapDoor();
              break;
            case GREEN:
              closeGreenTrapDoor();
              break;
            case BLUE:
              closeBlueTrapDoor();
              break;          
  }
}

void openRelevantTrapDoor(Color col)
{
  switch (col)
        {
          case RED:
            openRedTrapDoor();
            break;

          case GREEN:
            openGreenTrapDoor();
            break;

          case BLUE:
            openBlueTrapDoor();
            break;
        }
}

inline void openGreenTrapDoor()
{
  greenTrapDoorServo.writeMicroseconds(1400);
}

inline void openBlueTrapDoor()
{
 blueTrapDoorServo.writeMicroseconds(1400);
}

inline void openRedTrapDoor()
{
  redTrapDoorServo.writeMicroseconds(1400);
}

inline void closeGreenTrapDoor()
{
  greenTrapDoorServo.writeMicroseconds(500);
}

inline void closeBlueTrapDoor()
{
  blueTrapDoorServo.writeMicroseconds(500);
}

inline void closeRedTrapDoor()
{
  redTrapDoorServo.writeMicroseconds(500);
}


int coinVerifiedByHallSensor()
{
  return digitalRead(pinHallSenzor);
}


Color getCoinColor()
{
    int redMeasured = loadRed();
    int greenMeasured = loadGreen();
    int blueMeasured = loadBlue();
    Color col;
    if (redMeasured > 100 | greenMeasured > 100 | blueMeasured > 100){
    Serial.print(" Žádná mince");
    Serial.println();
    col = NO_COL;
    }
    else{
       if (redMeasured < 50) {
        Serial.print(" | Detekce cervene. ");
        Serial.println();
        col = RED;
        }
       else if (greenMeasured < 60) {
        Serial.print(" | Detekce zelene. ");
        Serial.println();
        col = GREEN;
        }
       else if (blueMeasured < 60) {
        Serial.print(" | Detekce modre. ");
        Serial.println();
        col = BLUE;
        }
       else {
        Serial.print(" | Detekce mince. ");
        Serial.println();
        col = UNKNOWN_COIN_COL;
        }
     }
    
    return col;
}


void rotacePoSmeru(int a, int b, int c, int d) {
  krok1(a,b,c,d);
  krok2(a,b,c,d);
  krok3(a,b,c,d);
  krok4(a,b,c,d);
  krok5(a,b,c,d);
  krok6(a,b,c,d);
  krok7(a,b,c,d);
  krok8(a,b,c,d);
}

void rotaceProtiSmeru(int a, int b, int c, int d) {
  krok8(a,b,c,d);
  krok7(a,b,c,d);
  krok6(a,b,c,d);
  krok5(a,b,c,d);
  krok4(a,b,c,d);
  krok3(a,b,c,d);
  krok2(a,b,c,d);
  krok1(a,b,c,d);
}

void krok1(int a, int b, int c, int d){
  digitalWrite(a, HIGH);
  digitalWrite(b, LOW);
  digitalWrite(c, LOW);
  digitalWrite(d, LOW);
  delay(velocity);
}

void krok2(int a, int b, int c, int d){
  digitalWrite(a, HIGH);
  digitalWrite(b, HIGH);
  digitalWrite(c, LOW);
  digitalWrite(d, LOW);
  delay(velocity);
}

void krok3(int a, int b, int c, int d){
  digitalWrite(a, LOW);
  digitalWrite(b, HIGH);
  digitalWrite(c, LOW);
  digitalWrite(d, LOW);
  delay(velocity);
}

void krok4(int a, int b, int c, int d){
  digitalWrite(a, LOW);
  digitalWrite(b, HIGH);
  digitalWrite(c, HIGH);
  digitalWrite(d, LOW);
  delay(velocity);
}

void krok5(int a, int b, int c, int d){
  digitalWrite(a, LOW);
  digitalWrite(b, LOW);
  digitalWrite(c, HIGH);
  digitalWrite(d, LOW);
  delay(velocity);
}

void krok6(int a, int b, int c, int d){
  digitalWrite(a, LOW);
  digitalWrite(b, LOW);
  digitalWrite(c, HIGH);
  digitalWrite(d, HIGH);
  delay(velocity);
}

void krok7(int a, int b, int c, int d){
  digitalWrite(a, LOW);
  digitalWrite(b, LOW);
  digitalWrite(c, LOW);
  digitalWrite(d, HIGH);
  delay(velocity);
}

void krok8(int a, int b, int c, int d){
  digitalWrite(a, HIGH);
  digitalWrite(b, LOW);
  digitalWrite(c, LOW);
  digitalWrite(d, HIGH);
  delay(velocity);
}

int loadRed(){
  digitalWrite(pinColorMeasure_S2, LOW);
  digitalWrite(pinColorMeasure_S3, LOW);
  delay(50);
  return pulseIn(pinColorMeasure_Out, LOW);
}

int loadGreen(){
  digitalWrite(pinColorMeasure_S2, HIGH);
  digitalWrite(pinColorMeasure_S3, HIGH);
  delay(50);
  return pulseIn(pinColorMeasure_Out, LOW);
}

int loadBlue(){
  digitalWrite(pinColorMeasure_S2, LOW);
  digitalWrite(pinColorMeasure_S3, HIGH);
  delay(50);
  return pulseIn(pinColorMeasure_Out, LOW);
}

void passCoinThroughGate()
{
  for(int i=0;i<(128);i++)
  {
    rotacePoSmeru(pinGateStepperCoil_1, pinGateStepperCoil_2, pinGateStepperCoil_3, pinGateStepperCoil_4);
  }
}

void payoutGreen(bool pos, int cnt){
   while(cnt > 0){
      if(pos){
        for(int i=0;i<(256);i++){
        rotaceProtiSmeru(pinDropper1StepperCoil_1, pinDropper1StepperCoil_2, pinDropper1StepperCoil_3, pinDropper1StepperCoil_4);
      }
        positionOfDropperRedGreen = false;
        cnt--;
        greenCoinsInCartridge--;
      }
      else if(pos==false){
        for(int i=0;i<(256);i++){
        rotacePoSmeru(pinDropper1StepperCoil_1, pinDropper1StepperCoil_2, pinDropper1StepperCoil_3, pinDropper1StepperCoil_4);
        }
        positionOfDropperRedGreen = true;
        cnt--;
        greenCoinsInCartridge--;
      }
      pos = positionOfDropperRedGreen;
   }
}

void payoutRed(bool pos, int cnt){
  while(cnt > 0){
      if(pos){
        for(int i=0;i<(256);i++){
        rotacePoSmeru(pinDropper1StepperCoil_1, pinDropper1StepperCoil_2, pinDropper1StepperCoil_3, pinDropper1StepperCoil_4); 
        }
        positionOfDropperRedGreen = false;
        cnt--;
        redCoinsInCartridge--;
      }
      else if(pos==false){
        for(int i=0;i<(256);i++){
        rotaceProtiSmeru(pinDropper1StepperCoil_1, pinDropper1StepperCoil_2, pinDropper1StepperCoil_3, pinDropper1StepperCoil_4);
        }
        positionOfDropperRedGreen = true;
        cnt--;
        redCoinsInCartridge--;
      }
      pos = positionOfDropperRedGreen;
  }
}

void payoutBlue(bool pos, int cnt){
  while(cnt>0){
      if(pos){
        for(int i=0;i<(256);i++){
        rotacePoSmeru(pinDropper2StepperCoil_1, pinDropper2StepperCoil_2, pinDropper2StepperCoil_3, pinDropper2StepperCoil_4);
      }
      positionOfDropperBlue = false;
      cnt=cnt-1;
      blueCoinsInCartridge--;
      }
      else if(pos == false){
        for(int i=0;i<(256);i++){
        rotaceProtiSmeru(pinDropper2StepperCoil_1, pinDropper2StepperCoil_2, pinDropper2StepperCoil_3, pinDropper2StepperCoil_4);
        }
      positionOfDropperBlue = true;
      cnt=cnt-1;
      blueCoinsInCartridge--;
      }
      pos = positionOfDropperBlue;
  }
}

void executeCommand(){
        int redToPayout;
        int greenToPayout;
        int blueToPayout;
    switch(orderCmd[0]){
      case 'p':
        redToPayout = getRedCoinsFromCmd(orderCmd);
        greenToPayout = getGreenCoinsFromCmd(orderCmd);
        blueToPayout = getBlueCoinsFromCmd(orderCmd);

        if (enoughCoinsToPayout(redToPayout, greenToPayout, blueToPayout))
        {
          payoutRed(positionOfDropperRedGreen, redToPayout);
          payoutGreen(positionOfDropperRedGreen, greenToPayout);
          payoutBlue(positionOfDropperBlue, blueToPayout);
        }
        else {
          Serial.println("Nedostatek mincí");
        }
        break;
       case 'n':
        serialPrintAmountOfCoins(redCoinsInCartridge, greenCoinsInCartridge, blueCoinsInCartridge);
        break;
       case 'z':
        redCoinsInCartridge = 0;
        greenCoinsInCartridge = 0;
        blueCoinsInCartridge = 0;
        break;
       case 'a':
        redToPayout = getRedCoinsFromCmd(orderCmd);
        greenToPayout = getGreenCoinsFromCmd(orderCmd);
        blueToPayout = getBlueCoinsFromCmd(orderCmd);
        redCoinsInCartridge += redToPayout;
        greenCoinsInCartridge += greenToPayout;
        blueCoinsInCartridge += blueToPayout;
        break;
       case 'j':
        Serial.println("JACKPOT");
        payoutRed(positionOfDropperRedGreen, redCoinsInCartridge);
        payoutGreen(positionOfDropperRedGreen, greenCoinsInCartridge);
        payoutBlue(positionOfDropperBlue, blueCoinsInCartridge);
        break;
    }
}

bool enoughCoinsToPayout(int redToPayout, int greenToPayout, int blueToPayout)
{
  return (redCoinsInCartridge >= redToPayout & 
          blueCoinsInCartridge >= greenToPayout & 
          greenCoinsInCartridge >= blueToPayout);
}

int getRedCoinsFromCmd(String command){
        return 10*( command[1] - '0' ) + ( command[2] - '0');
}
int getGreenCoinsFromCmd(String command){
        return 10*( command[3] - '0' ) + ( command[4] - '0');

}
int getBlueCoinsFromCmd(String command){
        return 10*( command[5] - '0' ) + ( command[6] - '0');
}

void serialPrintAmountOfCoins(int r, int g, int b){
 Serial.println();
 Serial.print("Počet červených mincí: ");
 Serial.print(r);
 Serial.print(" | Počet zelených mincí: ");
 Serial.print(g);
 Serial.print(" | Počet modrých mincí: ");
 Serial.print(b);
 Serial.println(); 
}
