#include <EEPROM.h>
#include <avr/wdt.h>

//INPUTS & OUTPUTS-----------------------------
int RAKE_DOWN = A5; //NO
int ON_SWITCHES = A4; //NO
int BLACKOUT = A3; //NC
int SECONDBALL = A2; //NC
int RESET_BUTTONS = A1; //NO
int PHOTO_EYE = A0; //NO
int CC_SWITCH = 0; //NO
int COUNTER_SWITCH = 1; //NO
int DECK_JAM_SWITCH = 2; //NC
int TURRET_JAM_SWITCH = 3; //NC
int AUX_CONTACTOR = 4;
int MOTOR_CONTACTOR = 5;
int SOLENOID = 6;
int SCORE_PINS = 7;
int MAGNET_OUT = 8;
int SECOND_BALL_OUT = 9;
int clock = 10;
int data = 11;
int latch = 12;

//TIMERS---------------------------------
byte ballSenseFlickerValue = 20; //Debounce ball sensing - in MILLISECONDS
int clutchTimerValue = 2200; //Clutch timer to stop pinwheel if pins are not moving up the CC - in MILLISECONDS
int cycleTimerStartOverValue = 100; //Cycle didn't take, go back to 0 - in MILLISECONDS
int machineNotCycling = 30000; //If rake is down way too long, actuate again (90 stop detection, 180 will do nothing) - in MILLISECONDS
byte machineOnCountValue = 25; //Debounce on switches - in FRAMES
int motorTimerValue = 2000; //Extra time for motor to run after pin count is satisfied - in MILLISECONDS
byte pinCountValue = 10; //Pins to move into turret before turning motor off - COUNT
int pinTimerValue = 4000; //Time sensed on CC to determine pins aren't moving up the conveyor - in MILLISECONDS
int rakeDownTimeOut = 3000; //After solenoid actuation, if rake is not detected down, reset to 0 - in MILLISECONDS (rake should take ~1 second after actuation to drop, will operate machine in a limp mode if rake sensor doesn't work)
int timeDelay1 = 4000; //Functional Time Delay first ball - in MILLISECONDS
int timeDelay2 = 200; //Functional Time Delay seconds ball - in MILLISECONDS
int solenoidTime = 400; //Functional time to actuate solenoid on cycle start - in MILLISECONDS
byte startCycleDebounceCount = 10; //Debounce reset buttons & rake down at start of cycle - in FRAMES
int startupTimeValue = 1000; //Start up delay to power photo eyes before running cycle code - in MILLISECONDS (prevents instant cycle on machine start up)

//VALUES--------------------------------
//int pinsInMachine = 19; //NOT USED YET
int sensorValueA = 150; //Value to consider sensor tripped - out of 1023
//int sensorValueB = 300; //NOT USED YET
//int sensorValueC = 450; //NOT USED YET
//int sensorValueD = 600; //NOT USED YET
//int sensorValueE = 750; //NOT USED YET

//VARIABLES--------------------------------
int ballTime = 0;
bool ccSwitch = 0;
unsigned long clutchTimer = 0;
bool cycleActive = 0;
int cycleCount = 0;
bool cycleCountSwitch = 0;
unsigned long cycleTimer = 0;
bool deckJamSignal = 0;
bool forceReset = 0;
byte i;
bool machineOn = 0;
byte machineOnCount = 0;
bool machineOnInit = 0;
bool motorOn = 0;
bool motorSwitch = 0;
unsigned long motorTimer = 0;
byte numberToDisplay = 0;
byte pinCount = 0;
bool pinsMoving = 0;
unsigned long pinTimer = 0;
byte rakeDebounce = 0;
bool rakeDown = 0;
byte resetButtonDebounce = 0;
byte resetSequence = 0;
bool secondBallSignal = 0;
int sensorValue = 0;
bool solenoidSignal = 0;
bool startUpSwitch = 0;
unsigned long startUpTimer = 0;
bool timeDelayActive = 0;
bool turretJamSignal = 0;


//-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
void setup() {
  wdt_disable();
  Serial.begin(9600); //SERIAL DEBUG ONLY
  pinMode(PHOTO_EYE, INPUT); //Photo Eye Analog
  pinMode(RESET_BUTTONS, INPUT); //Reset Buttons Circuit
  pinMode(SECONDBALL, INPUT); //2nd Ball Switch
  pinMode(BLACKOUT, INPUT); //Blackout Circuit
  pinMode(ON_SWITCHES, INPUT); //On Switch Circuit
  pinMode(RAKE_DOWN, INPUT); //Rake Down Sensor
  pinMode(CC_SWITCH, INPUT); //Cross Conveyor Switch
  pinMode(COUNTER_SWITCH, INPUT); //Cycle Counter Switch
  pinMode(DECK_JAM_SWITCH, INPUT); //Deck Jam Switch
  pinMode(TURRET_JAM_SWITCH, INPUT); //Turret Jam Switch
  pinMode(AUX_CONTACTOR, OUTPUT); //Aux Contactor (Pin Light, ACC Motor, Ball Return Motor) (Always On)
  pinMode(MOTOR_CONTACTOR, OUTPUT); //Motor Contactor (Motor Only) (Stand By Logic)
  pinMode(SOLENOID, OUTPUT); //Solenoid (Cycle Machine)
  pinMode(SCORE_PINS, OUTPUT); //Score Pins (Take Picture)
  pinMode(MAGNET_OUT, OUTPUT); //Magnet On
  pinMode(SECOND_BALL_OUT, OUTPUT);  //2ndB Out
  pinMode(clock, OUTPUT);  //clk
  pinMode(data, OUTPUT); //data
  pinMode(latch, OUTPUT); //latch
  EEPROM.get(0,cycleCount); //INT Address 0x000 to 0x001 out of 0x3FF (1024 bytes)
  EEPROM.get(4,resetSequence); //INT Address 0x004
  wdt_enable(WDTO_120MS);
}


