//#include <SoftwareSerial.h>
//SoftwareSerial custom(10,9);

// Program to control the test laser endstops for a cnc machine X and Y axis
// Made by Isma LB, released under CC0
#define PHOTOTRANSISTOR_PIN_X A1 //pin 12
#define LASER_PIN_X 10 //pin 2
#define ENDSTOP_PIN_X 3 //pin 10
#define PHOTOTRANSISTOR_PIN_Y 0 //pin 13
#define LASER_PIN_Y 9 //pin 3
#define ENDSTOP_PIN_Y 2 //pin 11 

#define PHOTOTRANSISTOR_THRESHOLD 500

#define SIGNAL_DELAY 275

bool isEndstopPressedX = false;
bool isEndstopPressedY = false;

bool onX = false;
bool onY = false;

bool sensingX = false;
bool sensingY = false;

unsigned long xOnUntil = 0;
unsigned long yOnUntil = 0;

bool isEndstopPressed(uint8_t pin, bool *flag ){
  if(! digitalRead(pin) && !*flag){
    //End stop activated
    *flag = true;
    return true;
  }
  if(digitalRead(pin)){
    *flag = false;
  }
  return false;
}


void setup() {
  CLKPR = (1 << CLKPCE);
  CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
  pinMode(PHOTOTRANSISTOR_PIN_X,INPUT);
  pinMode(PHOTOTRANSISTOR_PIN_Y,INPUT);
  pinMode(ENDSTOP_PIN_X,INPUT);
  pinMode(ENDSTOP_PIN_Y,INPUT);
  pinMode(LASER_PIN_X,OUTPUT);
  pinMode(LASER_PIN_Y,OUTPUT); 
}

bool test = false;
void loop() {
  
  if( xOnUntil + 1 <= millis() && isEndstopPressed(ENDSTOP_PIN_X,&isEndstopPressedX)){
    onX = true;
  }
  if( yOnUntil + 1 <= millis() && isEndstopPressed(ENDSTOP_PIN_Y,&isEndstopPressedY)){
    onY = true;
  }

  if(onX){
     digitalWrite(LASER_PIN_X, HIGH);
     
     if(analogRead(PHOTOTRANSISTOR_PIN_X)>PHOTOTRANSISTOR_THRESHOLD){
       sensingX = true;
     }
     if(sensingX && analogRead(PHOTOTRANSISTOR_PIN_X)<PHOTOTRANSISTOR_THRESHOLD){
       //send signal
       digitalWrite(ENDSTOP_PIN_X,LOW);
       pinMode(ENDSTOP_PIN_X,OUTPUT);
       sensingX = false;
       onX = false;
       xOnUntil = millis() + SIGNAL_DELAY;
     }
   }
   else{
     digitalWrite(LASER_PIN_X, LOW);
   }
   if(xOnUntil <= millis()){ //off
      pinMode(ENDSTOP_PIN_X, INPUT);
   }
 
  if(onY){
     digitalWrite(LASER_PIN_Y, HIGH);

     if(analogRead(PHOTOTRANSISTOR_PIN_Y)>PHOTOTRANSISTOR_THRESHOLD){
       sensingY = true;
     }
     if(sensingY && analogRead(PHOTOTRANSISTOR_PIN_Y)<PHOTOTRANSISTOR_THRESHOLD){
       digitalWrite(ENDSTOP_PIN_Y,LOW);
       pinMode(ENDSTOP_PIN_Y,OUTPUT);
       sensingY = false;
       onY = false;
       yOnUntil = millis() + SIGNAL_DELAY;
     }
   }
   else{
    digitalWrite(LASER_PIN_Y, LOW);
   }
   if(yOnUntil <= millis()) { 
    //turn signal off
     pinMode(ENDSTOP_PIN_Y, INPUT);
   }
}