// Filename: input/device/isotrak/Isotrak.cpp
// Author: Jorge Bernadas (JB)
// Date of Creation: 11/02/2007
// Version: 1.0
/*#include "base/General.h"
#include "base/Check.h"
#include "base/Limits.h"
#include "base/Logging.h"
#include "base/singleton/Singleton-inl.h"
#include "input/device/isotrak/Isotrak.h"
#include "base/concurrent/Thread.h"
*/
#include "Isotrak.h"
#include "../../../Base/LogManagement.h"

BEGIN_TEG_NAMESPACE();
//BEGIN_ENGINE_NAMESPACE();

// scale factors used by the isotrak to decode binary input
const float Isotrak::kInchesScaleFactor = 65.48f;
const float Isotrak::kCentimetersScaleFactor = 166.32f;
const float Isotrak::kDegreesScaleFactor = 229.18f;
const float Isotrak::kRadiansScaleFactor = 4.0f;
const float Isotrak::kCosinesScaleFactor = 1.0f;
const float Isotrak::kQuaternionScaleFactor = 1.0f;

Isotrak::Isotrak() : receiverFlags_(kBothReceiversBits) {
}

Isotrak::~Isotrak() {
}

bool Isotrak::Connect(const _TCHAR* port, int rate) {
  // connect using TSerial and check for errors
  if (!tserial_.Connect(port, rate, ext::TSerial::NO_PARITY))
    return false;

  // some wired stuff
  ResetSystem();

//  const char kControlD = static_cast<char>(4);
//  tserial_.SendArray(&kControlD, 1);  // set ISOTRAK II compatibility mode
//  tserial_.SendArray("Y", 1);         // set digitizer mode
//  tserial_.SendArray("e", 1);         // set RUN mode

  ResetAlignment();
  SetQuietMode(false);              // set non-quiet mode
  tserial_.SendArray("f", 1);       // set binary format
  tserial_.SendArray("c", 1);       // set non-continuous mode
  tserial_.SendArray("u", 1);       // set centimeter units
  tserial_.SendArray("O2,4\r", 5);  // set output format

  // no error found during the connection
	return true;
}

void Isotrak::Disconnect() {
  // disconnect from the serial port
  tserial_.Disconnect();
}

void Isotrak::ResetSystem() {
  // issue a ^Y instruction and sleep to wait for system restart
  static const int kSleepTime = 5000;
  //VLOG(ISOTRAK, 3, _T("Resetting Isotrak system, sleeping for %d msecs.\n"), kSleepTime);
  PRINT_LOG(TYPE_WARNING, _T("ISOTRACK: Resetting Isotrak system, sleeping for %d msecs.\n"), kSleepTime);
  const char kControlY = static_cast<char>(25);
  tserial_.SendArray(&kControlY, 1);
  ::Sleep(kSleepTime);
}

void Isotrak::ToggleReceivers(int toggleFlags) {
  // check each bit and turn on/off each station
  if (toggleFlags & kReceiverOneBit)
    tserial_.SendArray("l1\r", 3);
  if (toggleFlags & kReceiverTwoBit)
    tserial_.SendArray("l2\r", 3);
}

void Isotrak::EnableReceivers(int enableFlags) {
  // calculate the stations to turn on and set new flags
  const int turnOnFlags = enableFlags & ~receiverFlags_;
  receiverFlags_ |= enableFlags;
  ToggleReceivers(turnOnFlags);
}

void Isotrak::DisableReceivers(int disableFlags) {
  // calculate the stations to turn on and set new flags
  const int turnOffFlags = disableFlags & receiverFlags_;
  receiverFlags_ &= ~disableFlags;
  ToggleReceivers(turnOffFlags);
}

void Isotrak::SetQuietMode(bool enabled) {
  // check the flag and enable or disable accordingly
  if (enabled) EnableQuietMode();
  else DisableQuietMode();
}

void Isotrak::EnableQuietMode() {
  // send the instruction to the isotrak
  const char kQuietCommand = 'K';
  tserial_.SendArray(&kQuietCommand, 1);
}

void Isotrak::DisableQuietMode() {
  // send the instruction to the isotrak
  const char kNotQuietCommand = 'm';
  tserial_.SendArray(&kNotQuietCommand, 1);
}

void Isotrak::ResetAlignment() {
  // send the instruction to the isotrak device
  tserial_.SendArray("R1\r", 3);
}

void Isotrak::SetAlignment(const Vec3f& origin, const Vec3f& xAxis, const Vec3f& yAxis) {
  // TODO(jbernadas): Set the alignment here!!!
}

