#include "CUsbRocket.h"
#include <iostream>
#include <unistd.h>

using namespace std;

CUsbRocket::CUsbRocket() :
m_iMoveState(MOVE_STOP), m_iSpeed(SPEED_SLOW), m_hdlRock(NULL)
{

}

CUsbRocket::~CUsbRocket()
{
   CloseUsbRocket();
}

int
CUsbRocket::InitUsbRocket()
{
   struct usb_bus *busses;

   usb_init();
   usb_find_busses();
   usb_find_devices();

   busses = usb_get_busses();
   struct usb_bus *bus;
   int c, i, a;

   /* ... */

   for (bus = busses; bus; bus = bus->next)
   {
      struct usb_device *dev;

      for (dev = bus->devices; dev; dev = dev->next)
      {
         /* check for rocket */
         if (dev->descriptor.idVendor == VID && dev->descriptor.idProduct == PID)
         {
            m_hdlRock = usb_open(dev);
            cout << "open: " << m_hdlRock << endl;
            cout << "Num Config: " << (int) dev->descriptor.bNumConfigurations << endl;
            cout << "Num If: " << (int) dev->config[0].bNumInterfaces << endl;
            cout << "usb_detach_kernel_driver_np: " << usb_detach_kernel_driver_np(m_hdlRock, 0) << endl;
            cout << "usb_set_configuration: " << usb_set_configuration(m_hdlRock, 1) << endl;
            cout << "usb_claim_interface: " << usb_claim_interface(m_hdlRock, 0) << endl;
         }
      }
   }

   /*
   if (iRetval < 0)
   {
      cerr << "Init Error " << iRetval << endl; //there was an error
      return -1;

   }
   // cout << "Init usb session done...\n";

   m_hdlRock = libusb_open_device_with_vid_pid(m_ctx, VID, PID);

   // check if device was found
   if (m_hdlRock == NULL)
   {
      cerr << "Rocket device not found\n";
      return -1;
   }
   // cout << "Found Rocket\n";

   // check if device is used by kernel and remove if it is
   if (libusb_kernel_driver_active(m_hdlRock, 0))
   {
      //cerr << "Detaching from kernel\n";
      libusb_detach_kernel_driver(m_hdlRock, 0);
   }

   // setup configuration

   iRetval = libusb_set_configuration(m_hdlRock, 1);
   if (iRetval)
   {
      cerr << "Error setting configuration.\n";
   }
   // claim device
   iRetval = libusb_claim_interface(m_hdlRock, 0);
   if (iRetval)
   {
      cerr << "Error claiming device.\n";
      return -1;
   }
    */
   return 0;
}

void
CUsbRocket::CloseUsbRocket()
{
   if (m_hdlRock)
   {
      m_iMoveState = MOVE_EXIT;
      usleep(500000);
      MoveStop();
      usleep(500000);
      usb_close(m_hdlRock);
   }
   m_hdlRock = NULL;

   //   if (m_ctx)
   //      libusb_exit(m_ctx);
   //   m_ctx = NULL;
}

void
CUsbRocket::MoveUp()
{
   char ucData[] = {0x02, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
   if (m_hdlRock)
   {
      usb_control_msg(m_hdlRock, 0x21, 0x09, 0, 0, ucData, 8, 0);
   }


}

void
CUsbRocket::MoveDown()
{
   char ucData[] = {0x02, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
   if (m_hdlRock)
   {
      usb_control_msg(m_hdlRock, 0x21, 0x09, 0, 0, ucData, 8, 0);
   }
}

void
CUsbRocket::MoveLeft()
{
   char ucData[] = {0x02, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
   if (m_hdlRock)
   {
      usb_control_msg(m_hdlRock, 0x21, 0x09, 0, 0, ucData, 8, 0);
   }
}

void
CUsbRocket::MoveRight()
{
   char ucData[] = {0x02, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
   if (m_hdlRock)
   {
      usb_control_msg(m_hdlRock, 0x21, 0x09, 0, 0, ucData, 8, 0);
   }
}

void
CUsbRocket::Fire()
{
   char ucData[] = {0x02, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
   if (m_hdlRock)
   {
      usb_control_msg(m_hdlRock, 0x21, 0x09, 0, 0, ucData, 8, 0);
   }
}

void
CUsbRocket::MoveStop()
{
   char ucData[] = {0x02, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
   if (m_hdlRock)
   {
      usb_control_msg(m_hdlRock, 0x21, 0x09, 0, 0, ucData, 8, 0);
   }
}

int
CUsbRocket::StartMovement()
{
   if (InitUsbRocket() == 0)
   {
      pthread_create(&m_thread, NULL, &CUsbRocket::MoveThread, this);
      return 0;
   }
   return -1;
}

void*
CUsbRocket::MoveThread(void *ptr)
{
   //cout << "Running Move thread\n";
   CUsbRocket* pRocket = static_cast<CUsbRocket*> (ptr);
   while (pRocket->m_iMoveState != MOVE_EXIT)
   {
      switch (pRocket->m_iMoveState)
      {
         case MOVE_UP:
            pRocket->MoveUp();
            break;
         case MOVE_DOWN:
            pRocket->MoveDown();
            break;
         case MOVE_LEFT:
            pRocket->MoveLeft();
            break;
         case MOVE_RIGHT:
            pRocket->MoveRight();
            break;
         case MOVE_STOP:
            pRocket->MoveStop();
            break;
         case MOVE_FIRE:
            pRocket->MoveStop();
            pRocket->Fire();
            pRocket->m_iMoveState = MOVE_STOP;
            sleep(3);
            break;

         default:
            pRocket->m_iMoveState = MOVE_STOP;
      }

      
      switch (pRocket->m_iSpeed)
      {
         case SPEED_SLOW:
            usleep(2500);
            pRocket->MoveStop();
            usleep(20000);
            break;

         case SPEED_MEDIUM:
            usleep(10000);
            pRocket->MoveStop();
            usleep(20000);
            break;

         case SPEED_FASTEST:
            usleep(40000);
            break;

         default:
            pRocket->m_iSpeed = SPEED_SLOW;
      }
      
   }
   //cout << "Exiting Move thread\n";
   return NULL;
}

void
CUsbRocket::SetMoveDirection(MOVESTATE iDir)
{
   m_iMoveState = iDir;
}

void
CUsbRocket::SetMoveSpeed(MOVESPEED iSpeed)
{
   m_iSpeed = iSpeed;
}