// $Header: /PDIvrpn.root/2.0.0/PDIVRPN/vrpn/vrpn_Tracker_G4.h 1     6/05/12 12:21p Ben $
#ifndef VRPN_TRACKER_PDI_H
#define VRPN_TRACKER_PDI_H

// See if we have defined the use of this code in vrpn_Configure.h or in CMake
#include "vrpn_Configure.h"
#include "vrpn_Tracker.h"
#include "vrpn_Button.h"

#ifdef  VRPN_USE_PDI

#include "tchar.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <iostream>

#include "PDI.h"

#define VRPN_PDI_BUFFER_SIZE 0x1FA400   // 30 seconds of xyzaer+fc 8 sensors at 240 hz

#define VRPN_G4_HUB_NAME_SIZE 64
#define VRPN_G4_POWERTRAK_BUTTON_COUNT 4

//vrpn_Tracker_G4_HubMap_El and vrpn_Tracker_G4_HubMap
//classes are used in place of STL vector or list for listing
//ordered non-contiguous hub identifiers for management of
//digital i/o inputs on those hubs.
//These classes could easily be used for listing stylus
//button stations for other PDI trackers instead of the bitmap
//approach used now.  
class VRPN_API vrpn_Tracker_G4_HubMap_El
{
public:
	vrpn_Tracker_G4_HubMap_El( int nHub )
		: pBtnSrv(NULL), nHubID(nHub), pNext(NULL), nBtnCount(0) 
	{
		memset(BtnName, 0, sizeof(BtnName));
	};
	~vrpn_Tracker_G4_HubMap_El()
	{
		if (pBtnSrv)
			delete pBtnSrv;
	}

	void ButtonName( char * NewName )
	{
		if (!NewName)
		{}
		else
		{
			strncpy(BtnName, NewName, VRPN_G4_HUB_NAME_SIZE);
		}
	}

	void ButtonCount( int n )
	{
		nBtnCount = n;
	}


	vrpn_Tracker_G4_HubMap_El * Next()
	{ return pNext; }

	void SetNext( vrpn_Tracker_G4_HubMap_El *pn)
	{ pNext = pn; }

	friend class vrpn_Tracker_G4_HubMap;
	friend class vrpn_Tracker_G4;

private:
	int nHubID;
	vrpn_Button_Server * pBtnSrv;
	vrpn_Tracker_G4_HubMap_El * pNext;
	char BtnName[VRPN_G4_HUB_NAME_SIZE];
	int  nBtnCount;
};

typedef vrpn_Tracker_G4_HubMap_El HUBMAP_ENTRY;

class VRPN_API vrpn_Tracker_G4_HubMap
{
public:
	vrpn_Tracker_G4_HubMap():p_hub_map(NULL){};

	~vrpn_Tracker_G4_HubMap()
	{
		while (p_hub_map != NULL)
		{
			HUBMAP_ENTRY *next = p_hub_map->Next();
			delete p_hub_map;
			p_hub_map = next;
		}
	}

	void Add( int nHub )
	{
		HUBMAP_ENTRY *next = p_hub_map;
		p_hub_map = new HUBMAP_ENTRY( nHub );
		p_hub_map->SetNext(next);
	}

	HUBMAP_ENTRY * Find( int nHub )
	{
		HUBMAP_ENTRY * pH = p_hub_map;
		while (pH && (pH->nHubID != nHub))
			pH = pH->Next();
		return pH;
	}

	void ButtonInfo( int nHub, char * BtnName, int nBtnCount )
	{
		HUBMAP_ENTRY * pHub = Find( nHub );
		if (pHub)
		{
			pHub->ButtonName( BtnName );
			pHub->ButtonCount( nBtnCount );
		}
	}

	vrpn_Tracker_G4_HubMap_El * Begin()
	{
		return p_hub_map;
	}

	HUBMAP_ENTRY * p_hub_map;
};

class VRPN_API vrpn_Tracker_G4: public vrpn_Tracker {
  public:
	vrpn_Tracker_G4 (const char *name, vrpn_Connection *cn, const char *filepath,
		vrpn_float64 Hz = 10.0, const char *rcmd = NULL, vrpn_Tracker_G4_HubMap * pHMap=NULL);
   
	~vrpn_Tracker_G4(void);
	virtual void mainloop();
	int encode_to(char *buf);	
	BOOL Initialize ( VOID );
	BOOL InitDigIOBtns( void );
	BOOL Connect ( VOID );
	VOID Disconnect ( VOID );
	BOOL SetupDevice ( VOID );
	VOID UpdateStationMap ( VOID );
	BOOL DisplaySingle( timeval ct );
	BOOL StartCont ( VOID );
	BOOL StopCont ( VOID );	
	BOOL DisplayCont ( timeval ct );
	void sendCommand(char * scmd);
	void DoBoresightCmd(char *scmd);
	void DoFilterCmd(char *scmd);
	void SetFilterPreset( int nLev, CPDIfilter & f, char **pLevName );
	void DoFORCmd( char *scmd );
	void DoIncrementCmd( char *scmd );
	void DoTipOffsetCmd( char *scmd );

	void ParseG4NativeFrame( PBYTE pBuf, DWORD dwSize, timeval current_time );

