#include <TimerOne.h>
#define ACT 8
#define ERR 9
#define LSR 10
#define TRK 4
#define TRG 7
#define ACK1 12
#define LOCK 13
#define PWRPIN 11

int BTR = 150; //ms Interval
byte header = 0b10101010; //0xAA
byte data1 = 0b01011010; //0x5A
byte data2 = 0b10100101; //0xA5
unsigned int datastream = 0x0;  //Stream Pack 16bit HEADER+DATA

int lopwr = 50; // LOW POWER LASER
int mdpwr = 512; // MEDIUM POWER LASER
int hipwr = 1023; // HIGH POWER LASER
int PWR = 0;

char STAGE = 0;
unsigned long previousMillis = 0;
int counter = 15;
void send();
void pwrselect();
void error();

void setup() {
  Timer1.initialize(100);

  pinMode(ACT, OUTPUT); //8
  pinMode(ERR, OUTPUT); //9
  pinMode(LSR, OUTPUT); //10

  pinMode(TRK, INPUT); //4
  pinMode(TRG, INPUT); //7
  pinMode(ACK1, INPUT); //12
}

void loop() { while(1){//VOID LOOP
pwrselect();
 //*********************START TRACKING LOW POWER*******************************
if(digitalRead(TRK) ==1){
  Timer1.pwm(LSR, lopwr);  //------WARNING LASER!!!!! TRACKER ON
  if((digitalRead(TRG) == 1) && STAGE==0){
  datastream = ((header<<8) + data1);
  send();
  datastream = 0x0;
  }
}
else { // RESET IF TRK RELEASE
  Timer1.pwm(LSR, 0);
  digitalWrite(LOCK, LOW);
  STAGE = 0;
}
//**********************CHECK FOR LOCK SIGNAL**********************************
  if((digitalRead(ACK1) == 1) && (digitalRead(TRK) ==1)){ // CHECK FOR ACKNOWLEDGE
    digitalWrite(LOCK, HIGH);
    STAGE = 1;
    previousMillis = millis();
  }
//**********************RESET IF TIMEOUT**************************************  
  if(STAGE>=1 && previousMillis + 10000 < millis()){ // TIMEOUT for STAGE 1
    error();
  }
//**********************START FIRING HIGH POWER*********************************
  if((digitalRead(TRK) ==1) && STAGE>0 && (digitalRead(TRG) == 1)){
    STAGE=2;
    previousMillis = millis();
    datastream = ((header<<8) + data2);
    send();
    datastream = 0x0;
  }
}} //END VOID
//---------------------------ROUTINES-------------------------------
void pwrselect() {
if(digitalRead(PWRPIN) == 1){ // Power select
  PWR = hipwr;
}
else {PWR = mdpwr;}
}

void error(){
  digitalWrite(LOCK, LOW);
  for (int i = 0; i <8; i++){
    digitalWrite(ERR, HIGH);
    delay(125);
    digitalWrite(ERR, LOW);
    delay(125);
    } 
  STAGE = 0;
}

void send() { // Send data
  for(int i= 15; i > 0; i--){
    digitalWrite(ACT,HIGH);
    if(bitRead(datastream,i)){
      Timer1.pwm(LSR, PWR);
    }
    else Timer1.pwm(LSR, 0);
    counter--;
    delay(BTR); 
    digitalWrite(ACT,LOW);  
  }
  Timer1.pwm(LSR, 0);
}