//======================================================================================================
// Copyright 2022, NaturalPoint Inc.
//======================================================================================================
#include "dllcommon.h"
#include "GloveDeviceBase.h"

// Optitrack Peripheral Device API
#include "AnalogChannelDescriptor.h"
#include "IDeviceManager.h"
#include "GloveDataFormat.h"
using namespace AnalogSystem;
using namespace OptiTrackPluginDevices;
using namespace GloveDeviceProperties;

OptiTrackPluginDevices::GloveDeviceBase::GloveDeviceBase()
{
	this->SetCommonDeviceProperties();
	this->SetCommonGloveDeviceProperties();
}

void OptiTrackPluginDevices::GloveDeviceBase::SetCommonDeviceProperties()
{
	// Set appropriate default property values (and set advanced state)
	ModifyProperty(cPluginDeviceBase::kModelPropName,				 true, true, false);
	ModifyProperty(cPluginDeviceBase::kOrderPropName,				 true, false, true);
	ModifyProperty(cPluginDeviceBase::kDisplayNamePropName,			 false, false, false);
	
	// Reveal glove related prop name
	ModifyProperty(cPluginDeviceBase::kAssetPropName,				 false, false, false); // name of the paired skeleton asset

	// hide default properties that aren't relevant to glove device
	ModifyProperty(cPluginDeviceBase::kSyncModePropName,				 true, true, true);
	ModifyProperty(cPluginDeviceBase::kSyncStatusPropName,			 true, true, true);
	ModifyProperty(cPluginDeviceBase::kUseExternalClockPropName,		 true, true, true);
	ModifyProperty(cPluginDeviceBase::kMocapRateMultiplePropName,	 true, true, true);
	ModifyProperty(cPluginDeviceBase::kScalePropName,				 true, true, true);
	ModifyProperty(cPluginDeviceBase::kCalOffsetPropName,			 true, true, true);
	ModifyProperty(cPluginDeviceBase::kCalSquareRotationPropName,	 true, true, true);
	ModifyProperty(cPluginDeviceBase::kUserDataPropName,				 true, true, true);
	ModifyProperty(cPluginDeviceBase::kZeroPropName,					 true, true, true);
	ModifyProperty(cPluginDeviceBase::kGroupNamePropName,			 true, true, true);

	// hide advanced properties
	ModifyProperty(cPluginDeviceBase::kConnectedPropName,			 true, false, true);
	ModifyProperty(cPluginDeviceBase::kNamePropName,					 true, false, true);
	ModifyProperty(cPluginDeviceBase::kChannelCountPropName,			 true, false, true);
	ModifyProperty(cPluginDeviceBase::kAppRunModePropName,			 true, false, true);
	ModifyProperty(cPluginDeviceBase::kMocapSyncFramePropName,		 true, false, true);
	ModifyProperty(cPluginDeviceBase::kSyncFramePropName,			 true, false, true);
	ModifyProperty(cPluginDeviceBase::kNeedDeviceSyncFramePropName,	 false, false, true);
	ModifyProperty(cPluginDeviceBase::kNeedMocapSyncFramePropName,	 false, false, true);
	ModifyProperty(cPluginDeviceBase::kUseDriftCorrectionPropName,	 false, false, true);
	ModifyProperty(cPluginDeviceBase::kMasterSerialPropName,			 true, false, true);
	ModifyProperty(cPluginDeviceBase::kDriftCorrectionPropName,		 true, false , true);
	SetProperty(cPluginDeviceBase::kEnabledPropName, true);
	SetProperty(cPluginDeviceBase::kConnectedPropName, true);
	SetProperty(cPluginDeviceBase::kMocapRateMultiplePropName, 1);
}

