

#include 
#include 

#define DEBUG 0
#define LOWERLIMIT 400
#define UPPERLIMIT 650
#define COUNTLIMIT 20

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);
  
  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 retVal, decision;
  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);
   }
   
   switch(decision){
    case 0:
      initVal = 20;
      break;
    case 1:
      initVal = 65;
      break;
    case 2:
      initVal = 115;
      break;
    case 3:
      initVal = 160;
      break;
  }
   
    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 < 180) 
      {
        //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);
      }
      
      currentState = decision;
      
    }
   delay(50); 
} 
 
 
byte recordThermo(byte sector)
{
  byte pos;
  byte heat[9];
  
   int idxHigh=0;
   byte valHigh=0;
  
  switch(sector){
    case 0:
      pos = 2;
      break;
    case 1:
      pos = 65;
      break;
    case 2:
      pos = 115;
      break;
    case 3:
      pos = 178;
      break;
  }
  
  
  turret.write((int)pos);
  if(DEBUG) Serial.print("scanning sector ");
  if(DEBUG) Serial.println((int)sector);
  delay(500);
  
  int i=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;
 
     if(heat[i] > valHigh)
     {
       idxHigh = i;
       valHigh = heat[i];
     }
 
     if(DEBUG) Serial.print(heat[i], DEC);
     if(DEBUG) Serial.print(" ");
   }
   if(DEBUG) Serial.println("");
   if(valHigh > 3)
   {
      return pos - 5*(idxHigh-4);
      
   }

   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 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;
}