

/********************************************************
*  Main intelligence algorithm. Includes IR reception.
*  By: Liliane Barbour and Jonathan Mash
*  Date: Apr. 2009
*********************************************************/


#include 
#include 

#define DEBUG 0
#define LOWERLIMIT 400
#define UPPERLIMIT 650
#define COUNTLIMIT 2
#define OUTPIN 12                                 //Pulse out to Motor Controller
#define SETUPPIN 13                               //Pin to hold Motor at 90
#define srfAddress 0x70                           // Address of the SRF08
#define lightByte 0x01                            // Byte to read light sensor
#define rangeByte 0x02                            // Byte for start of ranging data
#define cmdByte 0x00                              // Command byte
#define TIMER_RESET  TCNT1 = 0
#define SAMPLE_SIZE  38
#define DEBUG 1
#define led_pin 4
#define IRpin 2


unsigned int TimerValue[SAMPLE_SIZE];
char direction[SAMPLE_SIZE];
byte change_count;
long time;
bool checkIR;
Servo turret;   

byte sensor_address = 0xd0;
int PIRREAD[4][5];
int PIR[4];
byte index = 0;
byte currentState = 4;
byte currentServo = 90;

void setup() 
{ 
  turret.attach(9);  // attaches the servo on pin 9 to the servo object 
  turret.write(90);

  Wire.begin();
  Serial.begin(9600);
  
  pinMode(OUTPIN, OUTPUT);
  pinMode(SETUPPIN, OUTPUT); 
 
  //TCCR1A = 0x00;          // COM1A1=0, COM1A0=0 => Disconnect Pin OC1 from Timer/Counter 1 -- PWM11=0,PWM10=0 => PWM Operation disabled
  // ICNC1=0 => Capture Noise Canceler disabled -- ICES1=0 => Input Capture Edge Select (not used) -- CTC1=0 => Clear Timer/Counter 1 on Compare/Match
  // CS12=0 CS11=1 CS10=1 => Set prescaler to clock/64
  //TCCR1B = 0x03;          // 16MHz clock with prescaler means TCNT1 increments every 4uS
  // ICIE1=0 => Timer/Counter 1, Input Capture Interrupt Enable -- OCIE1A=0 => Output Compare A Match Interrupt Enable -- OCIE1B=0 => Output Compare B Match Interrupt Enable
  // TOIE1=0 => Timer 1 Overflow Interrupt Enable
  //TIMSK1 = 0x00;          

  pinMode(IRpin, INPUT);
  pinMode(led_pin, OUTPUT);
  checkIR = false;
  //attachInterrupt(0, isrIR, FALLING);
 
  digitalWrite(SETUPPIN, HIGH);
  delay(10000);
  digitalWrite(SETUPPIN, LOW);
    
  for(int i=0; i<6; i++) {
    PIRREAD[0][i] = analogRead(0);
    PIRREAD[1][i] = analogRead(1);
    PIRREAD[2][i] = analogRead(2);
    PIRREAD[3][i] = analogRead(3);
  }
  delay(1000);
} 
 
 
void loop() 
{ 
  byte decision;
  int retVal =0;
  int initVal = 90;
  int pingVal = 0;
 
  //START IR CODE 
  if(checkIR == true)
  {    
     int retVal = getIR();
     checkIR = false;
     
     switch(retVal)
     {  
       case 384: //Volume up
         Serial.println("Volume Up");       
         break;
       case 64: //Volume down
         Serial.println("Volume Down");
         break;
       case 224: //Power
         Serial.println("Power");
         break;
       case 48: //Left
         Serial.println("Left");
         break;
       case 50: //Right
         Serial.println("Right");
         break;
         
     }
   }
   //END IR CODE
   recordPIR();
      
   decision = 4;
   if(PIR[0] > UPPERLIMIT || PIR[0] < LOWERLIMIT) {
     if(DEBUG) Serial.println("main loop 0");
     decision = stateZeroHit(0,currentState);
   }
   else if(PIR[1] > UPPERLIMIT || PIR[1] < LOWERLIMIT) {
     if(DEBUG) Serial.println("main loop 1");
     decision = stateOneHit(1,currentState);
   }
   else if(PIR[2] > UPPERLIMIT || PIR[2] < LOWERLIMIT) {
     if(DEBUG) Serial.println("main loop 2");
     decision = stateTwoHit(2,currentState);
   }
   else if(PIR[3] > UPPERLIMIT || PIR[3] < LOWERLIMIT) {
     if(DEBUG) Serial.println("main loop 3");
     decision = stateThreeHit(3,currentState);
   }
   
  
    if(decision < 4) {
      if(DEBUG) Serial.println(PIR[decision]);
      
      
      retVal = recordThermo(decision);
      pingVal = getRange();
      
      
      if(retVal < 200)
      {
        //c = (int)floor(sqrt(11600-8000*(cos((90+retVal)*2*PI/180))));
        
        //beta = (int)floor((90 - acos((-8400+c*c)/80*c)));
        //outServo.write(retVal);
        
        
        float c1, alpha;
        float final;
        
        if(retVal >90) {
          c1 = sqrt(10900.0 - (6000.0*(cos((270.0 - retVal)*PI/180.0))));
          alpha = (-9100.0+(c1*c1))/(60.0*c1);
          final = ((acos(alpha)*180/PI)+ 90);
        }
        else if (retVal <90) {
          c1 = sqrt(10900.0 - (6000.0*(cos((90 + retVal)*PI/180.0))));
          alpha = (-9100.0+(c1*c1))/(60.0*c1);
          final =(90.0-(acos(alpha)*180.0/PI));
        }
        else {
          final = 90.0;
        }
        

        
      // float final = retVal;
        if(final < 0)
          final = 0.0;
        if(final > 180)
          final = 180.0;
        Serial.print("Final: ");
        Serial.println((int)floor(final*100));      
        moveTV((int)floor(final));
      }
      currentState = decision;
      
    }
   delay(50); 
} 
 