bool Isotrak::ReadData() {
  // request data from the isotrak and read the receivers positions
  tserial_.SendArray("P", 1);
  for (int k = 0; k < 2; ++k) if (receiverFlags_ & (1 << k)) {
    // read 15 bytes with the record and check if it is valid data
    const int kRecordSize = 15;
    char buffer[kRecordSize];
    tserial_.GetArray(buffer, kRecordSize);
    if (buffer[0] == '0') {
      // get the data's station from the 2-nd byte
      const int station = buffer[1] - '1';
      Receiver& receiver = receiver_[station];

      // decode position information and put it in the receiver
      receiver.setX(DecodeBinary(buffer + 3, kCentimetersScaleFactor));
      receiver.setY(DecodeBinary(buffer + 5, kCentimetersScaleFactor));
      receiver.setZ(DecodeBinary(buffer + 7, kCentimetersScaleFactor));

      // decode orientation information and put it in the receiver
      receiver.setAzimuth(DecodeBinary(buffer + 9, kDegreesScaleFactor));
      receiver.setElevation(DecodeBinary(buffer + 11, kDegreesScaleFactor));
      receiver.setRoll(DecodeBinary(buffer + 13, kDegreesScaleFactor));

      // decode button information and put it in the receiver
      if (buffer[2] == '@') receiver.setButtonPressed(true);
      else if (buffer[2] == '*') receiver.setButtonPressed(false);

    /*  VLOG(ISOTRAK, 5, _T("Station     = %d\n"), station);
      VLOG(ISOTRAK, 5, _T("Position    = {%.4f, %.4f, %.4f}\n"), receiver.x(), receiver.y(), receiver.z());
      VLOG(ISOTRAK, 5, _T("Orientation = {%.4f, %.4f, %.4f}\n"), receiver.azimuth(), receiver.elevation(), receiver.roll());
      VLOG(ISOTRAK, 5, _T("Button      = %s\n"), receiver.buttonPressed() ? _T("true") : _T("false"));
      VLOG(ISOTRAK, 5, "Button Bit  = %c\n", buffer[2]);
    */
	  PRINT_LOG(TYPE_WARNING, _T("ISOTRAK: Station     = %d"), station);
      PRINT_LOG(TYPE_WARNING, _T("ISOTRAK: Position    = {%.4f, %.4f, %.4f}"), receiver.x(), receiver.y(), receiver.z());
      PRINT_LOG(TYPE_WARNING, _T("ISOTRAK: Orientation = {%.4f, %.4f, %.4f}"), receiver.azimuth(), receiver.elevation(), receiver.roll());
      PRINT_LOG(TYPE_WARNING, _T("ISOTRAK: Button      = %s"), receiver.buttonPressed() ? _T("true") : _T("false"));
      PRINT_LOG(TYPE_WARNING, _T("ISOTRAK: Button Bit  = %c"), buffer[2]);
    
	} else {
      //VLOG(ISOTRAK, 5, "%c%c%c\n", buffer[0], buffer[1], buffer[2]);
		PRINT_LOG(TYPE_WARNING, _T("ISOTRAK: %c%c%c"), buffer[0], buffer[1], buffer[2]);
    }
    //VLOG(ISOTRAK, 5, _T("\n"));
  }
  return true;
}

Receiver& Isotrak::receiver(int index) {
  // check the receiver index and return
  //CHECK(index >= 0 && index < kReceiverCount, _T("Error while trying to get receiver ")
  //  _T("from Isotrak device, index is out of bounds (index=%d).\n"), index);
  //LOG_IF(WARNING, !(receiverFlags_ & (1 << index)), _T("Trying to read receiver from ")
  //  _T("disabled receiver (index=%d, receiverFlags=%d).\n"), index, receiverFlags_);

  if (!(index >= 0 && index < kReceiverCount)) {
    PRINT_LOG(TYPE_WARNING, _T("Error while trying to get receiver from Isotrak device, index is out of bounds (index=%d)."), index);
  }
  if (!(receiverFlags_ & (1 << index))) {
    PRINT_LOG(TYPE_WARNING, _T("Trying to read receiver from disabled receiver (index=%d, receiverFlags=%d)."), index, receiverFlags_);
  }  
  return receiver_[index];
}

const Receiver& Isotrak::receiver(int index) const {
  // check the receiver index and return
/*  CHECK(index >= 0 && index < kReceiverCount, _T("Error while trying to get receiver ")
    _T("from Isotrak device, index is out of bounds (index=%d).\n"), index);
  LOG_IF(WARNING, !(receiverFlags_ & (1 << index)), _T("Trying to read receiver from ")
    _T("disabled receiver (index=%d, receiverFlags=%d).\n"), index, receiverFlags_);
*/
  if (!(index >= 0 && index < kReceiverCount)) {
    PRINT_LOG(TYPE_WARNING, _T("Error while trying to get receiver from Isotrak device, index is out of bounds (index=%d)."), index);
  }
  if (!(receiverFlags_ & (1 << index))) {
    PRINT_LOG(TYPE_WARNING, _T("Trying to read receiver from disabled receiver (index=%d, receiverFlags=%d)."), index, receiverFlags_);
  }  	
  return receiver_[index];
}

float Isotrak::DecodeBinary(const char* ptr, float scaleFactor) {
  // maximum values for a variable of 16 bits
  static const unsigned short kMaxInt16 = +32767;

  // get the value from the buffer and re-scale it
  const unsigned short value = ((static_cast<unsigned short>(ptr[1]) & 0xff) << 8) |
                       ((static_cast<unsigned short>(ptr[0]) & 0xff) << 0);
  return static_cast<unsigned short>(value) * (scaleFactor / kMaxInt16);
}

//END_ENGINE_NAMESPACE();
END_TEG_NAMESPACE();