/************************************************************************************

Filename    :   OVR_Sensor2Impl.cpp
Content     :   DK2 sensor device specific implementation.
Created     :   January 21, 2013
Authors     :   Lee Cooper

Copyright   :   Copyright 2014 Oculus VR, Inc. All Rights reserved.

Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License"); 
you may not use the Oculus VR Rift SDK except in compliance with the License, 
which is provided at the time of installation or download, or which 
otherwise accompanies this software in either electronic or hard copy form.

You may obtain a copy of the License at

http://www.oculusvr.com/licenses/LICENSE-3.1 

Unless required by applicable law or agreed to in writing, the Oculus VR SDK 
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

*************************************************************************************/

#include "OVR_Sensor2Impl.h"
#include "OVR_SensorImpl_Common.h"
#include "OVR_Sensor2ImplUtil.h"
#include "Kernel/OVR_Alg.h"

//extern FILE *SF_LOG_fp;

namespace OVR {

//-------------------------------------------------------------------------------------
// ***** Oculus Sensor2-specific packet data structures

enum {    
    Sensor2_VendorId            = Oculus_VendorId,
    Sensor2_ProductId           = 0x0021,

    Sensor2_BootLoader          = 0x1001,

    Sensor2_DefaultReportRate   = 1000, // Hz
};


// Messages we care for
enum Tracker2MessageType
{
    Tracker2Message_None              = 0,
    Tracker2Message_Sensors           = 11,
    Tracker2Message_Unknown           = 0x100,
    Tracker2Message_SizeError         = 0x101,
};


struct Tracker2Sensors
{
    UInt16	LastCommandID;
    UByte	NumSamples;
    UInt16	RunningSampleCount;				// Named 'SampleCount' in the firmware docs.
    SInt16	Temperature;
	UInt32	SampleTimestamp;
    TrackerSample Samples[2];
    SInt16	MagX, MagY, MagZ;
    UInt16	FrameCount;
	UInt32	FrameTimestamp;
    UByte	FrameID;
    UByte	CameraPattern;
    UInt16	CameraFrameCount;				// Named 'CameraCount' in the firmware docs.
	UInt32	CameraTimestamp;

