#include <cmath>
#include <signal.h>
#include <evolution/Resource.hpp>
#include <evolution/Util.hpp>

// Convenient typedefs for Evolution types
typedef Evolution::Result              Result;
typedef Evolution::ResourceManager     ResourceManager;
typedef Evolution::ResourceCallbackImpl ResourceCallbackImpl;
typedef Evolution::ResourceConfigParser ResourceConfigParser;
typedef Evolution::IResourceContainer  IResourceContainer;
typedef Evolution::IResource           IResource;
typedef Evolution::IDriveSystem        IDriveSystem;
typedef Evolution::CallbackId          CallbackId;
typedef Evolution::AcquirableLock      AcquirableLock;
typedef Evolution::IConditionVariable  IConditionVariable;
typedef Evolution::DeviceBusConfig     DeviceBusConfig;
typedef Evolution::DeviceList          DeviceList;
typedef Evolution::String              String;

IDriveSystem* drive = NULL;
IResourceContainer* resource_container = NULL;
ResourceManager* g_manager = NULL;
std::auto_ptr<ResourceManager> g_manager_cleanup;


void shutdown_interrupt(int signum)
{
    signal(signum, shutdown_interrupt);
}


void exit_handler (void)
{

}


int main(int argc, char** argv)
{
	ERSP_LOG_SET_ROOT_PRIORITY(LOG_INFO);

	signal (SIGINT, shutdown_interrupt);
#if !defined(EVOLUTION_PLATFORM_WIN32)
    signal(SIGCHLD, shutdown_interrupt);
    signal(SIGPIPE, shutdown_interrupt);
#endif

	std::atexit(exit_handler);

	ResourceCallbackImpl callback;

	//set up containers
    Result result = Evolution::RESULT_SUCCESS;
    ResourceManager* manager;
    manager = new ResourceManager (NULL, result);
    g_manager = manager;
    g_manager_cleanup.reset (g_manager);
    if (result != Evolution::RESULT_SUCCESS)
    {
        std::cout << "ERROR: Failed to load resource manager.\n";
        return 1;
    }

    if (manager->get_resource_container (0, &resource_container) != Evolution::RESULT_SUCCESS)
    {
        std::cout << "ERROR: Failed to get resource container from resource manager.\n";
        return 1;
    }

    if (resource_container == NULL)
    {
        std::cout << "ERROR: Got NULL resource container from resource manager.\n";
        return 1;
    }

	// Get the interface pointer.
    std::cout << "Obtaining the drive system interface\n";
    char* interface_name = "drive";
    if (resource_container->obtain_interface (0, interface_name,
                                     IDriveSystem::INTERFACE_ID,
                                     (void**) &drive) != Evolution::RESULT_SUCCESS)
    {
        std::cout << "ERROR: Resource manager failed to obtain Drived System"
                  << " '" << interface_name << "' (maybe it's not setup as a " 
                  << "Drive system?)\n";
        return (1);
    }

    if (drive == NULL) {
        std::cout << "ERROR: Drive system '" << interface_name << "' not found.\n";
        return (1);
    }


	result = drive->move_and_turn(0, 30, 15, 0, 0);
	Evolution::Platform::millisecond_sleep (1000);
	

	std::cout << "Deactivating resources\n";

    // Release allocated interfaces
    if (resource_container && drive) {
        resource_container->release_interface (0, drive);
        drive = NULL;
    }


	return 0;
}