#include "DriveRobot.h"
#include <libirobot/Robot.h>
#include <libirobot/Drive.h>
#include <libirobot/SerialControl.h>
#include <libirobot/sensors/BumpAndDropSensor.h>
#include <libirobot/sensors/BooleanSensor.h>
#include <libirobot/Song.h>
#include <libirobot/sensors/IRSensor.h>
#include <libirobot/sensors/ChargeSourceSensor.h>
#include <libirobot/sensors/UnsignedValueSensor.h>
#include <libirobot/sensors/ValueSensor.h>
#include <libirobot/Debug.h>

#include <math.h>

#ifndef __AVR__
#include <iostream>
#endif

using namespace iRobot;

#define QUARTER_NOTE 16 

DriveRobot::DriveRobot ( const char ttyDevice[] )
        : Robot ( ttyDevice ),
		  m_speed(0),
		  m_lastDir(Drive::Right),
		  m_tryingToDock(false),
		  m_dockAttempted(false),
		  m_dockTimeout(0)
{
    setMode ( Full );
    setAutoUpdate( Sensor::All, true );

	Song cliffMusic(2);
	Song::Note cliffnotes[] = {
		{ 60, QUARTER_NOTE },
		{ 57, QUARTER_NOTE },
	};
	for(int i = 0;i<2;i++) {
		cliffMusic.setNotePitch(i, cliffnotes[i].pitch);
		cliffMusic.setNoteLength(i, cliffnotes[i].length);
	}
	setSong(1, cliffMusic);
}

void
DriveRobot::avoid()
{
    Drive* d = getDrive();
	d->move(0);
    d->move(-(int)Drive::Medium );
    m_dockTimeout+=d->waitForDistance(100);
	d->move ( Drive::Medium, m_lastDir );
    d->waitForRotation(15);
}

bool
DriveRobot::reachedCliff()
{
		BooleanSensor* cliffLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffLeft ) );
		BooleanSensor* cliffFrontLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontLeft ) );
		BooleanSensor* cliffFrontRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontRight ) );
		BooleanSensor* cliffRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffRight ) );
		return cliffLeft->value() || cliffFrontRight->value() || cliffFrontLeft->value() || cliffRight->value();
}

DriveRobot::Side
DriveRobot::cliffSide()
{
    BooleanSensor* cliffLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffLeft ) );
    BooleanSensor* cliffFrontLeft = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontLeft ) );
    BooleanSensor* cliffFrontRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffFrontRight ) );
    BooleanSensor* cliffRight = static_cast<BooleanSensor*> ( getSensor( Sensor::CliffRight ) );
    if (cliffFrontLeft->value() && cliffFrontRight->value())
        return Center;
    if (cliffLeft->value() || cliffFrontLeft->value())
        return Left;
    if (cliffRight->value() || cliffFrontRight->value())
        return Right;
}

bool
DriveRobot::fellOffCliff()
{
    BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
	return s->flags() & BumpAndDropSensor::DropAny;
}

void
DriveRobot::dock()
{
    Drive* d = getDrive();
	IRSensor* ir = static_cast<IRSensor*> ( getSensor( Sensor::IR ) );
	unsigned char c = ir->value();
	ChargeSourceSensor* sources = static_cast<ChargeSourceSensor*> ( getSensor( Sensor::ChargeSources ) );
	ValueSensor* distance = static_cast<ValueSensor*> ( getSensor( Sensor::Distance ) );
	m_dockTimeout+=distance->value();
	if (m_dockTimeout > 2000) {
		m_dockTimeout = 0;
		m_tryingToDock = false;
		m_dockAttempted = false;
	} else if (sources->sources() & ChargeSourceSensor::Base) {
		d->move(0);
		m_tryingToDock = false;
		return;
	} else if (m_tryingToDock) {
		if (m_dockAttempted) {
			d->move(-Drive::Medium);
            m_dockTimeout+=d->waitForDistance(300);
			if (m_lastDir == Drive::Right)
				d->move(Drive::Fast, Drive::Left);
			else
				d->move(Drive::Fast, Drive::Right);
			d->waitForRotation(180);
            d->move(Drive::Medium);
            m_dockTimeout+=d->waitForDistance(100);
            m_dockAttempted = false;
		} else if ((c == (unsigned char)IRSensor::RedAndGreenBuoysAndForceField || c == (unsigned char)IRSensor::ForceField) && bumped()) {
			d->move(-Drive::Slow);
            m_dockTimeout+=d->waitForDistance(15);
			d->move(0);
			delay(1000000);
			m_dockAttempted = true;
		} else if (c == (unsigned char)IRSensor::RedAndGreenBuoys || c == (unsigned char)IRSensor::RedAndGreenBuoysAndForceField) {
			d->move(170);
		} else if (c == (unsigned char)IRSensor::RedBuoy || c == (unsigned char)IRSensor::RedBuoyAndForceField) {
			d->move(Drive::Medium, 100);
			m_lastDir = Drive::Left;
		} else if (c == (unsigned char)IRSensor::GreenBuoy || c == (unsigned char)IRSensor::GreenBuoyAndForceField) {
			d->move(Drive::Medium, -100);
			m_lastDir = Drive::Right;
		} else {
			if (m_lastDir == Drive::Right)
				d->move(Drive::Medium, 200);
			else
				d->move(Drive::Medium, -200);
		}
	} else {
		if (c == (unsigned char) IRSensor::ForceField || c == (unsigned char)IRSensor::RedBuoy || c == (unsigned char)IRSensor::GreenBuoy || c == (unsigned char)IRSensor::RedAndGreenBuoys) {
			m_tryingToDock = true;
		} else {
			d->move(Drive::Medium);
		}
	}
}

