

#include 
#include 

#define DEBUG 0
#define LOWERLIMIT 400
#define UPPERLIMIT 650
#define COUNTLIMIT 2
#define OUTPIN 12
#define SETUPPIN 13

Servo turret;   
//Servo outServo;
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);
//  outServo.attach(10);
//  outServo.write(90);
  Wire.begin();
  Serial.begin(9600);
  
  pinMode(OUTPIN, OUTPUT);
 pinMode(SETUPPIN, OUTPUT); 
 
 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;
 
   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]);
      
      /*
      if(abs(initVal-currentServo) > 5) {
        outServo.write(initVal);
        currentServo = initVal;
      }
      */
      //outServo.write(110);
      
      retVal = recordThermo(decision);
      
      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); 
} 
 
 
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;
}