/**
 * Glove.cpp - DG5Glove Interface
 * CSE 490I Neurobotics Winter 2007
 * Josh Storz <jstorz@cs.washington.edu>
 */

#include "Glove.h"

Glove::Glove()
{
  // initialize with default settings
  hcomm = NULL;
  glove = NULL;
  comm_port = 1;
  settings.COMPort = comm_port;
  glove_type = GLOVE_RIGHT;
  settings.GloveType = GLOVE_RIGHT;
  settings.Filter = 0;

  for (int i = 0; i < 5; i++)
  {
    settings.FingerMinValue[i] = 0;
    settings.FingerMaxValue[i] = 1023;
  }

  glove = LoadLibrary("DG5GloveLib.dll");
}

Glove::~Glove()
{
  // free comm port
  if (isConnected())
    close();
  if (hcomm != NULL)
    CloseHandle(hcomm);

  // free dll
  if (libLoaded())
    FreeLibrary(glove);
}

bool Glove::open()
{
  if (!libLoaded() || isConnected())
    return false;

  if ((hcomm = VHand_OpenComPort(comm_port)) == NULL)
    return false;
  
  // send hw command to start data capture
  if (!sendCmd(CMD_START))
  {
    close();
    return false;
  }

  return true;
}

bool Glove::close()
{
  if (!libLoaded() || !isConnected())
    return false;

  // send hw command to stop data capture
  sendCmd(CMD_STOP);
  
//  if (!VHand_CloseComPort(hcomm))
  {
    CloseHandle(hcomm);
    hcomm = NULL;
  }

  return true;
}

void Glove::setComm(int port)
{
  if (!libLoaded() || isConnected())
    return;
  
  comm_port = port;
  settings.COMPort = port;
}

int Glove::getComm()
{
  if (!libLoaded())
    return -1;
  else
    return comm_port;
}

void Glove::setCalib(Glove::GloveCalib calib)
{
  this->calib = calib;
}

Glove::GloveCalib Glove::getCalib()
{
  return calib;
}

void Glove::setType(int type)
{
  if (!libLoaded() || isConnected())
    return;
  if ((type != GLOVE_RIGHT) || (type != GLOVE_LEFT))
    return;

  glove_type = type;
  settings.GloveType = type;
}

int Glove::getType()
{
  if (!libLoaded())
    return -1;

  return glove_type;
}

bool Glove::getState(Glove::GloveData *frame)
{
  if (!libLoaded() || !isConnected())
    return false;
  
  // grab data frame
  if (!VHand_ReadGloveData(hcomm, buffer, BLOCK_LENGTH, &state, &settings, &verify_code))
    return false;

  for (int i = 0; i < 5; i++)
    frame->finger_pos[i] = normalize(i);

  for (int i = 0; i < 3; i++)
    frame->button_state[i] = state.Buttons[i];
  
  return true;
}

// interactive calibration method
bool Glove::calibrate(GloveCalib *calib)
{
  if (!libLoaded() || !isConnected() || calib == NULL)
    return false;

  // start with artificially high and low values
  float min[5] = { 32768.0,  32768.0,  32768.0,  32768.0,  32768.0};
  float max[5] = {-32768.0, -32768.0, -32768.0, -32768.0, -32768.0};

  GloveData frame;

  // work on approximately 5 seconds of input data
  for (int i = 0; i < 500; i++)
  {
    if (!getState(&frame))
      return false;

    // keep mins and maxs as data comes in
    for (int i = 0; i < 5; i++)
    {
      if (state.Finger[i] < settings.FingerMinValue[i] || state.Finger[i] > settings.FingerMaxValue[i])
        continue;
      if (state.Finger[i] < min[i])
        min[i] = state.Finger[i];
      if (state.Finger[i] > max[i])
        max[i] = state.Finger[i];
    }

    Sleep(10);
  }

  // store calibration data
  for (int i = 0; i < 5; i++)
  {
//    calib->offset[i] = -min[i];
    calib->min[i] = min[i]; // + calib->offset[i];
    calib->max[i] = max[i]; // + calib->offset[i];
  }

  return true;
}