#include <SoftwareSerial.h>//By including this library we can tlak serially with devices on pins other than our tx and rx pins (pins 2 and 3 on the arduino).  This is important
  //because we can still use the serial monitor to debug by printing error flags there and funny statements while still sending the proper commands to things like our servo controller
  
  SoftwareSerial Servos(10, 11); // We will communicate with the servocontroller on pins 10 and 11
  //RX is digital pin 10 (connect to TX of the servo controller)--we do not need to listen to the servo controller so we don't need to use/connect pin 11
  
  int n;//first random number
  int m;//second random number
  int o;//thrid random number
  int threshold=30;//of ambient light
  int targets=18;//current number of targets
  int score=0;//initialized to 0
  int TIME=60000;//total time for game==1 minute
  int TIME_Played=0;//initialize time played to 0
  
  //pins used
  int anodePin=4;//all led's use this pin for their long leg
  int clock=5;//used for the shift register
  int leftin=6;//data that gets shifted in
  int masterreset=7;//resets shift register
  
  //The following are address lines to our i/o expander A being the most significant bit (msb) and E being the least significant bit (lsb)
  int Apin=14;//A0
  int Bpin=15;//A1
  int Cpin=16;//A2
  int Dpin=17;//A3
  int Epin=18;//A4
  
  //This pin is the input Z to our i/o expander--it is what is being muxed to whatever address we want
  int cathodePin0=19;//A5
  
  //additonal variables
  //These will hold the state that we want our address pins in.  1 is for high, 0 is for low.
  int A;
  int B;
  int C;
  int D;
  int E;
  
  int delays[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};  //the delay is how long we want the servo up for--initalize all of them to be 0--I used a buffer instead of a variable like
  //the last code because now if we use more than two targets I do not need to add another variable, just another if statement
  //there are 19 0's because 18 targets+1 for the bonus
  
  int bonuspin=18;
  int bonustimer=0;
  int bonusflag=18;
  
  int tworandom(int i,int j);//random funcition generator for second number based off the first (j is the one that changes based on i)
  int threerandom(int i, int j, int k);
  int reads(int led);//read led
  int readsavg(int n, int led);//takes average of n readings for led
  int reset(int servonum1, int servonum2, int servonum3);//initialize current servonumber1, reset delay of current servo, assign a new serov to servonum1 based   off of the other two
  int scores(int servonum);//score when you shoot (shooting aliens==++)
  int scored(int servonum);//score when you don't shoot (don't shoot humans==++)
  
  void talk(int command, int servo, int data1, int data2);//communicate with the servo controller--do not play with unless your servo controller has a different id number
  void rotate(int servo,int angle);//use command 4 of the pololu protocol to move the servo to some angle--input is in degrees
  void initialize(void);//sets initial servo positons--all to 90, which is down for this setup
  
  int convert(int i);//convert an led number into binary and set A through E to the proper value so if we want to read the led using readsavg, we may
  
  void clocks(void);
  void mreset(void);
  void shiftin(void);
  void bonusroundtrigger(int bonus);
  void bonusround(void);
  void RESET(void);
  
  void level(int timer, int enemies);
  
  void setup()
  {
    
    //deactivates pullup resistors
    _SFR_IO8(0x35) |= 4;
    _SFR_IO8(0x35) |= (1<<4); 
    
    randomSeed(analogRead(0));//select start point of random number chain by reading analog.  This should allow for random start in the chain of ranodom numbers by inputing a floating number, which should theoretically not repeat between two consecutive games
    n=random(targets);  //choose first random number
    m=tworandom(n,m);  //choose second random number based off of the first
    o=threerandom(n,m,o);
    
    pinMode(anodePin, OUTPUT);  //set pins associated with led light sensors to outputs
    pinMode(Apin, OUTPUT);      //addresses are obviously outputs
    pinMode(Bpin, OUTPUT);
    pinMode(Cpin, OUTPUT);
    pinMode(Dpin, OUTPUT);
    pinMode(Epin, OUTPUT);
    pinMode(cathodePin0, OUTPUT);//we change this mode later but it should be initialized as an output
    pinMode(clock,OUTPUT);
    pinMode(leftin,OUTPUT);
    pinMode(masterreset,OUTPUT);
    
    clocks();
    mreset();  
    
    digitalWrite(anodePin, HIGH);//this will light up the first led that we read
    digitalWrite(cathodePin0,LOW);
    
    digitalWrite(Apin,LOW);//sets the first address to 0, will change the moment we read an led
    digitalWrite(Bpin,LOW);
    digitalWrite(Cpin,LOW);
    digitalWrite(Dpin,LOW);
    digitalWrite(Epin,LOW);
    
    
    initialize();//initialize all of the servos to the down postion
    
    Serial.begin(9600);//set up serial communication to monitor
    Servos.begin(9600);//set up serial communicatio to the servo controller
  }
  
  void loop(){
    //level 1
    level(3000);
    RESET();
    //level 2
    level(2000);
    RESET();
    //level 3
    level(1000);
    RESET();
  }
  
  int tworandom(int i,int j){ 
    //i is the number staying the same j is the one you are changing
    //chooses second random number that is not the same as the fist or equal to that of its pair
    int k=random(targets);//
    while(((k%(targets/2))==(i%(targets/2)))||(k==j)){
      //don't let k occupy the same space as i and don't let k be j since k is the new j--pick a new target
      k=random(targets);
    }
    return k;//return the new random number
  }
  int threerandom(int i, int j, int k){
    //i and j remain the same, and k is the one changing based on the other two
    int l=random(targets);
    while(((l%(targets/2))==(i%(targets/2)))||(((l%(targets/2))==(j%(targets/2)))||(l==k))){
      l=random(targets);
    }
    return l;//new assignment to k
  }
  int reads(int led){
    int val = 0;//initalize readin to 0
    int vcc=anodePin;//for shorthand set vcc to be the value of the anodePin
    convert(led);//get address of led
    //set multiplexer's to read that address
    digitalWrite(Apin, A);
    digitalWrite(Bpin, B);
    digitalWrite(Cpin, C);
    digitalWrite(Dpin, D);
    digitalWrite(Epin, E);
    
    digitalWrite(vcc,   HIGH);//turn on the led
    digitalWrite(cathodePin0, LOW);
    
    /*Serial.print(A);
    Serial.print(B);
    Serial.print(C);
    Serial.print(D);
    Serial.println(E);*/
    
    //wait 50ms
    delay(50);
    
    //switch potentials -- charge LED to -5V
    digitalWrite(vcc,   LOW);
    digitalWrite(cathodePin0, HIGH);
    
    //switch pinmode 
    pinMode(cathodePin0,  INPUT);
    
    //measure time it takes for cathodePin to go to zero, but if the value is too high leave the loop
    while((digitalRead(cathodePin0) != 0)&&(val<100)){
      delay(1);
      val++;      
    }
    pinMode(cathodePin0,  OUTPUT);//become an output agian
    
    digitalWrite(vcc,   HIGH);//turn the led back on
    digitalWrite(cathodePin0, LOW);
    return val;//return reading
  }
  
  int readsavg(int n, int led){
    int val=0;//initialize value
    for(int i=0; i<n; i++){
      val=val+reads(led);//read n times and sum
    }
    val=val/n;//divide by n for average
    return val;//return average
  }
    
  int reset(int servonum1, int servonum2, int servonum3){
    rotate(servonum1,90);//reset to down position
    delays[servonum1]=0;//reset delays
    int k=threerandom(servonum2, servonum3, servonum1);//change 1 based on 2 and 3
    return k;
  }  
  
  int scores(int servonum){
    //scores==score when shot
    if(servonum<=(targets/2)){
      //servos 0, 1, 2 are humans
      score--;//don't shoot humans 
    }
    if(servonum>((targets/2))){
      //servos 3, 4, 5 are aliens
      score++;//do shoot aliens
    }
  }
  int scored(int servonum){
    //scored==scored when did not shoot
    if(servonum<=(targets/2)){
      //servos 0, 1, 2 are humans
      score++;//good you did not shoot the humans
    }
    if(servonum>((targets/2))){
      //servos 3, 4, 5 are aliens
      score--;//baka, you forgot to shoot the aliens--have you seen Independence Day?
    }
  }
  
  //////////////////////
  ////////////////////
  //Servo Controller
  void talk(int command, int servo, int data1, int data2){  
  /*
  This uses the pololu portocol for controlling the servos--the shorting block must be removed for 
  these commands to work--I say this twice, because it is important.
  The data 1 is lsb and data 2 is msb, it ignores the first bit, and the units in 1/4 micro seconds.
  For example, the code uses 0x70, 0x2E->in binary 00111000, 00101110 to indicate 6000 1/4 micro seconds, which is  lsb:  0111000, msb:  
  0101110, but together in proper oder reads as 0x1770.  
  This is further 1500 microseconds which is the 90 degree position--The conversion from degrees to microseconds is covered below.
  */
    Servos.write(0xAA);  //start byte
    Servos.write(0x0C);  //device id which is 12 in decimal for the 24 serial servo controller
    Servos.write(command); //command number
    Servos.write((byte)servo);  //servo number (0-23)
    Servos.write((byte)data1);  //data 1--lsb--lowest 7 bits
    Servos.write((byte)data2);  //data 2--msb--highest 7 bits
    delay(75);
  }
  
  /*  I made this cheat sheet of postions and inputs until a proper conversion was found,
  it discusses how to convert from degrees to microseconds and then to proper bits need to be sent:
  
  Microseconds are found by multiplying by 11 and then adding 500 to degrees--remember that the controller
  takes ins quarter of a microsecond (four times this number) though and discrads the 
  first bit of data of each data byte (i.e. send the lowest 7 bits with a leading 0 and then
  the highest 7 bits with a leading 0)
  
  The afore said covnversion is found by noting that the signals to the servo controller vary
  from 500 (0) to 2500 (180).  Thus to get zero subtract 500.  To get everything else divide 2000 by 180.
  The result is not exactly 11, but the error is very slight.
  
  (microseconds--least significant bit, most significant bit)
  500--0X50,0X0F->about 0 degrees
  700--0X0F,0X15->18
  750--0X38,0X17->22
  800--0X00,0X19->27
  1000--0X20,0X1F->45
  1200--0X40,0X25->63
  1500--0X70,0X2E->about 90 degrees
  1800--0X20,0X38->118
  2500--0X10,0X4E->about 180 degrees
  */
  
  /////////////////////////////////////////////////////
  
  void rotate(int servo,int angle){
    //Uses talk() to send servo to some angle
    int converted=(angle*11+500)*4;//in quarter microseconds
    int data2=converted>>7;//most significant bit
    int data1=converted&0x7F;//least significant bit
    
    talk(0x04, servo, data1, data2);//sends data
  }
  
  void initialize(void){
    //set all servos to an inital 90 degrees.  Allows for rotation in either direction of 90 degrees
    for(int i=0; i<targets; i++){
      rotate(i,90);
    }
    delay(250);
  }
  
  int convert(int i){
    //converts a decimal number from 0 to 17, inclusively, to binary
    A=i/16;
    i=i-(A*16);
    B=i/8;
    i=i-(B*8);
    C=i/4;
    i=i-(C*4);
    D=i/2;
    i=i-(D*2);
    E=i;
  }  
  
  void clocks(void){
    digitalWrite(clock,HIGH);
    delay(25);
    digitalWrite(clock,LOW);
  }
  void mreset(void){
    digitalWrite(masterreset,LOW);
    delay(1000);
    digitalWrite(masterreset,HIGH);
    Serial.println("Reseting");
  }
  void shiftin(void){
    digitalWrite(leftin,HIGH);
    clocks();
    digitalWrite(leftin,LOW);
  }
  void bonusroundtrigger(int bonus){
    if(bonus==bonuspin){
      if(reads(bonuspin)<threshold){
        mreset();
        bonusflag=0;
        bonusround();
      }
      if(bonustimer==1500){
        bonusflag=0;//becomes input bonus
        bonustimer=0;
      }
      else{
        if((bonustimer==500)||(bonustimer==1000)){
          clocks();
          Serial.println("I am shifting so hard right now");
        }
      }
      bonustimer=bonustimer+(100);//making it variable based on number of readings would cause issues with timer==500
    }
  }
  void bonusround(void){
    
  }
  
  void RESET(void){
    //reset bonus
    bonusflag=18;
    //reset time
    TIME_Played=0;
    //reset servos
    n=random(targets);
    m=tworandom(n,m);
    o=threerandom(n,m,o); 
  }
  
  void level(int timer){
    while(TIME_Played<TIME){
      int time;
      int bonustime=random((2*TIME/3));//bonustime will always be less than 2/3 of total roundtime
      time=timer;//set time equal to varialbe input
      rotate(m,0);//pop up the first servos
      rotate(n,0);
      rotate(o,0);
      Serial.print("n, m, and o are");//until scoring happens and we are not adding anytihng new to the code, this will tell us the targets that are up--removable
      Serial.print(n);
      Serial.print(m);
      Serial.println(o);
      int number=1;//number of times led is read
      //all delays initialized at 0 or reset to 0 in "reset"
      delays[m]=delays[m]+(2*number*50);//each read delays 50 so n times 50 is full delay-the two is for two targets
      delays[n]=delays[n]+(2*number*50);
      delays[o]=delays[o]+(2*number*50);
      //read sensors
      int i = readsavg(number, n);//reads respective leds for some number of times
      int j = readsavg(number, m);
      int k = readsavg(number, o);
    
      if((i<threshold)&&(i>0)){
        //you hit target n
        Serial.println("You hit n");//agian removable code that we will keep until the final version
        scores(n);
        n=reset(n,m,o);
      }
      if((j<threshold)&&(j>0)){
        Serial.println("You hit m");
        //you hit target m
       scores(m);
        m=reset(m,n,o);
      }
      if((k<threshold)&&(k>0)){
        Serial.println("You hit o");
        //you hit target o
        scores(o);
        o=reset(o,m,n);
      }
      if(delays[n]>time){
        Serial.println("ran out of time 1");
        //you ran out of time for target n
        scored(n);
        n=reset(n,m,o);
      }
      if(delays[m]>time){
        Serial.println("ran out of time 2");
        //you ran out of time for target m
        scored(m);
        m=reset(m,n,o);
      }
      if(delays[o]>time){
        Serial.println("ran out of time 3");
        scored(o);
        o=reset(o,m,n);
      }
      if((TIME_Played>(TIME/3))||(TIME_Played<(2*TIME/3))){
        if(TIME_Played>bonustime){
          bonusroundtrigger(bonusflag);//if time played is greater than the bonus time that was randomly picked then it will trigger the bonustrigger loop that will control clock shift and reading the bonus pin--as well as causing the bonus round
        }    
      }
      //I removed the combo of delays because it is sort of uncessary--now if all delays expire at the same time there will be a delay because each one will reset after the loop completes
      //but it is unlikely that any combo will be synchronized except at the start--so no worries
      TIME_Played=TIME_Played+(2*number*50);//same as what we did for the delays at the start of the loop
      if(TIME_Played<TIME){
        Serial.print("Level over.  Your score is");
        Serial.print(score);
      }
    }
  }