#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

int anodePin=4;//all led's use this pin for their long leg

//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

//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};  //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
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 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
  
  
  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 time
  TIME_Played=0;
  //level 2
  level(2000);
  //reset time
  TIME_Played=0;
  //level 3
  level(1000);
  //reset time
  TIME_Played=0;  
}


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 level(int timer){
  while(TIME_Played<TIME){
    int time=0;
    time=timer;
    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);
    }
    //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("Game over.  Your score is");
      Serial.print(score);
    }
  }
}