//======================================================================================================
// Copyright 2016, NaturalPoint Inc.
//======================================================================================================

#include "dllcommon.h"
#include "ExampleDevice.h"

// stl
#include <string>
#include <list>
#include <thread>
using namespace std;

// OptiTrack Peripheral Device API
#include "AnalogChannelDescriptor.h"
#include "IDeviceManager.h"
using namespace OptiTrackPluginDevices;


///////////////////////////////////////////////////////////////////////////////
//
// Device Factory
//
const char* ExampleDeviceFactory::Name() const
{
    // REQUIRED: factory name
    return "Example";
}

unique_ptr<IDevice> ExampleDeviceFactory::Create() const
{
    // Create device
    ExampleDevice* pDevice = new ExampleDevice();

    // REQUIRED: Set device name/model/serial
    pDevice->SetProperty( AnalogSystem::cPluginDeviceBase::kNamePropName, (char*) DeviceName() );   // device name
    pDevice->SetProperty( cPluginDeviceBase::kDisplayNamePropName, (char*) DeviceName() );          // device display name
    pDevice->SetProperty( cPluginDeviceBase::kModelPropName, "ExampleModel" );                      // device model
    char szValue[MAX_PATH];
    sprintf_s( szValue, "%s-serial", DeviceName() );
    pDevice->SetProperty( cPluginDeviceBase::kSerialPropName, szValue );                            // device serial (must be unique)

    // OPTIONAL: Add data channels
    int channelIndex;
    channelIndex = pDevice->AddChannelDescriptor( "ExampleChannel1", ChannelType_Float );
    channelIndex = pDevice->AddChannelDescriptor( "ExampleChannel2", ChannelType_Float );

    // OPTIONAL: Set some property values
    pDevice->SetProperty( cPluginDeviceBase::kRatePropName, 100.0 );
    pDevice->SetProperty(cPluginDeviceBase::kMocapRateMultiplePropName, 2.0);
    pDevice->SetProperty( ExampleDeviceProp_Property1, "NewValue" );

    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_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);
    }

    // Transfer ownership of the created device to the host, Motive.
    unique_ptr<IDevice> ptrDevice( pDevice );
    return ptrDevice;
}


///////////////////////////////////////////////////////////////////////////////
//
// Device
//
ExampleDevice::ExampleDevice() :
    mCollecting( false ),
    mCollectionThread( nullptr )
{
    SetDeviceProperties();
}

ExampleDevice::~ExampleDevice()
{
}

void ExampleDevice::SetDeviceProperties()
{
    // OPTIONAL: Add custom properties
    AddProperty( ExampleDeviceProp_Property1, 33.0, "Settings" );
    ModifyProperty( ExampleDeviceProp_Property1, false, false );
    AddProperty( ExampleDeviceProp_Property2, "ExampleValue", "Custom Settings" );
    ModifyProperty( ExampleDeviceProp_Property2, true, false );

    // OPTIONAL: Hide unrelated properties
    ModifyProperty(cPluginDeviceBase::kOrderPropName, true, true);                   // device serial
    ModifyProperty(cPluginDeviceBase::kMasterSerialPropName, true, true);                       // master device serial
    ModifyProperty(cPluginDeviceBase::kUseExternalClockPropName, true, true);                   // clock sync devices
	ModifyProperty(cPluginDeviceBase::kExternalTriggerTerminalPropName, true, true);            // trigger sync devices
	ModifyProperty(cPluginDeviceBase::kCalOffsetPropName, true, true);                          // force plates
    ModifyProperty(cPluginDeviceBase::kCalSquareRotationPropName, true, true);                  // force plates
    ModifyProperty(cPluginDeviceBase::kZeroPropName, true, true);                               // zero
	ModifyProperty(cPluginDeviceBase::kAssetPropName, true, true);                              // gloves. paired skeleton
	ModifyProperty(cPluginDeviceBase::kUserDataPropName, true, true);                           // reserved
}

// OPTIONAL: handle property change events
void ExampleDevice::OnPropertyChanged( const char* propertyName )
{
    LogError( MessageType_StatusInfo, "[Example Device] Property changed (%s)", propertyName );
}

// OPTIONAL: handle channel property change events
void ExampleDevice::OnChannelPropertyChanged( const char* channelName, const char* propertyName )
{
    AnalogChannelDescriptor* pChannel = ChannelDescriptor( channelName );

    // Example : Handle when a user changes a channel's enabled state in Motive
    if( strcmp( propertyName, ChannelProp_Enabled ) == 0 )
    {
        bool enabled;
        pChannel->GetProperty( ChannelProp_Enabled, enabled );
        LogError( MessageType_StatusInfo, "[Example Device ] Channel Enabled State Changed (Channel:%s  Enabled:%d)", propertyName, (int) enabled );
    }
}

bool ExampleDevice::Configure()
{
    bool success = Deconfigure();
    if( !success )
        return false;

    // update device's buffer allocation based on current channel and data type configuration
    success = __super::Configure();
    if( !success )
        return false;

    // user specified enabled channels in Motive
    int nChannels = 0;
    for( int i = 0; i < ChannelDescriptorCount(); i++ )
    {
        AnalogChannelDescriptor* pChannelDesc = ChannelDescriptor( i );
        bool enabled;
        pChannelDesc->GetProperty( ChannelProp_Enabled, enabled );
        if( enabled )
        {
            // optional - do something when channel enabled state changes
        }
    }

    return success;
}

bool ExampleDevice::Deconfigure()
{
    bool success = __super::Deconfigure();
    return success;
}