void OptiTrackPluginDevices::GloveDeviceBase::SetCommonGloveDeviceProperties()
{
	// Add glove related properties
	AddProperty(GloveDeviceProperties::GloveDeviceProp_HandSide, GloveHandSide, kHandSideCount, 0, "Settings", false);
	AddProperty(GloveDeviceProperties::GloveDeviceProp_Battery, GloveDeviceProp_BatteryUninitialized, "Settings", false);
	AddProperty(GloveDeviceProperties::GloveDeviceProp_SignalStrength, GloveDeviceProp_SignalStrengthUnitialized, "Settings", false);
	//AddProperty(GloveDeviceProperties::GloveDeviceProp_Reconnect, false, "Settings", false);

	// Modify property visibility
	ModifyProperty(GloveDeviceProperties::GloveDeviceProp_SignalStrength, true, false, false, false);
	ModifyProperty(GloveDeviceProperties::GloveDeviceProp_HandSide, true, false, false);
	ModifyProperty(GloveDeviceProperties::GloveDeviceProp_Battery, true, false, false, false);
}

///////////////////////////////////////////////////////////////////////////////
// 
// Setter: Glove Device 
//
void OptiTrackPluginDevices::GloveDeviceBase::SetDeviceRate(int rate)
{
	mDeviceRateFPS = rate;
	mRequestedRateMS = (1.0f / mDeviceRateFPS) * 1000.0f; 
	return;
}

void OptiTrackPluginDevices::GloveDeviceBase::SetHandSide(eGloveHandSide side)
{
	mHandSide = side;
	return;
}

void OptiTrackPluginDevices::GloveDeviceBase::SetEnabled(bool enabled)
{
	bIsEnabled = enabled;
}

void OptiTrackPluginDevices::GloveDeviceBase::SetCollecting(bool collecting)
{
	bIsCollecting = collecting;
}

bool OptiTrackPluginDevices::GloveDeviceBase::SetDeviceSerial(std::string serial)
{
	mDeviceSerial = serial;
	// Set device serial property
	return false;
}

bool OptiTrackPluginDevices::GloveDeviceBase::SetDeviceData(sGloveDeviceData data)
{
	mLastGloveData = data;
	return true;
}

bool OptiTrackPluginDevices::GloveDeviceBase::SetBatteryLevel(int level)
{
	if (SetProperty(GloveDeviceProperties::GloveDeviceProp_Battery, (int)level))
	{
		mBatteryLevel = level;
		return true;
	}
	return false;
}

bool OptiTrackPluginDevices::GloveDeviceBase::SetSignalStrength(int signal)
{
	if (SetProperty(GloveDeviceProperties::GloveDeviceProp_SignalStrength, (int)signal))
	{
		mSignalStrength = signal;
		return true;
	}
	return false;
}

void OptiTrackPluginDevices::GloveDeviceBase::InitializeGloveProperty()
{
	if (mDeviceInfo.handSide == eGloveHandSide::Left)
	{
		SetProperty(GloveDeviceProperties::GloveDeviceProp_HandSide, 1);
		SetProperty(cPluginDeviceBase::kOrderPropName, 1);
		bIsInitialized = true;
	}
	else if (mDeviceInfo.handSide == eGloveHandSide::Right)
	{
		SetProperty(GloveDeviceProperties::GloveDeviceProp_HandSide, 2);
		SetProperty(cPluginDeviceBase::kOrderPropName, 2);
		bIsInitialized = true;
	}
	return;
}


void OptiTrackPluginDevices::GloveDeviceBase::UpdateGloveProperty(const sGloveDeviceBaseInfo& deviceInfo)
{
	// update glove property when the device is receiving data.
	int deviceState = 0;
	GetProperty(kDataStatePropName, deviceState);
	if (deviceState == DeviceDataState::DeviceState_ReceivingData)
	{
		if (mSignalStrength != 1)
		{
			mSignalStrength = deviceInfo.signalStrength;
			double signal = fabs(mSignalStrength);
			SetProperty(GloveDeviceProp_SignalStrength, (int)signal);
		}
		SetProperty(GloveDeviceProp_SignalStrength, -100);

		if (mBatteryLevel != deviceInfo.battery)
		{
			mBatteryLevel = (int)(deviceInfo.battery); //Battery level percentage.
			SetProperty(GloveDeviceProp_Battery, (int)mBatteryLevel);
		}
	}
}


