//timer interrupt calculator: http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html
//better one here: http://www.instructables.com/id/Arduino-Timer-Interrupts/ (but don't use the cli/sei commands)
//pins D0 and D1 are reserved for hard serial RX/TX (respectively)

const int sensorTics = 4;
const int warningTics = 8;

//pin assignments for the inputs of the 4 sensors
const int S1 = 2;  //sensor 1 input (fresh wood)
const int S2 = 3;  //sensor 2 input (start/stop cutting wood)
const int S3 = 4;  //sensor 3 input (start painting wood)
const int S4 = 5;  //sensor 4 input (stop painting wood)

//pin assignments for the outputs
const int motor_out = 6; //DC motor control out
const int threeLEDs_out = 7;  //3 LEDs for the "painting" portion
const int warningLED_out = 8;  //Light for notification of fresh wood

//whether or not this is the first time on (since I reuse S2 for two different actions)
bool startup = true;
//whether the motor is on, so I know which  stage of S2 I am currently in
bool motorOn = false;

//counters to count the Timer1 tics
volatile int S1counter = 0;
volatile int S2counter = 0;
volatile int S3counter = 0;
volatile int S4counter = 0;
volatile int warningCounter = 0;

//whether that particular stage is waiting on the interrupts to occur
volatile bool S1int = false;
volatile bool S2int = false;
volatile bool S3int = false;
volatile bool S4int = false;
volatile bool warningInt = false;

void setup() 
{
  pinMode(S1, INPUT_PULLUP); // sets the digital pin 2 as input
  pinMode(S2, INPUT_PULLUP); // sets the digital pin 3 as input
  pinMode(S3, INPUT_PULLUP); // sets the digital pin 4 as input
  pinMode(S4, INPUT_PULLUP); // sets the digital pin 5 as input

  pinMode(motor_out, OUTPUT);      // sets the digital pin 6 as an output
  digitalWrite(motor_out, LOW);
  pinMode(threeLEDs_out, OUTPUT);      // sets the digital pin 7 as an output
  digitalWrite(threeLEDs_out, LOW);
  pinMode(warningLED_out, OUTPUT);      // sets the digital pin 8 as an output
  digitalWrite(warningLED_out, LOW);
  
  startup = true;
  motorOn = false;
    
  //setup serial port of 9600baud
  Serial.begin(9600, SERIAL_8N1);     // opens serial port, sets data rate to 9600 bps with 8-N-1

  cli(); // stop interrupts

  S1counter = 0;
  S2counter = 0;
  S3counter = 0;
  S4counter = 0;
  warningCounter = 0;

  S1int = false;
  S2int = false;
  S3int = false;
  S4int = false;
  warningInt = false;

  //set timer1 interrupt at 1Hz
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1  = 0;//initialize counter value to 0
  // set compare match register for 1hz increments
  OCR1A = 15624;// = (16*10^6) / (1*1024) - 1 (must be <65536)
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS12 and CS10 bits for 1024 prescaler
  TCCR1B |= (1 << CS12) | (1 << CS10);  
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);

  sei(); // allow interrupts
  
  Serial.write("Commencing toy making!\n");
}

ISR(TIMER1_COMPA_vect)
{
  if(warningInt)
    warningCounter++;
  
  if(S1int)
    S1counter++;
  
  if(S2int)
    S2counter++;

  if(S3int)
    S3counter++;

  if(S4int)
    S4counter++;
}

void firstSensor()
{
  Serial.write("Kicking off warning light\n");
  digitalWrite(warningLED_out, HIGH);
  S1int = true;
  warningInt = true;
}
      
void secondSensor()
{
  if(!motorOn)
  {
    Serial.write("Turning motor on\n");
    digitalWrite(motor_out, HIGH);
    motorOn = true;
  }
  else
  {
    Serial.write("Turning motor off\n");
    digitalWrite(motor_out, LOW);
    motorOn = false;
  }
  S2int = true;
}    

void thirdSensor()
{
  Serial.write("Turning 3 LEDs on\n");
  digitalWrite(threeLEDs_out, HIGH);
  S3int = true;
}

void fourthSensor()
{
  Serial.write("Turning 3 LEDs off\n");
  digitalWrite(threeLEDs_out, LOW);
  S4int = true;
}

void loop()
{
  if(warningCounter >= warningTics)
  {
    warningCounter = 0;
    digitalWrite(warningLED_out, LOW);
    Serial.write("Turning off warning light\n");
    warningInt = false;
  }
  
  if(S1counter >= sensorTics)
  {
    S1counter = 0;
    S1int = false;
  }

  if(S2counter >= sensorTics)
  {
    S2counter = 0;
    S2int = false;
  }

  if(S3counter >= sensorTics)
  {
    S3counter = 0;
    S3int = false;
  }

  if(S4counter >= sensorTics)
  {
    S4counter = 0;
    S4int = false;
  }
  
  if (!digitalRead(S1))
  {
    if(!S1int)
    {
      firstSensor();
      startup = false;
    }
  }
  
  if(!digitalRead(S2))
  {
    if(!startup)
    {
      if(!S2int)
        secondSensor();
    }
    else
    {
      Serial.write("Detecting S2, but don't know which position it is in yet....\n");
    }
  }
  
  if(!digitalRead(S3))
  {
    if(!S3int)
    {
      thirdSensor();
      startup = false;
    }
  }
  
  if(!digitalRead(S4))
  {
    if(!S4int)
    {
      fourthSensor();
      startup = false;
    }
  }
}