bool ExampleDevice::StartCapture()
{
    bool success = __super::StartCapture();

    // REQUIRED: Start collecting data
    // EXAMPLE: Start collecting from device using a polling thread
    mCollecting = true;
    mCollectionThread = CreateThread( nullptr, 0, CollectionThread, this, 0, nullptr );
    if( mCollectionThread == nullptr )
    {
        return false;
        mCollecting = false;
    }

    return success;
}

bool ExampleDevice::StopCapture()
{
    bool success = __super::StopCapture();

    // REQUIRED: Stop collecting data. Terminate hardware device polling thread
    mCollecting = false;
    DWORD waitResult = WaitForSingleObject( mCollectionThread, 1000 );
    if( waitResult == WAIT_OBJECT_0 )
    {
        CloseHandle( mCollectionThread );
        mCollectionThread = nullptr;
        success = true;
    }
    else if( waitResult == WAIT_TIMEOUT )
    {
        BOOL result = TerminateThread( mCollectionThread, 0 );
        success = ( result == TRUE );
    }
    else
    {
        success = false;
    }

    return success;
}

// EXAMPLE: Hardware device polling thread
unsigned long ExampleDevice::CollectionThread( void* Context )
{
    ExampleDevice* pThis = static_cast<ExampleDevice*>( Context );
    return pThis->DoCollectionThread();
}


unsigned long ExampleDevice::DoCollectionThread()
{
    // simulated hardware device data structure
    const int maxFrames = 10;
    const int maxChannels = 5;
    float deviceData[maxFrames][maxChannels];
    int channelsRead, framesRead;

    // simulate hardware device timing on windows
    std::chrono::high_resolution_clock::time_point start, end;
    std::chrono::milliseconds actualFrameDurationMS;

    double deviceRate;
    GetProperty( cPluginDeviceBase::kRatePropName, deviceRate );
    double requestedRateMS = ( 1.0f / deviceRate ) * 1000.0f;
    double adjustment = 0.0;
    double durationDeltaMS = 0.0;

    while( mCollecting )
    {
        start = std::chrono::high_resolution_clock::now();

        // simulate reading data from hardware device
        ReadDataFromHardware( deviceData, channelsRead, framesRead );

        if( ( framesRead > 0 ) && ( channelsRead > 0 ) )
        {
            // copy data from hardware device to PluginDevice frame buffer
            for( int iFrame = 0; iFrame < framesRead; iFrame++ )
            {
                // Retrieve the next available data slot from this Device's ring buffer,
                // which is automatically allocated during the base Configure() routine
                // based on selected data type and number of data channels.
                //
                // A DeviceFrameID is required for later use by the host/sync system during retrieval.
                // The DeviceFrameID can come from the hardware itself, or from a simple software counter.
                // It is used to frame align (synchronize) the host system with this device

                // establish device FrameID
                long deviceFrameID = this->FrameCounter();            // use software frame counter

                // get frame from device buffer
                AnalogFrame<float>* pFrame = this->BeginFrameUpdate( deviceFrameID );
                if( pFrame )
                {
                    // fill in frame header
                    int flags = 0;
                    pFrame->SetID( deviceFrameID );
                    pFrame->SetFlag( flags );

                    // fill in frame data
                    memcpy( pFrame->ChannelData(), &deviceData[iFrame], channelsRead * sizeof( float ) );

                    // update device buffer and unlock
                    this->EndFrameUpdate();

                    // update frame counter
                    this->SetFrameCounter( deviceFrameID + 1 );

                    // test: check value copy
                    float src = deviceData[iFrame][0];
                    const AnalogFrame<float>* pTestFrame = this->GetFrameByDeviceFrameID( pFrame->ID() );
                    float dst = pTestFrame->ChannelData()[0];
                    if( src != dst )
                    {
                        this->LogError( MessageType_StatusError, "[Example Device] Error in data copy (source:%d, target:%d", src, dst );
                    }
                }
                else
                {
                    this->LogError( MessageType_StatusError, "[Example Device] Unable to get frame from  buffer (frameID:%d)", deviceFrameID );
                }
            }
        }

        // End frame update. Sleep the thread for the remaining frame period.
		std::this_thread::sleep_for(std::chrono::milliseconds((int)(requestedRateMS - adjustment)));
        end = std::chrono::high_resolution_clock::now();
		actualFrameDurationMS = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
		durationDeltaMS = actualFrameDurationMS.count() - requestedRateMS;
        
        // calculating offset adjustment from the delta to apply to the next frame.
		if (durationDeltaMS > 1.0)
			adjustment = -1.0;
		else if (durationDeltaMS > 2.0)
			adjustment = -2.0;
		else if (durationDeltaMS < -2.0)
			adjustment = 2.0;
		else if (durationDeltaMS < -1.0)
			adjustment = 1.0;
		else
			adjustment = 0.0;
		if (fabs(durationDeltaMS) > 1.0)
		{
            this->LogError(MessageType_StatusInfo, "[Example Device] Device timing resolution off by  %3.1f ms (requested:%3.1f, actual:%3.1f)", durationDeltaMS, requestedRateMS, (double) actualFrameDurationMS.count());
		}
    }
    return 0;
}

template <size_t N, size_t M>
void ExampleDevice::ReadDataFromHardware( float( &deviceData )[M][N], int& channelsRead, int& framesRead )
{
    // EXAMPLE: generate 1 sample with 2 channels : sine/cosine wave

    framesRead = 1;
    channelsRead = this->ActiveChannelCount();
    for( int frame = 0; frame < framesRead; frame++ )
    {
        for( int index = 0; index < channelsRead; index++ )
        {
            int channel = ChannelID( index );
            float counter = (float) this->FrameCounter() / 10.0f;
            float val;
            if( channel == 0 )
            {
                val = (float) this->FrameCounter();
            }
            else
            {
                val = (float) sin( counter );
            }
            deviceData[frame][index] = val;
        }
    }
}