  protected:
	vrpn_float64 update_rate;
	vrpn_float64 pos[3], quat[4];
	int status;
	char *cmd;	// additional commands for the tracker
	bool isCont;	// Keeps track of whether or not device is in continuous mode
	CPDIg4 pdiG4;	// PDI object for the device
	CPDImdat pdiMDat;	// Map of output format
	LPCTSTR srcCalPath;	// Filepath of the Source Calibration file
	BOOL bCnxReady;	// Keeps track of wheter the connection is active
	DWORD dwStationMap;	// map of the hubs and sensors
	ePDIoriUnits OriUnits;	// Orientaion Units e.g. Euler, Quaternion
	ePDIposUnits PosUnits;	// Positions Units e.g. Inches, Meters
	vrpn_Tracker_G4_HubMap * m_pHMap;

	HANDLE  hContEvent;
	HWND	hwnd;
	DWORD	dwOverflowCount;
	BYTE	pMotionBuf[VRPN_PDI_BUFFER_SIZE];
	PBYTE	pLastBuf;
	DWORD	dwLastSize;
};

class VRPN_API vrpn_Tracker_FastrakPDI: public vrpn_Tracker {
  public:
	vrpn_Tracker_FastrakPDI (const char * name, vrpn_Connection * cn, 
		vrpn_float64 Hz = 10, const char * rcmd = NULL, unsigned int nStylusMap = 0);
   
	~vrpn_Tracker_FastrakPDI(void);
	virtual void mainloop();
	int encode_to(char *buf);	
	BOOL Initialize ( VOID );
	BOOL InitStylusBtns();
	BOOL Connect ( VOID );
	VOID Disconnect ( VOID );
	BOOL SetupDevice ( VOID );
	VOID UpdateStationMap ( VOID );
	VOID DisplaySingle ( timeval current_time );
	BOOL StartCont ( VOID );
	BOOL StopCont ( VOID );	
	BOOL DisplayCont ( timeval current_time );
	VOID SendCommand( char *scmd );

	VOID ParseFastrakFrame ( PBYTE pBuf, DWORD dwSize, timeval current_time );

  protected:
	vrpn_float64 update_rate;
	vrpn_float64 pos[3], d_quat[4];
	int status;
	char *cmd; // pointer to individual command, member of rcmd, for tracker
	BOOL isBinary; // keeps track of whether or not the device is in binary mode
	BOOL isCont; // Keeps track of whether or not device is in continuous mode
	BOOL isMetric; // tracks pos units
	CPDIfastrak	pdiDev; // PDI object for the device
	CPDImdat    pdiMDat; // Map of output format
	CPDIser		pdiSer; // Serial connection object for the device
	BOOL		bCnxReady; // Keeps track of whether the connection is active
	DWORD		dwStationMap; // Map of the hubs and sensors
	DWORD		m_nStylusMap; // Map of sensors with stylus
	int			m_nHeaderSize;// Num byte in P&O frame header
	int			m_nFrameSize; // Num bytes in entire P&O frame (inc header)
	vrpn_Button_Server	* FTstylusBtns[FT_MAX_SENSORS];	//< Pointer to button on each sensor (NULL if none)

	HANDLE  hContEvent;
	HWND	hwnd;
	DWORD	dwOverflowCount;
	BYTE	pMotionBuf[VRPN_PDI_BUFFER_SIZE];
	PBYTE	pLastBuf;
	DWORD	dwLastSize;
};

class VRPN_API vrpn_Tracker_LibertyPDI: public vrpn_Tracker {
  public:
	vrpn_Tracker_LibertyPDI (const char * name, vrpn_Connection * cn, 
		vrpn_float64 Hz = 10, const char * rcmd = NULL, unsigned int nStylusMap = 0);
   
	~vrpn_Tracker_LibertyPDI(void);
	virtual void mainloop();
	int encode_to(char *buf);	
	BOOL Initialize ( VOID );
	BOOL InitStylusBtns();
	BOOL Connect ( VOID );
	VOID Disconnect ( VOID );
	BOOL SetupDevice ( VOID );
	VOID UpdateStationMap ( VOID );
	VOID DisplaySingle ( timeval current_time );
	BOOL StartCont ( VOID );
	BOOL StopCont ( VOID );	
	BOOL DisplayCont ( timeval current_time );
	VOID SendCommand( char *scmd );

	VOID ParseLibertyFrame ( PBYTE pBuf, DWORD dwSize, timeval current_time );

  protected:
	vrpn_float64 update_rate;
	vrpn_float64 pos[3], d_quat[4];
	int status;
	char *cmd; // pointer to individual command, member of rcmd, for tracker
	BOOL isBinary; // keeps track of whether or not the device is in binary mode
	BOOL isCont; // Keeps track of whether or not device is in continuous mode
	BOOL isMetric; // tracks pos units
	CPDIdev		pdiDev; // PDI object for the device
	CPDImdat    pdiMDat; // Map of output format
	CPDIser		pdiSer; // Serial connection object for the device
	BOOL		bCnxReady; // Keeps track of whether the connection is active
	DWORD		dwStationMap; // Map of the hubs and sensors
	DWORD		m_nStylusMap; // Map of sensors with stylus
	int			m_nHeaderSize;// Num byte in P&O frame header
	int			m_nFrameSize; // Num bytes in entire P&O frame (inc header)

	vrpn_Button_Server	* StylusBtns[LIBERTY_MAX_SENSORS];	//< Pointer to button on each sensor (NULL if none)

	HANDLE  hContEvent;
	HWND	hwnd;
	DWORD	dwOverflowCount;
	BYTE	pMotionBuf[VRPN_PDI_BUFFER_SIZE];
	PBYTE	pLastBuf;
	DWORD	dwLastSize;
};

#endif
#endif
