#include <Servo.h> 
#include <Wire.h>

Servo waist,front,back,neck;

#define wCenter 80
#define fCenter 70
#define bCenter 80
#define nCenter 80
#define wSwing 40
#define fSwing 35
#define bSwing 35
#define nSwing 40
#define FORWARD 0
#define BACKWARD 1
#define LEFT 2
#define RIGHT 3
#define STOP 4
int wPos,fPos,bPos,cycle,dir;
long start;

int getMode() {
  pinMode(5,OUTPUT);
  digitalWrite(5,LOW);
  digitalWrite(6,HIGH);
  pinMode(6,INPUT);
  if (digitalRead(6)==0) return 1;
  digitalWrite(7,HIGH);
  pinMode(7,INPUT);
  if (digitalRead(7)==0) return 2;
  digitalWrite(8,HIGH);
  pinMode(8,INPUT);
  if (digitalRead(8)==0) return 3;
  digitalWrite(9,HIGH);
  pinMode(9,INPUT);
  if (digitalRead(9)==0) return 4;
  return 0;
}

int compassAddress = 0x42 >> 1;  // From datasheet compass address is 0x42
                                 // shift the address 1 bit right, the Wire library only needs the 7
                                 // most significant bits for the address
void compassSetup() 
{ 
  Wire.begin();                // join i2c bus (address optional for master) 
} 

int getCompass() 
{ 
  int reading;
  Wire.beginTransmission(compassAddress);
  Wire.send('A');
  Wire.endTransmission();
  delay(10);
  Wire.requestFrom(compassAddress, 2);  // request 2 bytes from slave device #33 
  if(2 <= Wire.available())     // if two bytes were received 
  { 
    reading = Wire.receive();   // receive high byte (overwrites previous reading) 
    reading = reading << 8;     // shift high byte to be high 8 bits 
    reading += Wire.receive();  // receive low byte as lower 8 bits 
    reading /= 10;
    return reading;
  } 
  return -1;
} 


int mode;
int initialHeading;
int sequenceLength;

void setup() 
{ 
  waist.attach(13);
  front.attach(12);
  back.attach(11);
  neck.attach(0);
  wPos=wCenter;
  fPos=fCenter;
  bPos=bCenter;
  dir=STOP;
  cycle=0;
  waist.write(wCenter);
  front.write(fCenter);
  back.write(bCenter);
  neck.write(nCenter);
  compassSetup();
  neck.write(nCenter-nSwing);
  delay(500);
  neck.write(nCenter+nSwing);
  delay(500);
  neck.write(nCenter);
  delay(500);
  initialHeading=getCompass();
  delay(40);
  mode=getMode();
  start=millis();
  dir=FORWARD;
  sequenceLength=2000;
  if (mode==2 || mode==3)
    Serial.begin(9600);
} 

void centerTest() {
  waist.write(wCenter);
  front.write(fCenter);
  back.write(bCenter);
  neck.write(nCenter);
  delay(10);
}

void doForward(int cycle) {
  if (cycle<12) wPos=wCenter+wSwing;
  else if (cycle<37) wPos=wCenter-wSwing;
  else if (cycle<62) wPos=wCenter+wSwing;
  else if (cycle<87) wPos=wCenter-wSwing;
  else          wPos=wCenter+wSwing;
  if (cycle<25) fPos=fCenter+fSwing;
  else if (cycle<50) fPos=fCenter-fSwing; 
  else if (cycle<75) fPos=fCenter+fSwing;
  else fPos=fCenter-fSwing; 
  if (cycle<25) bPos=bCenter-bSwing;
  else if (cycle<50) bPos=bCenter+bSwing; 
  else if (cycle<75) bPos=bCenter-bSwing;
  else bPos=bCenter+bSwing; 
}

void doBackward(int cycle) {
  doForward(100-cycle);
}

void doLeft(int cycle) {
  doForward(cycle);
  if (wPos>wCenter) wPos=wCenter;
}

void doRight(int cycle) {
  doForward(cycle);
  if (wPos<wCenter) wPos=wCenter;
}

void doStop(){
  wPos=wCenter;
  fPos=fCenter;
  bPos=bCenter;
}

void posUpdate() {
  cycle=(cycle+3)%100;  // 100 step cycle
  switch(dir) {
    case FORWARD: doForward(cycle);  break;
    case BACKWARD: doBackward(cycle); break;
    case LEFT: doLeft(cycle); break;
    case RIGHT: doRight(cycle); break;
    case STOP: doStop(); break;
  }
}

#define TOOCLOSE 500
#define NOTCLEAR 400
int getDistance() {
  return analogRead(3);
}

void lookLeft() {
  neck.write(nCenter-nSwing);
  delay(200);
}

void lookRight() {
  neck.write(nCenter+nSwing);
  delay(200);
}

void lookForward() {
  neck.write(nCenter);
  delay(200);
}

void walkTest() {
  posUpdate();
  waist.write(wPos);
  delay(20);  
  front.write(fPos);
  delay(20);  
  back.write(bPos);
  delay(20);
  if (millis()-start<10000) dir=FORWARD;
  else if (millis()-start<20000) dir=LEFT;
  else if (millis()-start<30000) dir=RIGHT;
  else if (millis()-start<40000) dir=BACKWARD;
  else if (millis()-start<50000) {
    dir=FORWARD;
    start=millis();
  }
}


void run() {
  posUpdate();
  waist.write(wPos);
  delay(20);  
  front.write(fPos);
  delay(20);  
  back.write(bPos);
  delay(20);
  if (millis()-start>sequenceLength) {
    lookForward();
    int f=getDistance();
    if (f>TOOCLOSE) { // We got too close
      dir=BACKWARD;
      lookLeft();
      int l=getDistance();
      lookRight();
      int r=getDistance();
      // If we are in a corner do a big backup
      if (l>TOOCLOSE && r>TOOCLOSE) sequenceLength=20000;
    }
    else if (f>NOTCLEAR){  // The way forward is not clear
      sequenceLength=2000;
      lookLeft();
      int l=getDistance();
      lookRight();
      int r=getDistance();
      if (l<r) dir=LEFT;
      else     dir=RIGHT;
    } else {  // We are free to get back on course
      sequenceLength=2000;
      int heading=getCompass();
      dir=FORWARD;
      if (heading-initialHeading>20) dir=LEFT;
      if (heading-initialHeading<-20) dir=RIGHT;
    }
    start=millis();
  }
}

void distanceTest() {
  int d=getDistance();
  Serial.print("Distance ");
  Serial.println(d);
  delay(100);
}

void compassTest() {
  int c=getCompass();
  Serial.print("Compass ");
  Serial.println(c);
  delay(100);
}

void loop() 
{ 
  switch(mode) {
    case 1: centerTest(); break;
    case 2: distanceTest(); break;
    case 3: compassTest(); break;
    case 4: walkTest(); break;
    default: run(); break;
  }
}