void isrIR()
{
  checkIR = true; 
}
 
int recordThermo(byte sector)
{
  byte pos;
  byte heat[9];
    
  switch(sector){
    case 0:
      pos = 10;
      break;
    case 1:
      pos = 60;
      break;
    case 2:
      pos = 120;
      break;
    case 3:
      pos = 170;
      break;
  }
  
  
  turret.write((int)pos);
  if(DEBUG) Serial.print("scanning sector ");
  if(DEBUG) Serial.println((int)sector);
  delay(500);
  
  int i=0;
  int wsum = 0;
  int sum = 0;
   
   for(i=1; i<=9; i++)
   {
      Wire.beginTransmission(sensor_address>>1);
      Wire.send(i);
      Wire.endTransmission();
 
      Wire.requestFrom(sensor_address>>1, (int) 1);
      while(Wire.available() < 1)
      { ; }
 
      heat[i-1] = Wire.receive(); // receive a byte as character
   } 
   
   for(i = 1; i<9; i++)
   {
     heat[i]-=heat[0];
     if(heat[i] > 100)
       heat[i] = 0;
     else if(heat[i] < 3)
       heat[i] = 0;
       
     sum+=heat[i];
     wsum+=heat[i]*i;
     if(DEBUG) Serial.print(heat[i], DEC);
     if(DEBUG) Serial.print(" ");
   }
 
   float div = wsum/sum;
   int val = (int) round(div);
     
   
   if(DEBUG) Serial.println("");
   if(DEBUG) Serial.println(val);
   if(div > 0)
   {
     int newval = pos - (int)floor(5*(div-4));
     Serial.print("TPA Val: ");
     Serial.println(newval);
     return newval;
      
   }

   return 200;
}

byte stateZeroHit(byte detected, byte lastState) {
  if(DEBUG) Serial.println("hit state 0");
  byte count = 0;
  
  if(lastState != 1) {
    while(count < COUNTLIMIT && detected == 0) {
      recordPIR();
      if(PIR[1] > UPPERLIMIT || PIR[1] < LOWERLIMIT) {
        detected = 1;
        detected = stateOneHit(detected, 0);
      }
      count++;
    }
  }
  
  return detected;
}

byte stateOneHit(byte detected, byte lastState) {
  if(DEBUG) Serial.println("hit state 1");
  byte count = 0;
    
  if(lastState == 0) {
    while(count < COUNTLIMIT && detected == 1) {
      recordPIR();
      if(PIR[2] > UPPERLIMIT || PIR[2] < LOWERLIMIT) {
        detected = 2;
        detected = stateTwoHit(detected, 1);
      }
      count++;
    }
  }
  
  else if(lastState == 2) {
    while(count < COUNTLIMIT && detected == 1) {
      if(PIR[0] > UPPERLIMIT || PIR[0] < LOWERLIMIT) {
        detected = 0;
        detected = stateZeroHit(detected, 1);
      }
      else
        recordPIR();
      count++;
    }
  }
  
  return detected;
  
}

byte stateTwoHit(byte detected, byte lastState) {
  if(DEBUG) Serial.println("hit state 2");
 byte count = 0;
    
  if(lastState == 1) {
    while(count  UPPERLIMIT || PIR[3] < LOWERLIMIT) {
        detected = 3;
        detected = stateThreeHit(detected, 2);
      }
      count++;
    }
  }
  
  else if(lastState == 3) {
    while(count < COUNTLIMIT && detected == 2) {
      recordPIR();
      if(PIR[1] > UPPERLIMIT || PIR[1] < LOWERLIMIT) {
        detected = 1;
        detected = stateTwoHit(detected, 2);
      }
      count++;
    }
  }
  
  return detected;
}

