//###################################################################################
//#						                        Glove Object									                #
//#						                Used To Handle a 5DT Glove							              #
//#						                          Author : 							                      #
//#						                      Aymeric SUTEAU									                #
//#						                       LISA - ANGERS									                #
//###################################################################################


/* HEADER INCLUDE */
#include "Glove.h"
#include <string.h>


/* GLOBAL VARIABLES */
// Thresholds for calibration procedure
float f_thresholdUpperCoeff = 0.3f, f_thresholdLowerCoeff = 0.3f;


/*------------------------------- CALLBACK FUNCTION -------------------------------*/
void GloveCallback(LPVOID param) 
{
	// Cast void pointer to GloveObject type before using
	GloveObject *pGloveObject = (GloveObject*) param;
	// If we retrieve a valid pointer, we call the function which will update the sensors values
	if (pGloveObject)
		pGloveObject->UpdateGlove();
}


/*-------------------------- CONSTRUCTOR AND DESTRUCTOR ---------------------------*/
GloveObject::GloveObject() : Thread() 
{
	SetGlove(NULL);
	SetPort(NULL);
	SetShowRaw(false);
	SetGloveType(FD_GLOVENONE);
	SetType("?");
	SetNbrOfSensors(-1);
	SetGestureIndex(-1);
  SetCalibrationDone(true);
  SetInitValuesDone(true);
  SetInitDone(false);
  SetUSBIndex(0);
  SetGestureMode(GESTURE_WITH_THUMB);

  // Thread handling
  this->start();
  this->isRunning = true;
}

GloveObject::~GloveObject() 
{
  // Destroy dynamic table containing data glove serial number 
  //delete [] szSerialNumber;  

  // Thread handling
  this->isRunning = false;
  this->stop();
}


/*------------------------------ GETTERS AND SETTERS ------------------------------*/
fdGlove * GloveObject::GetGlove() 
{
	return this->pGlove;
}

void GloveObject::SetGlove(fdGlove * glove)
{
	this->pGlove = glove;
}

char * GloveObject::GetPort() 
{
	return this->szPort;
}

void GloveObject::SetPort(char * port) 
{
	this->szPort = port;
}

bool GloveObject::IsShowRaw() 
{
	return this->bShowRaw;
}

void GloveObject::SetShowRaw(bool showRaw) 
{
	this->bShowRaw = showRaw;
}

int GloveObject::GetGloveType() 
{
	return this->iGloveType;
}

void GloveObject::SetGloveType(int gloveType) 
{
	this->iGloveType = gloveType;
}

char * GloveObject::GetType() 
{
	return this->szType;
}

void GloveObject::SetType(char * type) 
{
	this->szType = type;
}

int GloveObject::GetGloveHand() 
{
	return this->iGloveHand;
}

void GloveObject::SetGloveHand(int gloveHand) 
{
	this->iGloveHand = gloveHand;
}

char * GloveObject::GetHand() 
{
	return this->szHand;
}

void GloveObject::SetHand(char * hand) 
{
	this->szHand = hand;
}

int GloveObject::GetNbrOfSensors() 
{
	return this->iNbrOfSensors;
}

void GloveObject::SetNbrOfSensors(int nbSensors) 
{
	this->iNbrOfSensors = nbSensors;
}

std::string GloveObject::GetSerialNumber() 
{
  return this->szSerialNumber;
}

void GloveObject::SetSerialNumber(std::string serialNumber) 
{
  this->szSerialNumber = serialNumber;
}

bool GloveObject::GetCalibrationDone()
{
  return this->bCalibrationDone;
}

void GloveObject::SetCalibrationDone(bool status) 
{
  this->bCalibrationDone = status;
}

bool GloveObject::GetInitValuesDone() 
{
  return this->bInitValuesDone;
}

void GloveObject::SetInitValuesDone(bool status) 
{
  this->bInitValuesDone = status;
}

bool GloveObject::GetInitDone() 
{
  return this->bInitDone;
}

void GloveObject::SetInitDone(bool status) 
{
  this->bInitDone = status;
}

int GloveObject::GetUSBIndex() 
{
  return this->iUSBIndex;
}