bool
DriveRobot::bumped()
{
    BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
    BooleanSensor* wall = static_cast<BooleanSensor*> ( getSensor ( Sensor::VirtualWall ) );
	return s->flags() & BumpAndDropSensor::BumpAny || wall->value();
}

bool
DriveRobot::bumpedLeft()
{
    BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
	return s->flags() & BumpAndDropSensor::BumpLeft;
}

bool
DriveRobot::bumpedRight()
{
    BumpAndDropSensor* s = static_cast<BumpAndDropSensor*> ( getSensor ( Sensor::BumpsAndDrops ) );
	return s->flags() & BumpAndDropSensor::BumpRight;
}

void
DriveRobot::loop()
{
	m_powerColor++;
	uint8_t leds;
	if (m_powerColor > 128)
		leds = Play;
	else
		leds = Advance;
	setLEDs(leds, m_powerColor, ((float)m_powerColor/255)*255);
	BooleanSensor* song = static_cast<BooleanSensor*> ( getSensor( Sensor::SongPlaying ) );
	if (song->value())
		return;
	ChargeSourceSensor* sources = static_cast<ChargeSourceSensor*> ( getSensor( Sensor::ChargeSources ) );
	if (sources->sources() & ChargeSourceSensor::Base) {
		UnsignedValueSensor* battery = static_cast<UnsignedValueSensor*> ( getSensor( Sensor::BatteryCharge ) );
		UnsignedValueSensor* capacity = static_cast<UnsignedValueSensor*> ( getSensor( Sensor::BatteryCapacity ) );
		IROBOT_DEBUG("Battery: " << battery->value() << "/" << capacity->value() << " " << (int)((battery->value()/(float)capacity->value())*100) << "%");
		return;
	}
	dock();
    Drive* d = getDrive();
	if (fellOffCliff()) {
		d->move(0);
		if (!song->value()) {
			playSong(1);
			delay(1000000);
		}
	} else if (reachedCliff()) {
        if (cliffSide() == Left)
            m_lastDir = Drive::Left;
        if (cliffSide() == Right)
            m_lastDir = Drive::Right;
		avoid();
	} else if ( bumped() ) {
		if ( bumpedLeft() && bumpedRight() ) {
			avoid();
		} else if ( bumpedLeft() ) {
			d->move ( Drive::Medium, Drive::Right);
			m_lastDir = Drive::Right;
            delay ( 3000 );
		} else {
			d->move ( Drive::Medium, Drive::Left );
			m_lastDir = Drive::Left;
            BooleanSensor* wall = static_cast<BooleanSensor*> ( getSensor( Sensor::Wall ) );
            do {
                updateSensor( Sensor::Wall );
            } while (wall->value());
		}
		m_speed = 30;
	} else if (!m_tryingToDock) {
		if (m_speed < Drive::Medium)
			m_speed+=20;
		d->move ( m_speed );
	}
}

void
DriveRobot::disconnected()
{
    IROBOT_DEBUG("Disconnected.");
    exit ( 0 );
}