///////////////////////////////////////////////////////////////////////////////
// 
// Glove Device Factory
//
void OptiTrackPluginDevices::GloveDeviceFactoryBase::SetCommonGloveDeviceProperties(GloveDeviceBase* pDevice) const
{
	// REQUIRED: Set device name/model/serial
	pDevice->SetProperty(cPluginDeviceBase::kNamePropName, (char*)DeviceName());
	pDevice->SetProperty(cPluginDeviceBase::kDisplayNamePropName, (char*)DeviceName());
	pDevice->SetProperty(cPluginDeviceBase::kModelPropName, "Glove Model");	// model
	char mDeviceSerial[MAX_PATH];
	sprintf_s(mDeviceSerial, "%s-serial", DeviceName());
	pDevice->SetProperty(cPluginDeviceBase::kSerialPropName, mDeviceSerial);									// device serial (must be unique!)
	pDevice->SetProperty(cPluginDeviceBase::kDeviceTypePropName, (long)DeviceType_Glove);					// set device type as glove
	pDevice->SetProperty(cPluginDeviceBase::kRatePropName, 120.0);											// glove sampling rate
	pDevice->SetProperty(cPluginDeviceBase::kUseDriftCorrectionPropName, true);								// drift correction to fetch most recent data.
	pDevice->SetProperty(cPluginDeviceBase::kOrderPropName, (int) eGloveHandSide::Unknown);					// device order: (0 = uninitialized, 1=left, 2=right)
}


void OptiTrackPluginDevices::GloveDeviceFactoryBase::SetDeviceIndex(int t_index)
{
	mDeviceIndex = t_index;
	return;
}

void OptiTrackPluginDevices::GloveDeviceFactoryBase::SetQuaternionDataChannels(GloveDeviceBase* pDevice) const
{
	// Set glove data channels: 
	//   5 fingers * 3 joints per finger * 4 floats per quat (x/y/z/w) = 60 channels
	int channelIndex;
	int gloveAnalogChannelCount = 60;
	char szChannelNames[MAX_PATH];

	// add channels
	for (int i = 0; i < gloveAnalogChannelCount; i++)
	{
		int finger = i / 12;		// 12 channels per finger
		switch (finger)
		{
		case 0: sprintf_s(szChannelNames, "T"); break;
		case 1: sprintf_s(szChannelNames, "I"); break;
		case 2: sprintf_s(szChannelNames, "M"); break;
		case 3: sprintf_s(szChannelNames, "R"); break;
		case 4: sprintf_s(szChannelNames, "P"); break;
		}

		int joint = (i / 4) % 3;	// 3 joints per finger
		switch (joint)
		{
		case 0: sprintf_s(szChannelNames, "%s-MCP", szChannelNames); break;
		case 1: sprintf_s(szChannelNames, "%s-PIP", szChannelNames); break;
		case 2: sprintf_s(szChannelNames, "%s-DIP", szChannelNames); break;
		}

		int axis = i % 4;			// 4 floats per joint
		switch (axis)
		{
		case 0: sprintf_s(szChannelNames, "%s W", szChannelNames); break;
		case 1: sprintf_s(szChannelNames, "%s X", szChannelNames); break;
		case 2: sprintf_s(szChannelNames, "%s Y", szChannelNames); break;
		case 3: sprintf_s(szChannelNames, "%s Z", szChannelNames); break;
		}
		channelIndex = pDevice->AddChannelDescriptor(szChannelNames, ChannelType_Float);

	}

	// enable all channels by default
	for (int i = 0; i <= channelIndex; i++)
	{
		// data channel enabled by default
		pDevice->ChannelDescriptor(i)->SetProperty(ChannelProp_Enabled, true);

		// hide unused channel properties
		pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_Units, true, true);
		pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_Name, true, true);
		pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_MinVoltage, true, true);
		pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_MaxVoltage, true, true);
		pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_TerminalName, true, true);
		pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_TerminalType, true, true);
		pDevice->ChannelDescriptor(i)->ModifyProperty(ChannelProp_MaxVoltage, true, true);
	}
}