#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>
#include <stdlib.h>
  
  
#include "rec/robotino/api2/all.h"
  
using namespace rec::robotino::api2;
  
float _rect[2]= {0,0};
  
bool _run = true;
  
  
class MyCom : public Com
{
    public:
  	MyCom()
  		: Com( "rect" )
  	{
  	}
  
  	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;
  	}
  
  	void pingEvent( float timeMs )
  	{
  		//std::cout << "Ping: " << timeMs << "ms" << std::endl;
  	}
};
  
class MyBumper : public Bumper
{
    public:
  	MyBumper()
  		: bumped( false )
  	{
  	}
  
  	void bumperEvent( bool hasContact )
  	{
  		bumped |= hasContact;
  		std::cout << "Bumper has " << ( hasContact ? "contact" : "no contact") << std::endl;
  	}
  
  	bool bumped;
};
    
  MyCom com;
  MyBumper bumper;
  
void init( const std::string& hostname )
{
    // Initialize the actors
  
    // 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()
{
     
}
  
void destroy()
{
    com.disconnectFromServer();
}
  
int main( int argc, char **argv )
{
  
    std::string hostname = "172.26.1.1";
   
    if( argc > 1 )
    {
  	hostname = argv[1];
    }
  
    try
    {
  	init( hostname );
  	drive();
  	destroy();
    }
    
    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();
}