    Tracker2MessageType Decode(const UByte* buffer, int size)
    {
        if (size < 64)
            return Tracker2Message_SizeError;

		LastCommandID		= DecodeUInt16(buffer + 1);
        NumSamples			= buffer[3];
        RunningSampleCount	= DecodeUInt16(buffer + 4);
        Temperature			= DecodeSInt16(buffer + 6);
        SampleTimestamp		= DecodeUInt32(buffer + 8);
        
		// Only unpack as many samples as there actually are.
        UByte iterationCount = (NumSamples > 1) ? 2 : NumSamples;

        for (UByte i = 0; i < iterationCount; i++)
        {
			UnpackSensor(buffer + 12 + 16 * i, &Samples[i].AccelX, &Samples[i].AccelY, &Samples[i].AccelZ);
            UnpackSensor(buffer + 20 + 16 * i, &Samples[i].GyroX,  &Samples[i].GyroY,  &Samples[i].GyroZ);
		}

        MagX = DecodeSInt16(buffer + 44);
        MagY = DecodeSInt16(buffer + 46);
        MagZ = DecodeSInt16(buffer + 48);

		FrameCount = DecodeUInt16(buffer + 50);

		FrameTimestamp		= DecodeUInt32(buffer + 52);
		FrameID				= buffer[56];
		CameraPattern		= buffer[57];
		CameraFrameCount	= DecodeUInt16(buffer + 58);
		CameraTimestamp		= DecodeUInt32(buffer + 60);
        
        return Tracker2Message_Sensors;
    }
};

struct Tracker2Message
{
    Tracker2MessageType Type;
    Tracker2Sensors     Sensors;
};

// Sensor reports data in the following coordinate system:
// Accelerometer: 10^-4 m/s^2; X forward, Y right, Z Down.
// Gyro:          10^-4 rad/s; X positive roll right, Y positive pitch up; Z positive yaw right.


// We need to convert it to the following RHS coordinate system:
// X right, Y Up, Z Back (out of screen)
//
Vector3f AccelFromBodyFrameUpdate(const Tracker2Sensors& update, UByte sampleNumber)
{
    const TrackerSample& sample = update.Samples[sampleNumber];
    float                ax = (float)sample.AccelX;
    float                ay = (float)sample.AccelY;
    float                az = (float)sample.AccelZ;

    return Vector3f(ax, ay, az) * 0.0001f;
}


Vector3f MagFromBodyFrameUpdate(const Tracker2Sensors& update)
{   
    return Vector3f( (float)update.MagX, (float)update.MagY, (float)update.MagZ) * 0.0001f;
}

Vector3f EulerFromBodyFrameUpdate(const Tracker2Sensors& update, UByte sampleNumber)
{
    const TrackerSample& sample = update.Samples[sampleNumber];
    float                gx = (float)sample.GyroX;
    float                gy = (float)sample.GyroY;
    float                gz = (float)sample.GyroZ;

    return Vector3f(gx, gy, gz) * 0.0001f;
}

bool  Sensor2DeviceImpl::decodeTracker2Message(Tracker2Message* message, UByte* buffer, int size)
{
    memset(message, 0, sizeof(Tracker2Message));

    if (size < 4)
    {
        message->Type = Tracker2Message_SizeError;
        return false;
    }

    switch (buffer[0])
    {
    case Tracker2Message_Sensors:
        message->Type = message->Sensors.Decode(buffer, size);
        break;

    default:
        message->Type = Tracker2Message_Unknown;
        break;
    }

    return (message->Type < Tracker2Message_Unknown) && (message->Type != Tracker2Message_None);
}

//-------------------------------------------------------------------------------------
// ***** Sensor2Device

Sensor2DeviceImpl::Sensor2DeviceImpl(SensorDeviceCreateDesc* createDesc)
    :   SensorDeviceImpl(createDesc),
        LastNumSamples(0),
        LastRunningSampleCount(0),
        FullCameraFrameCount(0),
        LastCameraTime("C"),
        LastFrameTime("F"),
        LastSensorTime("S"),
        LastFrameTimestamp(0)
{
    // 15 samples ok in min-window for DK2 since it uses microsecond clock.
    TimeFilter = SensorTimeFilter(SensorTimeFilter::Settings(15));

    pCalibration = new SensorCalibration(this);
}

Sensor2DeviceImpl::~Sensor2DeviceImpl()
{
    delete pCalibration;
}

void Sensor2DeviceImpl::openDevice()
{

    // Read the currently configured range from sensor.
    SensorRangeImpl sr(SensorRange(), 0);

    if (GetInternalDevice()->GetFeatureReport(sr.Buffer, SensorRangeImpl::PacketSize))
    {
        sr.Unpack();
        sr.GetSensorRange(&CurrentRange);
    }

    // Read the currently configured calibration from sensor.
    SensorFactoryCalibrationImpl sc;
    if (GetInternalDevice()->GetFeatureReport(sc.Buffer, SensorFactoryCalibrationImpl::PacketSize))
    {
        sc.Unpack();
        AccelCalibrationOffset = sc.AccelOffset;
        GyroCalibrationOffset  = sc.GyroOffset;
        AccelCalibrationMatrix = sc.AccelMatrix;
        GyroCalibrationMatrix  = sc.GyroMatrix;
        CalibrationTemperature = sc.Temperature;
    }

    // If the sensor has "DisplayInfo" data, use HMD coordinate frame by default.
    SensorDisplayInfoImpl displayInfo;
    if (GetInternalDevice()->GetFeatureReport(displayInfo.Buffer, SensorDisplayInfoImpl::PacketSize))
    {
        displayInfo.Unpack();
        Coordinates = (displayInfo.DistortionType & SensorDisplayInfoImpl::Mask_BaseFmt) ?
                      Coord_HMD : Coord_Sensor;
    }
	Coordinates = Coord_HMD; // TODO temporary to force it behave

    // Read/Apply sensor config.
    setCoordinateFrame(Coordinates);
    setReportRate(Sensor2_DefaultReportRate);
    setOnboardCalibrationEnabled(false);

    // Must send DK2 keep-alive. Set Keep-alive at 10 seconds.
    KeepAliveMuxReport keepAlive;
    keepAlive.CommandId = 0;
    keepAlive.INReport = 11;
    keepAlive.Interval = 10 * 1000;

    // Device creation is done from background thread so we don't need to add this to the command queue.
    KeepAliveMuxImpl keepAliveImpl(keepAlive);
    GetInternalDevice()->SetFeatureReport(keepAliveImpl.Buffer, KeepAliveMuxImpl::PacketSize);

    // Read the temperature  data from the device
    pCalibration->Initialize();
}

bool Sensor2DeviceImpl::SetTrackingReport(const TrackingReport& data)
{ 
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setTrackingReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setTrackingReport(const TrackingReport& data)
{
    TrackingImpl ci(data);
    return GetInternalDevice()->SetFeatureReport(ci.Buffer, TrackingImpl::PacketSize);
}

bool Sensor2DeviceImpl::GetTrackingReport(TrackingReport* data)
{
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getTrackingReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::getTrackingReport(TrackingReport* data)
{
    TrackingImpl ci;
    if (GetInternalDevice()->GetFeatureReport(ci.Buffer, TrackingImpl::PacketSize))
    {
        ci.Unpack();
        *data = ci.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::SetDisplayReport(const DisplayReport& data)
{ 
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setDisplayReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setDisplayReport(const DisplayReport& data)
{
    DisplayImpl di(data);
    return GetInternalDevice()->SetFeatureReport(di.Buffer, DisplayImpl::PacketSize);
}

bool Sensor2DeviceImpl::GetDisplayReport(DisplayReport* data)
{
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getDisplayReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::getDisplayReport(DisplayReport* data)
{
    DisplayImpl di;
    if (GetInternalDevice()->GetFeatureReport(di.Buffer, DisplayImpl::PacketSize))
    {
        di.Unpack();
        *data = di.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::SetMagCalibrationReport(const MagCalibrationReport& data)
{ 
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setMagCalibrationReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setMagCalibrationReport(const MagCalibrationReport& data)
{
    MagCalibrationImpl mci(data);
    return GetInternalDevice()->SetFeatureReport(mci.Buffer, MagCalibrationImpl::PacketSize);
}

bool Sensor2DeviceImpl::GetMagCalibrationReport(MagCalibrationReport* data)
{
    // direct call if we are already on the device manager thread
    if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
    {
        return getMagCalibrationReport(data);
    }

	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getMagCalibrationReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::getMagCalibrationReport(MagCalibrationReport* data)
{
    MagCalibrationImpl mci;
    if (GetInternalDevice()->GetFeatureReport(mci.Buffer, MagCalibrationImpl::PacketSize))
    {
        mci.Unpack();
        *data = mci.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::SetPositionCalibrationReport(const PositionCalibrationReport& data)
{ 
    bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setPositionCalibrationReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setPositionCalibrationReport(const PositionCalibrationReport& data)
{
	UByte version = GetDeviceInterfaceVersion();
	if (version < 5)
	{
		PositionCalibrationImpl_Pre5 pci(data);
		return GetInternalDevice()->SetFeatureReport(pci.Buffer, PositionCalibrationImpl_Pre5::PacketSize);
	}
	
	PositionCalibrationImpl pci(data);
    return GetInternalDevice()->SetFeatureReport(pci.Buffer, PositionCalibrationImpl::PacketSize);
}

bool Sensor2DeviceImpl::getPositionCalibrationReport(PositionCalibrationReport* data)
{
	UByte version = GetDeviceInterfaceVersion();
	if (version < 5)
	{
		PositionCalibrationImpl_Pre5 pci;
		if (GetInternalDevice()->GetFeatureReport(pci.Buffer, PositionCalibrationImpl_Pre5::PacketSize))
		{
			pci.Unpack();
			*data = pci.Settings;
			return true;
		}

		return false;
	}

    PositionCalibrationImpl pci;
    if (GetInternalDevice()->GetFeatureReport(pci.Buffer, PositionCalibrationImpl::PacketSize))
    {
        pci.Unpack();
        *data = pci.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::GetAllPositionCalibrationReports(Array<PositionCalibrationReport>* data)
{
    bool result;
    if (!GetManagerImpl()->GetThreadQueue()->
        PushCallAndWaitResult(this, &Sensor2DeviceImpl::getAllPositionCalibrationReports, &result, data))
    {
        return false;
    }

    return result;
}

bool Sensor2DeviceImpl::getAllPositionCalibrationReports(Array<PositionCalibrationReport>* data)
{
    PositionCalibrationReport pc;
    bool result = getPositionCalibrationReport(&pc);
    if (!result)
        return false;

    int positions = pc.NumPositions;
    data->Clear();
    data->Resize(positions);

    for (int i = 0; i < positions; i++)
    {
        result = getPositionCalibrationReport(&pc);
        if (!result)
            return false;
        OVR_ASSERT(pc.NumPositions == positions);

        (*data)[pc.PositionIndex] = pc;
        // IMU should be the last one
        OVR_ASSERT(pc.PositionType == (pc.PositionIndex == positions - 1) ? 
            PositionCalibrationReport::PositionType_IMU : PositionCalibrationReport::PositionType_LED);
    }
    return true;
}

bool Sensor2DeviceImpl::SetCustomPatternReport(const CustomPatternReport& data)
{ 
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setCustomPatternReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setCustomPatternReport(const CustomPatternReport& data)
{
    CustomPatternImpl cpi(data);
    return GetInternalDevice()->SetFeatureReport(cpi.Buffer, CustomPatternImpl::PacketSize);
}

bool Sensor2DeviceImpl::GetCustomPatternReport(CustomPatternReport* data)
{
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getCustomPatternReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::getCustomPatternReport(CustomPatternReport* data)
{
    CustomPatternImpl cpi;
    if (GetInternalDevice()->GetFeatureReport(cpi.Buffer, CustomPatternImpl::PacketSize))
    {
        cpi.Unpack();
        *data = cpi.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::SetManufacturingReport(const ManufacturingReport& data)
{ 
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setManufacturingReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setManufacturingReport(const ManufacturingReport& data)
{
    ManufacturingImpl mi(data);
    return GetInternalDevice()->SetFeatureReport(mi.Buffer, ManufacturingImpl::PacketSize);
}

bool Sensor2DeviceImpl::GetManufacturingReport(ManufacturingReport* data)
{
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getManufacturingReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::getManufacturingReport(ManufacturingReport* data)
{
    ManufacturingImpl mi;
    if (GetInternalDevice()->GetFeatureReport(mi.Buffer, ManufacturingImpl::PacketSize))
    {
        mi.Unpack();
        *data = mi.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::SetLensDistortionReport(const LensDistortionReport& data)
{ 
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setLensDistortionReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setLensDistortionReport(const LensDistortionReport& data)
{
    LensDistortionImpl ui(data);
    return GetInternalDevice()->SetFeatureReport(ui.Buffer, LensDistortionImpl::PacketSize);
}

bool Sensor2DeviceImpl::GetLensDistortionReport(LensDistortionReport* data)
{
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getLensDistortionReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::getLensDistortionReport(LensDistortionReport* data)
{
    LensDistortionImpl ui;
    if (GetInternalDevice()->GetFeatureReport(ui.Buffer, LensDistortionImpl::PacketSize))
    {
        ui.Unpack();
        *data = ui.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::SetUUIDReport(const UUIDReport& data)
{ 
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setUUIDReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setUUIDReport(const UUIDReport& data)
{
    UUIDImpl ui(data);
    return GetInternalDevice()->SetFeatureReport(ui.Buffer, UUIDImpl::PacketSize);
}

bool Sensor2DeviceImpl::GetUUIDReport(UUIDReport* data)
{
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getUUIDReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::getUUIDReport(UUIDReport* data)
{
    UUIDImpl ui;
    if (GetInternalDevice()->GetFeatureReport(ui.Buffer, UUIDImpl::PacketSize))
    {
        ui.Unpack();
        *data = ui.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::SetKeepAliveMuxReport(const KeepAliveMuxReport& data)
{ 
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setKeepAliveMuxReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setKeepAliveMuxReport(const KeepAliveMuxReport& data)
{
    KeepAliveMuxImpl kami(data);
    return GetInternalDevice()->SetFeatureReport(kami.Buffer, KeepAliveMuxImpl::PacketSize);
}

bool Sensor2DeviceImpl::GetKeepAliveMuxReport(KeepAliveMuxReport* data)
{
	bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getKeepAliveMuxReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::getKeepAliveMuxReport(KeepAliveMuxReport* data)
{
    KeepAliveMuxImpl kami;
    if (GetInternalDevice()->GetFeatureReport(kami.Buffer, KeepAliveMuxImpl::PacketSize))
    {
        kami.Unpack();
        *data = kami.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::SetTemperatureReport(const TemperatureReport& data)
{
    // direct call if we are already on the device manager thread
    if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
    {
        return setTemperatureReport(data);
    }

    bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::setTemperatureReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::setTemperatureReport(const TemperatureReport& data)
{
    TemperatureImpl ti(data);
    return GetInternalDevice()->SetFeatureReport(ti.Buffer, TemperatureImpl::PacketSize);
}

bool Sensor2DeviceImpl::getTemperatureReport(TemperatureReport* data)
{
    TemperatureImpl ti;
    if (GetInternalDevice()->GetFeatureReport(ti.Buffer, TemperatureImpl::PacketSize))
    {
        ti.Unpack();
        *data = ti.Settings;
        return true;
    }

    return false;
}

bool Sensor2DeviceImpl::GetAllTemperatureReports(Array<Array<TemperatureReport> >* data)
{
    // direct call if we are already on the device manager thread
    if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
    {
        return getAllTemperatureReports(data);
    }

    bool result;
    if (!GetManagerImpl()->GetThreadQueue()->
        PushCallAndWaitResult(this, &Sensor2DeviceImpl::getAllTemperatureReports, &result, data))
    {
        return false;
    }

    return result;
}

bool Sensor2DeviceImpl::getAllTemperatureReports(Array<Array<TemperatureReport> >* data)
{
    TemperatureReport t;
    bool result = getTemperatureReport(&t);
    if (!result)
        return false;

    int bins = t.NumBins, samples = t.NumSamples;
    data->Clear();
    data->Resize(bins);
    for (int i = 0; i < bins; i++)
        (*data)[i].Resize(samples);

    for (int i = 0; i < bins; i++)
        for (int j = 0; j < samples; j++)
        {
            result = getTemperatureReport(&t);
            if (!result)
                return false;
            OVR_ASSERT(t.NumBins == bins && t.NumSamples == samples);

            (*data)[t.Bin][t.Sample] = t;
        }
    return true;
}

bool Sensor2DeviceImpl::GetGyroOffsetReport(GyroOffsetReport* data)
{
    // direct call if we are already on the device manager thread
    if (GetCurrentThreadId() == GetManagerImpl()->GetThreadId())
    {
        return getGyroOffsetReport(data);
    }

    bool result;
	if (!GetManagerImpl()->GetThreadQueue()->
            PushCallAndWaitResult(this, &Sensor2DeviceImpl::getGyroOffsetReport, &result, data))
	{
		return false;
	}

	return result;
}

bool Sensor2DeviceImpl::getGyroOffsetReport(GyroOffsetReport* data)
{
    GyroOffsetImpl goi;
    if (GetInternalDevice()->GetFeatureReport(goi.Buffer, GyroOffsetImpl::PacketSize))
    {
        goi.Unpack();
        *data = goi.Settings;
        return true;
    }

    return false;
}

void Sensor2DeviceImpl::onTrackerMessage(Tracker2Message* message)
{
    if (message->Type != Tracker2Message_Sensors)
        return;
    
    const float     sampleIntervalTimeUnit   = (1.0f / 1000.f);
    double          scaledSampleIntervalTimeUnit  = sampleIntervalTimeUnit;
    Tracker2Sensors& s = message->Sensors;
    
    double       absoluteTimeSeconds = 0.0;

    if (SequenceValid)
    {
        UInt32 runningSampleCountDelta;

        if (s.RunningSampleCount < LastRunningSampleCount)
        {
            // The running sample count on the device rolled around the 16 bit counter
            // (expect to happen about once per minute), so RunningSampleCount 
            // needs a high word increment.
            runningSampleCountDelta = ((((int)s.RunningSampleCount) + 0x10000) - (int)LastRunningSampleCount);
        }
        else
        {
            runningSampleCountDelta = (s.RunningSampleCount - LastRunningSampleCount);
        }

        absoluteTimeSeconds = LastSensorTime.TimeSeconds;
        scaledSampleIntervalTimeUnit = TimeFilter.ScaleTimeUnit(sampleIntervalTimeUnit);
 
        // If we missed a small number of samples, replicate the last sample.
        if ((runningSampleCountDelta > LastNumSamples) && (runningSampleCountDelta <= 254))
        {
            if (HandlerRef.HasHandlers())
            {
                MessageBodyFrame sensors(this);

                sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - s.NumSamples * scaledSampleIntervalTimeUnit;
                sensors.TimeDelta     = (float) ((runningSampleCountDelta - LastNumSamples) * scaledSampleIntervalTimeUnit);
                sensors.Acceleration  = LastAcceleration;
                sensors.RotationRate  = LastRotationRate;
                sensors.MagneticField = LastMagneticField;
                sensors.Temperature   = LastTemperature;

                pCalibration->Apply(sensors);
                HandlerRef.Call(sensors);
            }
        }
    }
    else
    {
        LastAcceleration = Vector3f(0);
        LastRotationRate = Vector3f(0);
        LastMagneticField= Vector3f(0);
        LastTemperature  = 0;
        SequenceValid    = true;
    }

    LastNumSamples = s.NumSamples;
    LastRunningSampleCount = s.RunningSampleCount;

    if (HandlerRef.HasHandlers())
    {
        MessageBodyFrame sensors(this);        
        UByte            iterations = s.NumSamples;

        if (s.NumSamples > 2)
        {
            iterations        = 2;
            sensors.TimeDelta = (float) ((s.NumSamples - 1) * scaledSampleIntervalTimeUnit);
        }
        else
        {
            sensors.TimeDelta = (float) scaledSampleIntervalTimeUnit;
        }

        for (UByte i = 0; i < iterations; i++)
        {            
            sensors.AbsoluteTimeSeconds = absoluteTimeSeconds - ( iterations - 1 - i ) * scaledSampleIntervalTimeUnit;
            sensors.Acceleration = AccelFromBodyFrameUpdate(s, i);
            sensors.RotationRate = EulerFromBodyFrameUpdate(s, i);
            sensors.MagneticField= MagFromBodyFrameUpdate(s);
            sensors.Temperature  = s.Temperature * 0.01f;

            pCalibration->Apply(sensors);
            HandlerRef.Call(sensors);

            // TimeDelta for the last two sample is always fixed.
            sensors.TimeDelta = (float) scaledSampleIntervalTimeUnit;
        }

        // Send pixel read only when frame timestamp changes.
        if (LastFrameTimestamp != s.FrameTimestamp)
        {
            MessagePixelRead pixelRead(this);
            // Prepare message for pixel read
            pixelRead.PixelReadValue    = s.FrameID;
            pixelRead.RawFrameTime      = s.FrameTimestamp;
            pixelRead.RawSensorTime     = s.SampleTimestamp;
            pixelRead.SensorTimeSeconds = LastSensorTime.TimeSeconds;
            pixelRead.FrameTimeSeconds  = LastFrameTime.TimeSeconds;

            HandlerRef.Call(pixelRead);
            LastFrameTimestamp = s.FrameTimestamp;
        }

        UInt16 lowFrameCount = (UInt16) FullCameraFrameCount;
        // Send message only when frame counter changes
        if (lowFrameCount != s.CameraFrameCount)
        {
            // check for the rollover in the counter
            if (s.CameraFrameCount < lowFrameCount)
                FullCameraFrameCount += 0x10000;
            // update the low bits
            FullCameraFrameCount = (FullCameraFrameCount & ~0xFFFF) | s.CameraFrameCount;

            MessageExposureFrame vision(this);
            vision.CameraPattern = s.CameraPattern;
            vision.CameraFrameCount = FullCameraFrameCount;
            vision.CameraTimeSeconds = LastCameraTime.TimeSeconds;

            HandlerRef.Call(vision);
        }

        LastAcceleration = sensors.Acceleration;
        LastRotationRate = sensors.RotationRate;
        LastMagneticField= sensors.MagneticField;
        LastTemperature  = sensors.Temperature;

        //LastPixelRead = pixelRead.PixelReadValue;
        //LastPixelReadTimeStamp = LastFrameTime;
    }
    else
    {
        if (s.NumSamples != 0)
		{
			UByte i = (s.NumSamples > 1) ? 1 : 0;
			LastAcceleration  = AccelFromBodyFrameUpdate(s, i);
			LastRotationRate  = EulerFromBodyFrameUpdate(s, i);
			LastMagneticField = MagFromBodyFrameUpdate(s);
			LastTemperature   = s.Temperature * 0.01f;
		}
    }
}

// Helper function to handle wrap-around of timestamps from Tracker2Message and convert them
// to system time.
//   - Any timestamps that didn't increment keep their old system time.
//   - This is a bit tricky since we don't know which one of timestamps has most recent time.
//   - The first timestamp must be the IMU one; we assume that others can't be too much ahead of it

void UpdateDK2Timestamps(SensorTimeFilter& tf,
                         SensorTimestampMapping** timestamps, UInt32 *rawValues, int count)
{
    int     updateIndices[4];
    int     updateCount = 0;
    int     i;
    double  now = Timer::GetSeconds();

    OVR_ASSERT(count <= sizeof(updateIndices)/sizeof(int));

    // Update timestamp wrapping for any values that changed.
    for (i = 0; i < count; i++)
    {        
        UInt32 lowMks = (UInt32)timestamps[i]->TimestampMks;  // Low 32-bits are raw old timestamp.

        if (rawValues[i] != lowMks)
        {
            if (i == 0)
            {
                // Only check for rollover in the IMU timestamp
                if (rawValues[i] < lowMks)
                {
                    LogText("Timestamp %d rollover, was: %u, now: %u\n", i, lowMks, rawValues[i]);
                    timestamps[i]->TimestampMks += 0x100000000;
                }
                // Update the low bits
                timestamps[i]->TimestampMks = (timestamps[i]->TimestampMks & 0xFFFFFFFF00000000) | rawValues[i];
            }
            else
            {
                // Take the high bits from the main timestamp first (not a typo in the first argument!)
                timestamps[i]->TimestampMks = 
                    (timestamps[0]->TimestampMks & 0xFFFFFFFF00000000) | rawValues[i];
                // Now force it into the reasonable range around the expanded main timestamp
                if (timestamps[i]->TimestampMks > timestamps[0]->TimestampMks + 0x1000000)
                    timestamps[i]->TimestampMks -= 0x100000000;
                else if (timestamps[i]->TimestampMks + 0x100000000 < timestamps[0]->TimestampMks + 0x1000000)
                    timestamps[i]->TimestampMks += 0x100000000;
            }

            updateIndices[updateCount] = i;
            updateCount++;
        }
    }


    // TBD: Simplify. Update indices should no longer be needed with new TimeFilter accepting
    //      previous values.
    // We might want to have multi-element checking time roll-over.

    static const double mksToSec = 1.0 / 1000000.0;

    for (int i = 0; i < updateCount; i++)
    {
        SensorTimestampMapping& ts  = *timestamps[updateIndices[i]];

        ts.TimeSeconds = tf.SampleToSystemTime(((double)ts.TimestampMks) * mksToSec,
                                               now, ts.TimeSeconds, ts.DebugTag);
    }
}


void Sensor2DeviceImpl::OnInputReport(UByte* pData, UInt32 length)
{
	bool processed = false;
    if (!processed)
    {
		Tracker2Message message;
        if (decodeTracker2Message(&message, pData, length))
        {
            processed = true;

            // Process microsecond timestamps from DK2 tracker.
            // Mapped and raw values must correspond to one another in each array.
            // IMU timestamp must be the first one!
            SensorTimestampMapping* tsMaps[3] =
            {                
                &LastSensorTime,
                &LastCameraTime,
                &LastFrameTime
            };
            UInt32 tsRawMks[3] =
            {                
                message.Sensors.SampleTimestamp,
                message.Sensors.CameraTimestamp,
                message.Sensors.FrameTimestamp
            };
            // Handle wrap-around and convert samples to system time for any samples that changed.
            UpdateDK2Timestamps(TimeFilter, tsMaps, tsRawMks, sizeof(tsRawMks)/sizeof(tsRawMks[0]));            

            onTrackerMessage(&message);

            /*
            if (SF_LOG_fp)
            {
                static UInt32 lastFrameTs  = 0;
                static UInt32 lastCameraTs = 0;
                
                if ((lastFrameTs != message.Sensors.FrameTimestamp) ||
                    (lastCameraTs = message.Sensors.CameraTimestamp))
                    fprintf(SF_LOG_fp, "msg cameraTs: 0x%X frameTs: 0x%X sensorTs: 0x%X\n",
                            message.Sensors.CameraTimestamp, message.Sensors.FrameTimestamp,
                            message.Sensors.SampleTimestamp);

                lastFrameTs  = message.Sensors.FrameTimestamp;
                lastCameraTs = message.Sensors.CameraTimestamp;
            }
            */            

#if 0
            // Checks for DK2 firmware bug.
            static unsigned SLastSampleTime = 0;
            if ((SLastSampleTime >  message.Sensors.SampleTimestamp) && message.Sensors.SampleTimestamp > 1000000 )
            {
                fprintf(SF_LOG_fp, "*** Sample Timestamp Wrap! ***\n");
                OVR_ASSERT (SLastSampleTime <= message.Sensors.SampleTimestamp);
            }            
            SLastSampleTime = message.Sensors.SampleTimestamp;

            static unsigned SLastCameraTime = 0;
            if ((SLastCameraTime > message.Sensors.CameraTimestamp) && message.Sensors.CameraTimestamp > 1000000 )
            {
                fprintf(SF_LOG_fp, "*** Camera Timestamp Wrap! ***\n");
                OVR_ASSERT (SLastCameraTime <= message.Sensors.CameraTimestamp);
            }            
            SLastCameraTime = message.Sensors.CameraTimestamp;

            static unsigned SLastFrameTime = 0;
            if ((SLastFrameTime > message.Sensors.FrameTimestamp) && message.Sensors.FrameTimestamp > 1000000 )
            {
                fprintf(SF_LOG_fp, "*** Frame Timestamp Wrap! ***\n");
                OVR_ASSERT (SLastFrameTime <= message.Sensors.FrameTimestamp);
            }                        
            SLastFrameTime = message.Sensors.FrameTimestamp;
#endif            
        }       
    }
}

double Sensor2DeviceImpl::OnTicks(double tickSeconds)
{

    if (tickSeconds >= NextKeepAliveTickSeconds)
    {
        // Must send DK2 keep-alive. Set Keep-alive at 10 seconds.
        KeepAliveMuxReport keepAlive;
        keepAlive.CommandId = 0;
        keepAlive.INReport = 11;
        keepAlive.Interval = 10 * 1000;

        // Device creation is done from background thread so we don't need to add this to the command queue.
        KeepAliveMuxImpl keepAliveImpl(keepAlive);
        GetInternalDevice()->SetFeatureReport(keepAliveImpl.Buffer, KeepAliveMuxImpl::PacketSize);

		// Emit keep-alive every few seconds.
        double keepAliveDelta = 3.0;        // Use 3-second interval.
        NextKeepAliveTickSeconds = tickSeconds + keepAliveDelta;
    }
    return NextKeepAliveTickSeconds - tickSeconds;
}

/*
// TBD: don't report calibration for now, until we figure out the logic between camera and mag yaw correction
bool Sensor2DeviceImpl::IsMagCalibrated()
{
    return pCalibration->IsMagCalibrated();
}
*/

} // namespace OVR