#include <Wire.h>
#include <Servo.h>
 
#define PIR_HIST 4
#define DEBUG 0
 
Servo turret;   
Servo outServo;
byte sensor_address = 0xd0;
int test=0;
int PIR[4];
int test2=1;
 
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);
  PIR[0] = analogRead(0);
  PIR[1] = analogRead(1);
  PIR[2] = analogRead(2);
  PIR[3] = analogRead(3);
  delay(1000);
} 
 
 
void loop() 
{ 
   int i;
   byte retVal;
   int c, beta;
 //record sensors
   recordPIR();
 
  i=5;
  //analyse PIR
  if(PIR[0] > 650 || PIR[0] < 400)
    i=0;
  if(PIR[1] > 650 || PIR[1] < 400)
    i=1;
  if(PIR[2] > 650 || PIR[2] < 400)
    i=2;
  if(PIR[3] > 650 || PIR[3] < 400)
    i=3;
 
    if(i < 5) {
      if(DEBUG) Serial.println(PIR[i]);
      retVal = recordThermo(i);
 
      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);
      }
    }
 
   delay(50); 
} 
 
 
byte recordThermo(byte sector)
{
  byte pos;
  byte heat[9];
 
   int idxHigh=0;
   byte valHigh=0;
 
  switch(sector){
    case 0:
      pos = 20;
      break;
    case 1:
      pos = 65;
      break;
    case 2:
      pos = 115;
      break;
    case 3:
      pos = 160;
      break;
  }
 
 
  turret.write(pos);
  if(DEBUG) Serial.print("scanning sector ");
  if(DEBUG) Serial.println((int)sector);
  delay(1500);
 
  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;
 
}
 
void recordPIR()
{
  PIR[0] = analogRead(0);
  PIR[1] = analogRead(1);
  PIR[2] = analogRead(2);
  PIR[3] = analogRead(3);
 
  Serial.println(PIR[0]);
  Serial.println(PIR[1]);
  Serial.println(PIR[2]);
  Serial.println(PIR[3]);
}