void GloveObject::SetUSBIndex(int index) 
{
  this->iUSBIndex = index;
}

int GloveObject::GetGestureMode() 
{
  return this->iGestureMode;
}

void GloveObject::SetGestureMode(int gestureMode) 
{
  this->iGestureMode = gestureMode; 
}


/*----- Return gesture index between 0 and 15 (thumb not handled) -----*/
int GloveObject::GetGestureIndex() 
{
	return this->iGestureIndex;
}

void GloveObject::SetGestureIndex(int gestureIndex) 
{
	this->iGestureIndex = gestureIndex;
}


/*----- Return new gesture index between 0 and 31 (thumb handled) -----*/
int GloveObject::GetGestureWithThumbIndex() 
{
  int *pIndexValues, gestureValue = 0, thresholdDiff = 0;
  pIndexValues = new int[GetNbrOfSensors()];

  // Retrieve the current sensors values and the thresholds defined during auto calibration
  fdGetSensorRawAll(GetGlove(), pValues);
  fdGetCalibrationAll(GetGlove(), pMax, pMin);
  
  // Define a new index position (boolean) for each finger
  for (int i=0; i<GetNbrOfSensors(); i++) 
  {
	  // Define new thresholds
    thresholdDiff = pMax[i] - pMin[i];
    pMax[i] -= (int)(f_thresholdUpperCoeff*thresholdDiff);
    pMin[i] += (int)(f_thresholdLowerCoeff*thresholdDiff);
	
	  // Adjust thresholds with a coefficient
    // Normal position : -1
    if (pValues[i] >= pMin[i] && pValues[i] <= pMax[i]) 
    {
      pIndexValues[i] = -1;
    }
	
    // Closed finger : 0
    else if (pValues[i] < pMin[i]) 
    {
      pIndexValues[i] = 0;
    }
	
    // Opened finger : 1
    else if (pValues[i] > pMax[i]) 
    {
      pIndexValues[i] = 1;
    }
  }
  
  // Define new gesture index (-1 if normal gesture, otherwise between 0 and 31)
  if (pIndexValues[FD_THUMBNEAR] == -1 || pIndexValues[FD_INDEXNEAR] == -1 || pIndexValues[FD_MIDDLENEAR] == -1 || pIndexValues[FD_RINGNEAR] == -1  
                                       || pIndexValues[FD_LITTLENEAR] == -1) 
  {
    // Unrecognizable gesture
    gestureValue = -1;
  }
  
  // Otherwise, combine the index values of each finger
  else 
  {
    // Index between 0 and 31 (32 gestures are possible)
    gestureValue = pIndexValues[FD_THUMBNEAR] + 2*pIndexValues[FD_INDEXNEAR] + 4*pIndexValues[FD_MIDDLENEAR] + 8*pIndexValues[FD_RINGNEAR]
                                              + 16*pIndexValues[FD_LITTLENEAR];
  }
  return gestureValue;
}

void GloveObject::SetGestureWithThumbIndex(int gestureWithThumbIndex) 
{
	this->iGestureTHIndex = gestureWithThumbIndex;
}