//-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
void loop() {
wdt_reset();

//SERIAL DEBUG ONLY-------------------------------------------------------------------------------------------------------------------------------------------
Serial.print(resetSequence);
Serial.println("");


//IO processing------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
deckJamSignal = !digitalRead(DECK_JAM_SWITCH);
turretJamSignal = digitalRead(DECK_JAM_SWITCH) & !digitalRead(TURRET_JAM_SWITCH);

forceReset = digitalRead(RESET_BUTTONS);
rakeDown = digitalRead(RAKE_DOWN);
secondBallSignal = digitalRead(SECONDBALL);
sensorValue = analogRead(PHOTO_EYE);
solenoidSignal = digitalRead(SOLENOID);

digitalWrite(SECOND_BALL_OUT, secondBallSignal);
digitalWrite(AUX_CONTACTOR, machineOn);


//STARTUP------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
if (digitalRead(ON_SWITCHES) == 1 && digitalRead(DECK_JAM_SWITCH) == 1 && digitalRead(TURRET_JAM_SWITCH) == 1 && digitalRead(BLACKOUT) == 1) machineOnInit = 1; //Machine On
else
{
  if (machineOn == 1)
  {
    EEPROM.put(4,resetSequence);
    delay(100000); //must be longer than WDT
  }
  startUpTimer = millis();
  digitalWrite(SOLENOID, LOW);
  digitalWrite(SCORE_PINS, LOW);
}

if (machineOnInit == 1 && machineOnCount < machineOnCountValue) machineOnCount++;
if (machineOnCount >= machineOnCountValue) machineOn = 1;
if (startUpSwitch == 0 && millis() - startUpTimer > startupTimeValue) startUpSwitch = 1;


//STATUS LEDS BINARY-----------------------------------------------------------------------------------------------------------------------------------------------
//numberToDisplay = machineOn*pow(2,0) + secondBallSignal*pow(2,1) + cycleActive*pow(2,2) + solenoidSignal*pow(2,3) + timeDelayActive*pow(2,4) + deckJamSignal*pow(2,5) + turretJamSignal*pow(2,6);
numberToDisplay = turretJamSignal << 6 | deckJamSignal << 5 | timeDelayActive << 4 | solenoidSignal << 3 | cycleActive << 2 | secondBallSignal << 1 | machineOn;

  digitalWrite(latch, LOW);
  shiftOut(data, clock, MSBFIRST, numberToDisplay);
  digitalWrite(latch, HIGH);


//CYCLE MACHINE-----------------------------------------------------------------------------------------------------------------------------------------------------
if (startUpSwitch == 1)
{
  switch (resetSequence)
  {
    case 0: //WAITING WITH MACHINE IN HOME POS (or recover from power outage)------------------------------------------------------------------
      digitalWrite(SOLENOID, LOW);
      digitalWrite(SCORE_PINS, LOW);
      ballTime = 0;
      cycleActive = 0;
      resetButtonDebounce = 0;
      rakeDebounce = 0;
      if (sensorValue < sensorValueA)
      {
        cycleTimer = millis();
        resetSequence++;
      }
      else if (forceReset == 1)
      {
          resetSequence++;
          cycleTimer = millis();
      }
      else if (rakeDown == 1)
      {
        resetSequence++;
        cycleTimer = millis();
      }
    break;

    case 1: //Ball Sensing (add to time delay based on ball speed)----------------------------------------------------------------------------
      if (sensorValue < sensorValueA && millis() - cycleTimer > ballSenseFlickerValue) //Ball Sensed
      {
        if (secondBallSignal == 0) pinCount = 0;
        ballTime = millis() - cycleTimer;
        cycleTimer = millis();
        resetSequence++;
      }
      else if (sensorValue > sensorValueA && forceReset == 0 && rakeDown == 0) // No Read / flicker / debounce
      {
        resetSequence--;
        cycleTimer = millis();
      }
      else if (forceReset == 1) //debounce reset button
      {
        resetButtonDebounce++;
        if (resetButtonDebounce > startCycleDebounceCount)
        {
        if (secondBallSignal == 0) pinCount = 0;
          resetSequence = 4;
          cycleTimer = millis();
        }
      }
      else if (rakeDown == 1) //debounce rake down
      {
        rakeDebounce++;
        if (rakeDebounce > startCycleDebounceCount)
        {
        if (secondBallSignal == 0) pinCount = 0;
          resetSequence++;
          cycleTimer = millis();
        }
      }
      else if (millis() - cycleTimer > cycleTimerStartOverValue) // No Read / delay too long
      {
        resetSequence--;
        cycleTimer = millis();
      }
    break;

    case 2: //Time Delay-------------------------------------------------------------------------------------------------------------------------
    timeDelayActive = 1;
      if ((millis() - cycleTimer > (timeDelay1+ballTime) && secondBallSignal == 0) || (millis() - cycleTimer > (timeDelay2+ballTime) && secondBallSignal == 1))
      {
        resetSequence = 3;
        cycleTimer = millis();
      }
    break;

    case 3: //Power Solenoid Score Pins------------------------------------------------------------------------------------------------------------
    timeDelayActive = 0;
      digitalWrite(SOLENOID, HIGH);
      digitalWrite(SCORE_PINS, HIGH);
      if (millis() - cycleTimer > solenoidTime)
      {
        resetSequence = 5;
        cycleTimer = millis();
      }
    break;

    case 4: //Power Solenoid DONT Score Pins------------------------------------------------------------------------------------------------------------
      digitalWrite(SOLENOID, HIGH);
      if (millis() - cycleTimer > solenoidTime)
      {
        resetSequence++;
        cycleTimer = millis();
      }
    break;

    case 5: //Wait for Rake Down-----------------------------------------------------------------------------------------------------------------------
      digitalWrite(SOLENOID, LOW);
      if (rakeDown == 1)
      {
        cycleTimer = millis();
        resetSequence++;
      }
      else if (millis() - cycleTimer > rakeDownTimeOut)
      {
        cycleTimer = millis();
        resetSequence = 0;
      }
    break;

    case 6: //Wait for Rake Back Up-------------------------------------------------------------------------------------------------------------------
      cycleActive = 1;
      if (pinsMoving == 0) cycleTimer = millis(); //180 detection
      if (rakeDown == 0) //See if machine is cycling, if not cycle machine again to prevent 90 stop (wait after rake down sensed or power outage occured)
      {
        cycleTimer = millis();
        resetSequence++;
      }
      else if (millis() - cycleTimer > machineNotCycling)
      {
        cycleTimer = millis();
        resetSequence = 4;
      }
    break;

    case 7: //Finish and debounce---------------------------------------------------------------------------------------------------------------------
      if (rakeDown == 0 && millis() - cycleTimer > 500) //Wait for rake to be all the way up to allow another cycle
      {
        resetSequence = 0;
        cycleTimer = millis();
      }
      else if (rakeDown == 1)
      {
        resetSequence = 6;
        cycleTimer = millis();
      }
    break;
  }
}


//Pin counting on CC----------------------------------------------------------------------------------------------------------------------------------------
if (millis() - pinTimer > pinTimerValue) pinsMoving = 0;
else pinsMoving = 1;
switch (ccSwitch)
{
  case 0:
    if (digitalRead(CC_SWITCH) == 1)
    {
      pinTimer = millis();
      ccSwitch = 1;
      pinCount++;
    }
  break;
  case 1:
    if (digitalRead(CC_SWITCH) == 0)
    {
      pinTimer = millis();
      ccSwitch = 0;
    }
  break;
}


//Cycle Count-----------------------------------------------------------------------------------------------------------------------------------------------
switch (cycleCountSwitch)
{
  case 0:
    if (digitalRead(COUNTER_SWITCH) == 1)
    {
      cycleCountSwitch = 1;
      cycleCount++;
      EEPROM.put(0,cycleCount);
    }
  break;
  case 1:
    if (digitalRead(COUNTER_SWITCH) == 0)
    {
      cycleCountSwitch = 0;
    }
  break;
}


//Magnet Clutch CONTROL-------------------------------------------------------------------------------------------------------------------------------------
if (millis() - clutchTimer > clutchTimerValue || motorOn == 0) digitalWrite(MAGNET_OUT, LOW);
else digitalWrite(MAGNET_OUT, HIGH);

if (digitalRead(CC_SWITCH) == 1) clutchTimer = millis();


//MOTOR STANDBY CONTROL-----------------------------------------------------------------------------------------------------------------------------------------------------
switch (motorSwitch)
{
  case 0:
    digitalWrite(MOTOR_CONTACTOR, HIGH);
    motorOn = 1;
    if (rakeDown == 1 || pinCount < pinCountValue) motorTimer = millis();
    if ((pinCount >= pinCountValue && resetSequence == 0 && millis() - motorTimer > motorTimerValue) || machineOn == 0) motorSwitch = 1;
  break;
  case 1:
    digitalWrite(MOTOR_CONTACTOR, LOW);
    motorOn = 0;
    if ((resetSequence != 0 || pinCount < pinCountValue) && machineOn == 1)
    {
      motorSwitch = 0;
      motorTimer = millis();
    }
  break;
}


}