#include <Servo.h> 

// I used the NewPing library for my distance sensors, I think from here: http://code.google.com/p/arduino-new-ping/
#include <NewPing.h>

// Init the servo stuff
Servo fl;
Servo fr;
Servo rl;
Servo rr;
Servo neck;

// Wheel servo variables
int wheelSpeed=20;

// Distance sensor variables
int wallTrigPin=6;
int wallEchoPin=2;
int floorTrigPin=7;
int floorEchoPin=3;
int neckDelay=150;
int lrDist=25;
int cDist=10;
int reallyCloseDist=5;
int maxFloorDist=7;
int turnTime=600;
int backTime=200;
int giveUpAfterTime=15000;
int turnWayAroundTime=2500;
int turnAllTheWayAroundTime=4000;
int forwardSafeAfterTime=2000;
boolean firstTimeThrough=true;

NewPing sonar[2]={
  NewPing(wallTrigPin, wallEchoPin, lrDist),
  NewPing(floorTrigPin, floorEchoPin, maxFloorDist)
};
  
int lastNeckPos=0; // 0=center, 1=left, 2=center, 3=right
int d, dl, dc, dr;
unsigned long giveUpTurningTime;
unsigned long drivingSinceTime=0;

void setup() {
	Serial.begin (9600);

	// attach motor servos
	rl.attach(8);
	fl.attach(9);
	fr.attach(10);
	rr.attach(11);

	// attach neck servo
	neck.attach(12);

	// setup distance sensors
	pinMode(wallTrigPin, OUTPUT);
	pinMode(wallEchoPin, INPUT);
	pinMode(floorTrigPin, OUTPUT);
	pinMode(floorEchoPin, INPUT);

	// start the neck off in the center, give it time to get there before taking off
	neck.write(90);
	delay(neckDelay);

	// start stopped, setup initial timing variables
	drive('s');
	giveUpTurningTime=millis()+giveUpAfterTime;

	Serial.println("setup complete");
}

void loop() {
	// look at the floor
	d=sonar[1].ping();
	if (d>0) d=d/US_ROUNDTRIP_CM; else (d=254);
	if (firstTimeThrough==false && (d>maxFloorDist)){
		// floor is too low, might be stairs!
		Serial.print("floor dropped: ");
		Serial.print(d);
		Serial.println("cm");
		drive('b');
		delay(backTime*4);
		drive('r');
		delay(turnAllTheWayAroundTime);
		giveUpTurningTime=millis()+giveUpAfterTime;
	} else {
		// turn neck
		lastNeckPos++;
		if (lastNeckPos>3) lastNeckPos=0;

		switch (lastNeckPos){
			case 0:
			case 2:
				neck.write(90);
				break;
			case 1:
				neck.write(45);
				break;
			case 3:
				neck.write(135);
		}
		// give the neck time to get there
		delay(neckDelay);

		// look for an obstacle
		d=sonar[0].ping();
		if (d>0) d=d/US_ROUNDTRIP_CM;
		if (d>0 && ((lastNeckPos%2==0 && d<cDist) || (lastNeckPos%2!=0 && d<lrDist))){
			// object spotted within a threshold distance

			// see if it's been a long time since we were able to drive forward without obstacles
			if (millis()>giveUpTurningTime){
				// yep, it's been a long time. Maybe we're in a corner. Let's turn this car around.
				drive('b');
				delay(backTime*3);
				drive('l');
				delay(turnWayAroundTime);
				drive('s');
				// reset the clock for giving up on forward driving
				giveUpTurningTime=millis()+giveUpAfterTime;
			} else {
				// stop and scan all directions
				drive('s');
				drivingSinceTime=0;

				// look right
				neck.write(45);
				delay(neckDelay);
				dr=sonar[0].ping();
				if (dr>0) dr=dr/US_ROUNDTRIP_CM; else (dr=254);

				// look middle
				neck.write(90);
				delay(neckDelay);
				dc=sonar[0].ping();
				if (dc>0) dc=dc/US_ROUNDTRIP_CM; else (dc=254);

				// look left
				neck.write(135);
				delay(neckDelay);
				dl=sonar[0].ping();
				if (dl>0) dl=dl/US_ROUNDTRIP_CM; else (dl=254);

				if (dl<lrDist || dc<cDist || dr<lrDist){
					// ok, we still see an object after looking around
					int thisTurnTime=turnTime;
					if (dl<reallyCloseDist || dr<reallyCloseDist || dc<reallyCloseDist){
						// something is REALLY close, turn a little farther
						thisTurnTime=thisTurnTime*2;
					} else if (dl<lrDist && dr<lrDist) {
						// blocked on both right and left, which probably means a corner, or a wall straight on. 
						// turn a lot so we don't waste too much time going back and forth
						thisTurnTime=thisTurnTime*3;
					}

					if (dl<dr && dl<dc){
						Serial.println("object on left");
						// object is on left, keep looking left, backup, turn right
						neck.write(135);
						drive('b');
						delay(backTime);
						drive('r');
						delay(thisTurnTime);
						drive('s');
					} else if (dr<dl && dr<dc){
						Serial.println("object on right");  
						// object is on right, keep looking right, backup, turn left
						neck.write(45);
						drive('b');
						delay(backTime);
						drive('l');
						delay(thisTurnTime);
						drive('s');
					} else if (dc<cDist){
						Serial.println("object on center");
						// object is in center, keep looking center, get lesser of left/right and turn the other direction
						neck.write(90);
						drive('b');
						delay(backTime);
						if (dr<dl){
							drive('l');
						} else {
							drive('r');
						}
						delay(thisTurnTime);
						drive('s');
					} else {
						// just where the hell is the object? just move on.
					}
				} else {
					// well, I guess it was a ghost. Just move on.
				}
			}
		} else {
			// looked for an obstacle, nothing found, ok to move
			drive('f');
			if (drivingSinceTime==0) {
				drivingSinceTime=millis();
			} else if (drivingSinceTime+forwardSafeAfterTime<millis()){
				// been driving forward for a while now. it's safe to assume we're not in a corner.
				giveUpTurningTime=millis()+giveUpAfterTime;
			}
		}
	}
	firstTimeThrough=false;
}

void drive(char dir){
	switch (dir){
	case 'f':
		fl.write(90-wheelSpeed);
		fr.write(90+wheelSpeed);
		rl.write(90-wheelSpeed);
		rr.write(90+wheelSpeed);
	break;

	case 'b':
		fl.write(90+wheelSpeed);
		fr.write(90-wheelSpeed);
		rl.write(90+wheelSpeed);
		rr.write(90-wheelSpeed);
	break;

	case 'r':
		fl.write(90+wheelSpeed);
		fr.write(90+wheelSpeed);
		rl.write(90+wheelSpeed);
		rr.write(90+wheelSpeed);
	break;

	case 'l':
		fl.write(90-wheelSpeed);
		fr.write(90-wheelSpeed);
		rl.write(90-wheelSpeed);
		rr.write(90-wheelSpeed);
	break;

	case 's':
		fl.write(90);
		fr.write(90);
		rl.write(90);
		rr.write(90);
		break;
	}
}

int getDistance(int trigPin, int echoPin){
	long duration, distance;
	digitalWrite(trigPin, LOW);
	delayMicroseconds(2);
	digitalWrite(trigPin, HIGH);
	delayMicroseconds(10);
	digitalWrite(trigPin, LOW);
	duration = pulseIn(echoPin, HIGH);
	distance = (duration/2) / 29.1;

	return (distance);
}