#include <iostream>
#include <stdlib.h>
#include "rec/robotino/api2/all.h"
#include <iostream>

using namespace rec::robotino::api2;
using namespace std;
bool _run = true;

class MyCom : public Com
{
public:
	MyCom()
	    : Com( "TrafficLight" )
	{
	}

	void errorEvent( const char* errorString )
	{
		std::cerr << "Error: " << errorString << std::endl;
	}

	void connectedEvent()
	{
		std::cout << "Connected." << std::endl;
	}

	void connectionClosedEvent()
	{
		std::cout << "Connection closed." << std::endl;
	}

	void logEvent( const char* message, int level )
	{
		std::cout << message << std::endl;
	}
};

class MyBumper : public Bumper
{
public:
	MyBumper()
		: bumped( false )
	{
	}

	void bumperEvent( bool hasContact )
	{
		bumped |= hasContact;
    if (true == bumped){
      std::cout << "Bumper has " << ( hasContact ? "contact" : "no contact") << std::endl;
    }
	}

	bool bumped;
};

double x_distance = 0;
double y_distance = 0;
int phi_absolute = 0;
unsigned int sequence = 0;




class MyOdometry: public Odometry{
  void readingsEvent(double x,double y, double phi, float vx, float vy, float omega, unsigned int seq ){
	cout << "X: " << x << " Y: " << y << " Richtung: " << phi << " Sequenz: " << seq << endl;  
	x_distance = x;
	y_distance = y;
	phi_absolute = phi;
	sequence = seq;
	}
};








MyCom com;
MyBumper bumper;
OmniDrive omniDrive;
MyOdometry odo;
DigitalOutput myout;

void init( const std::string& hostname )
{

	// Connect
	std::cout << "Connecting...";
	com.setAddress( hostname.c_str() );

	com.connectToServer( true );

	if( false == com.isConnected() )
	{
		std::cout << std::endl << "Could not connect to " << com.address() << std::endl;
		rec::robotino::api2::shutdown();
		exit( 1 );
	}
	else
	{
		std::cout << "success" << std::endl;
	}
}







void drive()
{
  	odo.set(0,0,0,-1);
	

	int d = 0;
	while( com.isConnected() && false == bumper.value() && _run )
	{
	for (int i=0; i<8; i++){
		myout.setOutputNumber(i);
		myout.setValue(false);
	}
	rec::robotino::api2::msleep(100); 
    
    
	if (0 == d){
		for (int i=0; i<8; i++){
			myout.setOutputNumber(i);
			myout.setValue(false);
		}
		msleep(50);
		myout.setOutputNumber(5);
		myout.setValue(true);
		omniDrive.setVelocity(0.2, 0, 0);
		
		
		if (0.5 <= x_distance){
			odo.set(0,0,0,-1);
			for (int i=0; i<75; i++){
				for (int i=0; i<8; i++){
					myout.setOutputNumber(i);
					myout.setValue(false);
				}
				myout.setOutputNumber(7);
				myout.setValue(true);
				omniDrive.setVelocity(0,0,0);
				msleep(50);
			}
			d = 1;
		}
	}

	if (1 == d){
		for (int i=0; i<8; i++){
			myout.setOutputNumber(i);
			myout.setValue(false);
		}

		myout.setOutputNumber(5);
		myout.setValue(true);
		omniDrive.setVelocity(0, 0.2, 0);
		msleep(50);
		
		if (0.5 <= y_distance){
			odo.set(0,0,0,-1);
			for (int i=0; i<75; i++){
					for (int i=0; i<8; i++){
					myout.setOutputNumber(i);
					myout.setValue(false);
				}
				omniDrive.setVelocity(0,0,0);
				myout.setOutputNumber(7);
				myout.setValue(true);
				msleep(50);
			}
			d = 2;
		}
	}
	if (2 == d){
		for (int i=0; i<8; i++){
			myout.setOutputNumber(i);
			myout.setValue(false);
		}

		myout.setOutputNumber(5);
		myout.setValue(true);
		omniDrive.setVelocity(-0.2, 0, 0);
		msleep(50);
		
		if (-0.5 >= x_distance){
			odo.set(0,0,0,-1);
			for (int i=0; i<75; i++){
				for (int i=0; i<8; i++){
					myout.setOutputNumber(i);
					myout.setValue(false);
					}
				myout.setOutputNumber(7);
				myout.setValue(true);
				omniDrive.setVelocity(0,0,0);
				msleep(50);
			}
			d = 3;
		}
	}
	if (3 == d){
		for (int i=0; i<8; i++){
			myout.setOutputNumber(i);
			myout.setValue(false);
		}

		myout.setOutputNumber(5);
		myout.setValue(true);
		omniDrive.setVelocity(0, -0.2, 0);
		msleep(50);
		
		if (-0.5 >= y_distance){

			odo.set(0,0,0,-1);
			for (int i=0; i<75; i++){
					for (int i=0; i<8; i++){
					myout.setOutputNumber(i);
					myout.setValue(false);
				}
				myout.setOutputNumber(7);
				myout.setValue(true);
				omniDrive.setVelocity(0,0,0);
				msleep(50);
			}
			d = 0;
		}
	}

	com.processEvents();
    rec::robotino::api2::msleep(100); 

  }

}


int main( int argc, char **argv )
{
	std::string hostname = "192.168.100.174";
	if( argc > 1 )
	{
		hostname = argv[1];
	}



	//main program routine starts in here
	try
	{
		//connect to your robotino (or try at least)
		init( hostname );
		//drive (this contains a while loop)


		drive();
		//exit
		for (int i=0; i<8; i++){
			myout.setOutputNumber(i);
			myout.setValue(false);
		}
		com.disconnectFromServer();

	}
    catch( const rec::robotino::api2::RobotinoException& e )
	{
		std::cerr << "Com Error: " << e.what() << std::endl;
	}
	catch( const std::exception& e )
	{
		std::cerr << "Error: " << e.what() << std::endl;
	}
	catch( ... )
	{
		std::cerr << "Unknow Error" << std::endl;
	}

	rec::robotino::api2::shutdown();

#ifdef WIN32
	std::cout << "Press any key to exit..." << std::endl;
	rec::robotino::api2::waitForKey();
#endif
}