/*-------------------------------- ADDITIONAL FUNCTIONS ---------------------------------*/
/*----- Initialize glove object -----*/
bool GloveObject::Init(int gestureMode) 
{
  // Check for any available USB glove
	unsigned short aPID[5];
	int nNumFound = 5, nChosen = 0;
  char portName[10] = "USB";
  bool bInitOk = false;
  //szSerialNumber = new char[12];

  // Scan USB ports
  fdScanUSB(aPID, nNumFound);
  // DEBUG : MMechostr(MSKDEBUG, "-> Init ...gloves found : %d\n", nNumFound);
  for (int i=0; i<nNumFound; i++)
  {
    // Update the name of current USB port
    sprintf(portName, "USB%d", i);

		// Try to open the first device of the list, else try to open the next one
    SetGlove(fdOpen(portName, true));
    if (!GetGlove()) 
    {
      // Initialization failed : data glove already opened
      MMechostr(MSKDEBUG, "-> Init ...failed : data glove n.%d already opened\n", i);
    }
    else 
    {
      // Set USB port and index
      SetPort(portName);
      SetUSBIndex(i);
      MMechostr(MSKDEBUG, "-> Init ...successful\n");
      bInitOk = true;
      break;
    }
	}

  // If no gloves have been found, we must exit the function
  if (!bInitOk) 
  {
    MMechostr(MSKDEBUG, "-> Init ...failed : couldn't open any data glove\n");
    return false;
  }

  // Retrieve the data glove serial number
  char tmpserial[13];
  fdGetSerialNumber(GetGlove(), tmpserial);
  MMechostr(MSKDEBUG, "Glove 5DT serial : %s", tmpserial);
  szSerialNumber = tmpserial;
  SetSerialNumber(szSerialNumber);

  // Disable auto calibration (enabled by default)
  fdSetAutoCalibrate(GetGlove(), false);

  // Initialize data glove gesture mode
  SetGestureMode(gestureMode);

  // Set Glove Type and Hand Type
	SetGloveType(fdGetGloveType(pGlove));
	switch (GetGloveType()) 
  {
		// Not a Glove : we consider that the initialization of the device was a failure
		case FD_GLOVENONE: 
			SetType("None");
			return false; 
			break;

		// Get Glove Type
		case FD_GLOVE7:    
			SetType("5DT Glove"); 
			break;

		case FD_GLOVE7W:   
			SetType("5DT Wireless Glove"); 
			break;

		case FD_GLOVE16:   
			SetType("16DT Glove");
			break;

		case FD_GLOVE16W:  
			SetType("16DT Wireless Glove"); 
			break;		

		case FD_GLOVE5U: 
			SetType("DG5 Ultra Series"); 
			break;

		case FD_GLOVE5UW: 
			SetType("DG5 Wireless Ultra Series"); 
			break;

		case FD_GLOVE5U_USB: 
			SetType("DG5 Ultra USB");
			break;

		case FD_GLOVE14U: 
			SetType("DG14 Ultra Series"); 
			break;

		case FD_GLOVE14UW: 
			SetType("DG14 Wireless Ultra Series"); 
			break;

		case FD_GLOVE14U_USB: 
			SetType("DG14 Ultra USB"); 
			break;		
	}
	SetGloveHand(fdGetGloveHand(pGlove));
	if (GetGloveHand() == FD_HAND_RIGHT)
		SetHand("Right");
	else
		SetHand("Left");

	// Set Number of Glove Sensors
	SetNbrOfSensors(fdGetNumSensors(pGlove));

	// Init successful
  SetInitDone(true);
	return true;
}


/*----- Initialize data glove sensors values -----*/
void GloveObject::InitValues() 
{
	// Get Number of Values to Retrieve
	pValues = new unsigned short[GetNbrOfSensors()];
	pMax = new unsigned short[GetNbrOfSensors()];
	pMin = new unsigned short[GetNbrOfSensors()];

	// Define Minimum and Maximum Values Before Calibration
	for (int i=0; i<GetNbrOfSensors(); i++) 
  {
		pMin[i] = 0;
		pMax[i] = 4095;
	}

  // Update initialization procedure status
  SetInitValuesDone(true);
}


/*----- Set callback to be called when a new packet is received by the driver -----*/
void GloveObject::SetCallback() 
{
	// Set the update function callback (once the calibration is done)
	fdSetCallback(GetGlove(), (void*)GloveCallback, this);
}


/*----- Delete callback -----*/
void GloveObject::DeleteCallback() 
{
	// Delete the callback function previously added (in order to stop the acquisition of sensors values)
	fdSetCallback(GetGlove(), NULL, NULL);
}