byte stateThreeHit(byte detected, byte lastState) {
  if(DEBUG) Serial.println("hit state 3");
  byte count = 0;
  
  if(lastState != 2) {
    while(count < COUNTLIMIT && detected == 3) {
      recordPIR();
      if(PIR[2] > UPPERLIMIT || PIR[2] < LOWERLIMIT) {
        detected = 2;
        detected = stateTwoHit(detected, 3);
      }
      count++;
    }
  }
  
  return detected;
}

void moveTV(int pos) {
  float newPos = pos;
  int inDelays = (int)floor(1300.0 + (400.0*newPos/180.0));
 // Serial.println(inDelays);
  
  digitalWrite(OUTPIN, HIGH);
  delayMicroseconds(inDelays);
  digitalWrite(OUTPIN, LOW);
}

void recordPIR()
{
  int count, currentval;
    PIRREAD[0][index] = analogRead(0);
    delay(5);
    PIRREAD[1][index] = analogRead(1);
    delay(5);
    PIRREAD[2][index] = analogRead(2);
    delay(5);
    PIRREAD[3][index] = analogRead(3);

//for each sensor
  for (int i = 0; i<4; i++) {
   
    //for each index in the sensor
    for(int m = 0; m<5; m++) {
      currentval = PIRREAD[i][m];
      count = 0;
      for(int q = 0; q<5; q++) {
        if(currentval >= PIRREAD[i][q])
          count++;
      }
      
      if (count == 3) {
        PIR[i]=PIRREAD[i][m];
        m=20;
      }
      
    }
  }
  index++;
    
  if(index >=5)
    index = 0;
}

int getRange()
{                                   // This function gets a ranging from the SRF08  
  int range = 0; 
  byte highByte = 0x00;                             // Stores high byte from ranging
  byte lowByte = 0x00;                              // Stored low byte from ranging
  
  Wire.beginTransmission(srfAddress);             // Start communticating with SRF08
  Wire.send(cmdByte);                             // Send Command Byte
  Wire.send(0x51);                                // Send 0x51 to start a ranging
  Wire.endTransmission();
  
  delay(100);                                     // Wait for ranging to be complete
  
  Wire.beginTransmission(srfAddress);             // start communicating with SRFmodule
  Wire.send(rangeByte);                           // Call the register for start of ranging data
  Wire.endTransmission();
  
  Wire.requestFrom(srfAddress, 2);                // Request 2 bytes from SRF module
  while(Wire.available() < 2);                    // Wait for data to arrive
  highByte = Wire.receive();                      // Get high byte
  lowByte = Wire.receive();                       // Get low byte

  range = (highByte << 8) + lowByte;              // Put them together
  
  return(range);                                  // Returns Range
}

int getIR()
{
  int value = 0;
  
  //Capture Signal
  change_count = 0;
  while(digitalRead(IRpin) == HIGH) {}                                 
  TIMER_RESET;
  TimerValue[change_count] = TCNT1;
  direction[change_count++] = '0';
  while (change_count < SAMPLE_SIZE) {
    if (direction[change_count-1] == '0') {
      while(digitalRead(IRpin) == LOW) {}
      TimerValue[change_count] = TCNT1;
      direction[change_count++] = '1';
    } else {
      while(digitalRead(IRpin) == HIGH) {}
      TimerValue[change_count] = TCNT1;
      direction[change_count++] = '0';
    }
  }
  
  //Analyze Signal
  change_count = 0;
  time = (long) TimerValue[change_count] * 4;
  change_count++;
  while (change_count < SAMPLE_SIZE) {
    time = (long) TimerValue[change_count] * 4;
    if(direction[change_count-1] == '0' && direction[change_count] == '1')
    {
      //Do nothing
    }
    else if(direction[change_count-1] == '1' && direction[change_count] == '0')
    {
      if(time - (long)TimerValue[change_count-1] * 4 > 2000)
      {
        if(DEBUG) Serial.print("-");
        value = 0;
      }
      else if(time - (long)TimerValue[change_count-1] * 4 > 1000)
      {
        value+=1;
        value = value*2;
        if(DEBUG) Serial.print((int)1);
      }
      else if(time - (long)TimerValue[change_count-1] * 4 < 1000)
      {
        value = value*2;
        if(DEBUG) Serial.print((int)0);       
      }
    }
    change_count++;  
  }
  if(DEBUG) Serial.print("  (");
  if(DEBUG) Serial.print(value);
  if(DEBUG) Serial.println(") end");
  delay(200); 
  
  return value;
}