/*----- To be called by glove callback function -----*/
void GloveObject::UpdateGlove() 
{
	double angleX = 0.0, angleY = 0.0;
	// If and only if a valid pointer to the glove device is available
	if (GetGlove()) 
  {
		fdGetSensorRawAll(GetGlove(), pValues);		            // Update sensors values

    // Retrieve the current gesture according to the gesture mode
    if (GetGestureMode() == 0) 
    {
		  SetGestureIndex(fdGetGesture(GetGlove()));	          // Retrieve and set current gesture index
		  PostMessage(HScol, GLOVE_NEWDATA_WITHOUT_THUMB_CB, (int)this, (LPARAM)NULL);
    }
    else 
    {
      SetGestureWithThumbIndex(fdGetGesture(GetGlove()));   // Retrieve and set current gesture index (with thumb)
      PostMessage(HScol, GLOVE_NEWDATA_CB, (int)this, (LPARAM)NULL);
    }

		// Notify Scol window that a new hand gesture has been made
		PostMessage(HScol, GLOVE_HAND_CB, (int)this, (LPARAM)NULL);
	}
}


/*----- Close glove -----*/
int GloveObject::Close() 
{
	int iErrorCode = fdClose(GetGlove());   // Free the glove device and communication port
  SetGlove(NULL);                         // Make sure the pointer is reset
  return iErrorCode;
}


/*----- Calibrate automatically or manually the glove -----*/
void GloveObject::AutoCalibrate(bool bAuto) 
{
  time_t startTime, currentTime;

  // Notify Scol window that the calibration has started
  PostMessage(HScol, GLOVE_CALIBRATION_START_CB, (int)this, (LPARAM)NULL);

	// ---------- Case 1 : Auto Calibration
  if (bAuto) 
  {
    MMechostr(MSKDEBUG, "-> AutoCalibrate ...calibration mode : AUTO\n");
    // Reset calibration values
	  for (int i=0; i<18; i++) 
    {
      pMax[i] = 0;
			pMin[i] = 4095;
	  }
    fdSetCalibrationAll(GetGlove(), pMax, pMin);

    // Enable auto calibration
    fdSetAutoCalibrate(GetGlove(), TRUE);

	  // Start calibration : the process will last 5 seconds
    // During this period, the user must open/close his hand several times to complete calibration process
	  time(&startTime);
	  do 
    {
		  time(&currentTime);
	  } while ((currentTime - startTime) < 5);

    // Disable auto calibration
    fdSetAutoCalibrate(GetGlove(), FALSE);
  }

	// ---------- Case 2 : Manual Calibration
	else 
  {
    MMechostr(MSKDEBUG, "-> AutoCalibrate ...calibration mode : MANUAL\n");
		// Default Values to Start Calibration
		for (int i=0; i<GetNbrOfSensors(); i++) 
    {
			pMax[i] = 0;
			pMin[i] = 4095;
		}

		// Get Calibration Start Time
		time(&startTime);
		do 
    {
			// Get Sensors Values
			fdGetSensorRawAll(GetGlove(), pValues);

			// For each value, we get the minimum and maximum
			for (int i=0; i<GetNbrOfSensors(); i++)
      {
				if(pValues[i]!=0) {
					if (pValues[i] > pMax[i])
						pMax[i] = pValues[i];
					
					if (pValues[i] < pMin[i]) 
						pMin[i] = pValues[i];
				}
			}

			// Get Current Time
			time(&currentTime);

		} while ((currentTime - startTime) < 5);
	}

  // Notify Scol window that the calibration has ended
  PostMessage(HScol, GLOVE_CALIBRATION_END_CB, (int)this, (LPARAM)NULL);

  // Update calibration status
  MMechostr(MSKDEBUG, "-> AutoCalibrate ...calibration done for %s.\n", GetType());
  SetCalibrationDone(true);
}


/*----- Thread handling -----*/
void GloveObject::run() 
{
  try 
  {
    while(isRunning) 
    {
      // TODO : Handle USB disconnection of the data glove
      // Will be implemented by 5DT Technical support in a few days

      // Calibration
      if (!GetCalibrationDone()) 
      {
        AutoCalibrate(true);            
      }

      // Initialize data glove sensors values
      else if (!GetInitValuesDone()) 
      {
        InitValues();
      }
    }
    // Release connection to the data glove
    if (Close() == 0) 
    {
      SetGlove(NULL);
    }
  }
  catch (ThreadException ex) 
  {
    MMechostr(MSKDEBUG, "error thread : %s\n", ex.getMessage().c_str());
  }		
}