path: root/LibOVR/Src/CAPI/CAPI_HMDState.cpp

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

Filename    :   CAPI_HMDState.cpp
Content     :   State associated with a single HMD
Created     :   January 24, 2014
Authors     :   Michael Antonov

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 "CAPI_HMDState.h"
#include "CAPI_GlobalState.h"
#include "../OVR_Profile.h"

namespace OVR { namespace CAPI {

//-------------------------------------------------------------------------------------
// ***** HMDState


HMDState::HMDState(HMDDevice* device)
    : pHMD(device), HMDInfoW(device), HMDInfo(HMDInfoW.h),    
      EnabledHmdCaps(0), HmdCapsAppliedToSensor(0),
      SensorStarted(0), SensorCreated(0), SensorCaps(0),
      AddSensorCount(0), AddLatencyTestCount(0), AddLatencyTestDisplayCount(0),
      RenderState(getThis(), pHMD->GetProfile(), HMDInfoW.h),
      LastFrameTimeSeconds(0.0f), LastGetFrameTimeSeconds(0.0),
      LatencyTestActive(false),
      LatencyTest2Active(false)
{
    pLastError = 0;
    GlobalState::pInstance->AddHMD(this);
    
    // Should be in renderer?
    TimeManager.Init(RenderState.RenderInfo);

    EyeRenderActive[0] = false;
    EyeRenderActive[1] = false;

    LatencyTestDrawColor[0] = 0;
    LatencyTestDrawColor[1] = 0;
    LatencyTestDrawColor[2] = 0;

    OVR_CAPI_VISION_CODE( pPoseTracker = 0; )

    RenderingConfigured = false;
    BeginFrameCalled    = false;
    BeginFrameThreadId  = 0;
    BeginFrameTimingCalled = false;
}

HMDState::HMDState(ovrHmdType hmdType)
  : pHMD(0), HMDInfoW(hmdType), HMDInfo(HMDInfoW.h),
    EnabledHmdCaps(0),
    SensorStarted(0), SensorCreated(0), SensorCaps(0),
    AddSensorCount(0), AddLatencyTestCount(0), AddLatencyTestDisplayCount(0),
    RenderState(getThis(), 0, HMDInfoW.h), // No profile. 
    LastFrameTimeSeconds(0.0), LastGetFrameTimeSeconds(0.0)
{
    // TBD: We should probably be looking up the default profile for the given
    // device type + user.

    pLastError = 0;
    GlobalState::pInstance->AddHMD(this);

    // Should be in renderer?
    TimeManager.Init(RenderState.RenderInfo);

    EyeRenderActive[0] = false;
    EyeRenderActive[1] = false;

    OVR_CAPI_VISION_CODE( pPoseTracker = 0; )

    RenderingConfigured = false;
    BeginFrameCalled   = false;
    BeginFrameThreadId = 0;
    BeginFrameTimingCalled = false;
}


HMDState::~HMDState()
{
    OVR_ASSERT(GlobalState::pInstance);
   
    StopSensor();
    ConfigureRendering(0,0,0,0);

    OVR_CAPI_VISION_CODE( OVR_ASSERT(pPoseTracker == 0); )

    GlobalState::pInstance->RemoveHMD(this);
}



//-------------------------------------------------------------------------------------
// *** Sensor 

bool HMDState::StartSensor(unsigned supportedCaps, unsigned requiredCaps)
{
    Lock::Locker lockScope(&DevicesLock);

    bool crystalCoveOrBetter = (HMDInfo.HmdType == HmdType_CrystalCoveProto) ||
                               (HMDInfo.HmdType == HmdType_DK2);
    bool sensorCreatedJustNow = false;

    // TBD: In case of sensor not being immediately available, it would be good to check
    //      yaw config availability to match it with ovrHmdCap_YawCorrection requirement.
    // 

    if (!crystalCoveOrBetter)
    {
        if (requiredCaps & ovrSensorCap_Position)
        {
            pLastError = "ovrSensorCap_Position not supported on this HMD.";
            return false;            
        }
    }

    supportedCaps |= requiredCaps;

    if (pHMD && !pSensor)
    {
        // Zero AddSensorCount before creation, in case it fails (or succeeds but then
        // immediately gets disconnected) followed by another Add notification.        
        AddSensorCount      = 0;
        pSensor             = *pHMD->GetSensor();
        sensorCreatedJustNow= true;

        if (pSensor)
        {
            pSensor->SetReportRate(500);
            SFusion.AttachToSensor(pSensor);
            applyProfileToSensorFusion();
        }
        else
        {
            if (requiredCaps & ovrSensorCap_Orientation)
            {
                pLastError = "Failed to create sensor.";
                return false;
            }
        }
    }


    if ((requiredCaps & ovrSensorCap_YawCorrection) && !pSensor->IsMagCalibrated())
    {
        pLastError = "ovrHmdCap_YawCorrection not available.";
        if (sensorCreatedJustNow)
        {
            SFusion.AttachToSensor(0);
            SFusion.Reset();
            pSensor.Clear();
        }
        return false;
    }        

    SFusion.SetYawCorrectionEnabled((supportedCaps & ovrSensorCap_YawCorrection) != 0);

    if (pSensor && sensorCreatedJustNow)
    {
        LogText("Sensor created.\n");
        SensorCreated = true;
    }

    updateDK2FeaturesTiedToSensor(sensorCreatedJustNow);    
    

#ifdef OVR_CAPI_VISIONSUPPORT

    if (crystalCoveOrBetter && (supportedCaps & ovrSensorCap_Position))
    {
        if (!pPoseTracker)
        {
            pPoseTracker = new Vision::PoseTracker(SFusion);
            if (pPoseTracker)
            {
                pPoseTracker->AssociateHMD(pSensor);
                LogText("Sensor Pose tracker created.\n");
            }
        }

        // TBD: How do we verify that position tracking is actually available
        //      i.e. camera is plugged in?
    }
    else if (pPoseTracker)
    {
        // TBD: Internals not thread safe - must fix!!
        delete pPoseTracker;
        pPoseTracker = 0;
        LogText("Sensor Pose tracker destroyed.\n");
    }

#endif // OVR_CAPI_VISIONSUPPORT

    SensorCaps    = supportedCaps;
    SensorStarted = true;

    return true;
}


// Stops sensor sampling, shutting down internal resources.
void HMDState::StopSensor()
{
    Lock::Locker lockScope(&DevicesLock);

    if (SensorStarted)
    {
#ifdef OVR_CAPI_VISIONSUPPORT
        if (pPoseTracker)
        {
            // TBD: Internals not thread safe - must fix!!
            delete pPoseTracker;
            pPoseTracker = 0;
            LogText("Sensor Pose tracker destroyed.\n");
        }        
#endif // OVR_CAPI_VISION_CODE

        SFusion.AttachToSensor(0);
        SFusion.Reset();
        pSensor.Clear();
        HmdCapsAppliedToSensor = 0;
        AddSensorCount = 0;
        SensorCaps     = 0;
        SensorCreated  = false;
        SensorStarted  = false;

        LogText("StopSensor succeeded.\n");
    }
}

// Resets sensor orientation.
void HMDState::ResetSensor()
{
    SFusion.Reset();
}


// Returns prediction for time.
ovrSensorState HMDState::PredictedSensorState(double absTime)
{    
    SensorState ss;

    // We are trying to keep this path lockless unless we are notified of new device
    // creation while not having a sensor yet. It's ok to check SensorCreated volatile
    // flag here, since GetSensorStateAtTime() is internally lockless and safe.

    if (SensorCreated || checkCreateSensor())
    {   
        ss = SFusion.GetSensorStateAtTime(absTime);

        if (!(ss.StatusFlags & ovrStatus_OrientationTracked))
        {
            Lock::Locker lockScope(&DevicesLock);

#ifdef OVR_CAPI_VISIONSUPPORT
            if (pPoseTracker)
            {
                // TBD: Internals not thread safe - must fix!!
                delete pPoseTracker;
                pPoseTracker = 0;
                LogText("Sensor Pose tracker destroyed.\n");
            }        
#endif // OVR_CAPI_VISION_CODE
            // Not needed yet; SFusion.AttachToSensor(0);
            // This seems to reset orientation anyway...
            pSensor.Clear();
            SensorCreated = false;         
            HmdCapsAppliedToSensor = 0;
        }
    }
    else
    {
        // SensorState() defaults to 0s.
        // ss.Pose.Orientation       = Quatf();
        // ..

        // John:
        // We still want valid times so frames will get a delta-time
        // and allow operation with a joypad when the sensor isn't
        // connected.
        ss.Recorded.TimeInSeconds  = absTime;
        ss.Predicted.TimeInSeconds = absTime;
    }

    ss.StatusFlags |= ovrStatus_HmdConnected;
    return ss;
}


bool  HMDState::checkCreateSensor()
{
    if (!(SensorStarted && !SensorCreated && AddSensorCount))
        return false;

    Lock::Locker lockScope(&DevicesLock);

    // Re-check condition once in the lock, in case the state changed.
    if (SensorStarted && !SensorCreated && AddSensorCount)
    {        
        if (pHMD)
        {
            AddSensorCount = 0;
            pSensor        = *pHMD->GetSensor();
        }

        if (pSensor)
        {
            pSensor->SetReportRate(500);
            SFusion.AttachToSensor(pSensor);
            SFusion.SetYawCorrectionEnabled((SensorCaps & ovrSensorCap_YawCorrection) != 0);
            applyProfileToSensorFusion();

#ifdef OVR_CAPI_VISIONSUPPORT
            if (SensorCaps & ovrSensorCap_Position)
            {
                pPoseTracker = new Vision::PoseTracker(SFusion);
                if (pPoseTracker)
                {
                    pPoseTracker->AssociateHMD(pSensor);
                }
                LogText("Sensor Pose tracker created.\n");
            }
#endif // OVR_CAPI_VISION_CODE

            LogText("Sensor created.\n");

            SensorCreated = true;
            return true;
        }
    }

    return SensorCreated;
}

bool HMDState::GetSensorDesc(ovrSensorDesc* descOut)
{
    Lock::Locker lockScope(&DevicesLock);

    if (SensorCreated)
    {
        OVR_ASSERT(pSensor);
        OVR::SensorInfo si;
        pSensor->GetDeviceInfo(&si);
        descOut->VendorId  = si.VendorId;
        descOut->ProductId = si.ProductId;
        OVR_ASSERT(si.SerialNumber.GetSize() <= sizeof(descOut->SerialNumber));
        OVR_strcpy(descOut->SerialNumber, sizeof(descOut->SerialNumber), si.SerialNumber.ToCStr());
        return true;
    }
    return false;
}


void HMDState::applyProfileToSensorFusion()
{
    if (!pHMD)
        return;
    Profile* profile = pHMD->GetProfile();
    if (!profile)
    {
        OVR_ASSERT(false);
        return;
    }
    SFusion.SetUserHeadDimensions ( *profile, RenderState.RenderInfo );
}

void HMDState::updateLowPersistenceMode(bool lowPersistence) const
{
	OVR_ASSERT(pSensor);
	DisplayReport dr;
    
    if (pSensor.GetPtr())
    {
	    pSensor->GetDisplayReport(&dr);

	    dr.Persistence = (UInt16) (dr.TotalRows * (lowPersistence ? 0.18f : 1.0f));
	    dr.Brightness = lowPersistence ? 255 : 0;
    
	    pSensor->SetDisplayReport(dr);
    }
}

void HMDState::updateLatencyTestForHmd(bool latencyTesting)
{
    if (pSensor.GetPtr())
    {
        DisplayReport dr;
        pSensor->GetDisplayReport(&dr);

        dr.ReadPixel = latencyTesting;

        pSensor->SetDisplayReport(dr);
    }

    if (latencyTesting)
    {
        LatencyUtil2.SetSensorDevice(pSensor.GetPtr());
    }
    else
    {
        LatencyUtil2.SetSensorDevice(NULL);
    }
}


void HMDState::updateDK2FeaturesTiedToSensor(bool sensorCreatedJustNow)
{
    Lock::Locker lockScope(&DevicesLock);

    if (!SensorCreated || (HMDInfo.HmdType != HmdType_DK2))
        return;

    // Only send display reports if state changed or sensor initializing first time.
    if (sensorCreatedJustNow ||
         ((HmdCapsAppliedToSensor ^ EnabledHmdCaps) & ovrHmdCap_LowPersistence))
    {
        updateLowPersistenceMode((EnabledHmdCaps & ovrHmdCap_LowPersistence) ? true : false);         
    }

    if (sensorCreatedJustNow || ((HmdCapsAppliedToSensor ^ EnabledHmdCaps) & ovrHmdCap_LatencyTest))
    {
        updateLatencyTestForHmd((EnabledHmdCaps & ovrHmdCap_LatencyTest) != 0);
    }

    HmdCapsAppliedToSensor = EnabledHmdCaps & (ovrHmdCap_LowPersistence|ovrHmdCap_LatencyTest);
}



void HMDState::SetEnabledHmdCaps(unsigned hmdCaps)
{
    
    if (HMDInfo.HmdType == HmdType_DK2)
    {
        if ((EnabledHmdCaps ^ hmdCaps) & ovrHmdCap_DynamicPrediction)
        {
            // DynamicPrediction change
            TimeManager.ResetFrameTiming(TimeManager.GetFrameTiming().FrameIndex,
                                         (hmdCaps & ovrHmdCap_DynamicPrediction) ? true : false,
                                         RenderingConfigured);
        }
    }

    if ((EnabledHmdCaps ^ hmdCaps) & ovrHmdCap_NoVSync)
    {
        TimeManager.SetVsync((hmdCaps & ovrHmdCap_NoVSync) ? false : true);
    }


    EnabledHmdCaps             = hmdCaps & ovrHmdCap_Writable_Mask;
    RenderState.EnabledHmdCaps = EnabledHmdCaps;

    // Unfortunately, LowPersistance and other flags are tied to sensor.
    // This flag will apply the state of sensor is created; otherwise this will be delayed
    // till StartSensor.
    // Such behavior is less then ideal, but should be resolved with the service model.

    updateDK2FeaturesTiedToSensor(false);
}


//-------------------------------------------------------------------------------------
// ***** Property Access

// TBD: This all needs to be cleaned up and organized into namespaces.

float HMDState::getFloatValue(const char* propertyName, float defaultVal)
{
	if (OVR_strcmp(propertyName, "LensSeparation") == 0)
	{
		return HMDInfo.LensSeparationInMeters;
	}
    else if (OVR_strcmp(propertyName, "CenterPupilDepth") == 0)
    {        
        return SFusion.GetCenterPupilDepth();
    }
	else if (pHMD)
	{
		Profile* p = pHMD->GetProfile();
		if (p)
		{
			return p->GetFloatValue(propertyName, defaultVal);
		}
	}
	return defaultVal;
}

bool HMDState::setFloatValue(const char* propertyName, float value)
{
    if (OVR_strcmp(propertyName, "CenterPupilDepth") == 0)
    {
        SFusion.SetCenterPupilDepth(value);
        return true;
    }
    return false;
}


static unsigned CopyFloatArrayWithLimit(float dest[], unsigned destSize,
                                        float source[], unsigned sourceSize)
{
    unsigned count = Alg::Min(destSize, sourceSize);
    for (unsigned i = 0; i < count; i++)
        dest[i] = source[i];
    return count;
}


unsigned HMDState::getFloatArray(const char* propertyName, float values[], unsigned arraySize)
{
	if (arraySize)
	{
		if (OVR_strcmp(propertyName, "ScreenSize") == 0)
		{
            float data[2] = { HMDInfo.ScreenSizeInMeters.w, HMDInfo.ScreenSizeInMeters.h };

            return CopyFloatArrayWithLimit(values, arraySize, data, 2);
		}
        else if (OVR_strcmp(propertyName, "DistortionClearColor") == 0)
        {
            return CopyFloatArrayWithLimit(values, arraySize, RenderState.ClearColor, 4);
        }
        else if (OVR_strcmp(propertyName, "DK2Latency") == 0)
        {
            if (HMDInfo.HmdType != HmdType_DK2)
                return 0;

            float data[3];            
            TimeManager.GetLatencyTimings(data);
            
            return CopyFloatArrayWithLimit(values, arraySize, data, 3);
        }

        /*
        else if (OVR_strcmp(propertyName, "CenterPupilDepth") == 0)
        {
            if (arraySize >= 1)
            {
                values[0] = SFusion.GetCenterPupilDepth();
                return 1;
            }
            return 0;
        } */
		else if (pHMD)
		{        
			Profile* p = pHMD->GetProfile();

			// TBD: Not quite right. Should update profile interface, so that
			//      we can return 0 in all conditions if property doesn't exist.
			if (p)
			{
				unsigned count = p->GetFloatValues(propertyName, values, arraySize);
				return count;
			}
		}
	}

	return 0;
}

bool HMDState::setFloatArray(const char* propertyName, float values[], unsigned arraySize)
{
    if (!arraySize)
        return false;
    
    if (OVR_strcmp(propertyName, "DistortionClearColor") == 0)
    {
        CopyFloatArrayWithLimit(RenderState.ClearColor, 4, values, arraySize);
        return true;
    }
    return false;
}


const char* HMDState::getString(const char* propertyName, const char* defaultVal)
{
	if (pHMD)
	{
		// For now, just access the profile.
		Profile* p = pHMD->GetProfile();

		LastGetStringValue[0] = 0;
		if (p && p->GetValue(propertyName, LastGetStringValue, sizeof(LastGetStringValue)))
		{
			return LastGetStringValue;
		}
	}

	return defaultVal;
}

//-------------------------------------------------------------------------------------
// *** Latency Test

bool HMDState::ProcessLatencyTest(unsigned char rgbColorOut[3])
{
    bool result = false;

    // Check create.
    if (pLatencyTester)
    {
        if (pLatencyTester->IsConnected())
        {
            Color colorToDisplay;

            LatencyUtil.ProcessInputs();
            result = LatencyUtil.DisplayScreenColor(colorToDisplay);
            rgbColorOut[0] = colorToDisplay.R;
            rgbColorOut[1] = colorToDisplay.G;
            rgbColorOut[2] = colorToDisplay.B;
        }
        else
        {
            // Disconnect.
            LatencyUtil.SetDevice(NULL);
            pLatencyTester = 0;
            LogText("LATENCY SENSOR disconnected.\n");
        }
    }
    else if (AddLatencyTestCount > 0)
    {
        // This might have some unlikely race condition issue which could cause us to miss a device...
        AddLatencyTestCount = 0;

        pLatencyTester = *GlobalState::pInstance->GetManager()->
                            EnumerateDevices<LatencyTestDevice>().CreateDevice();
        if (pLatencyTester)
        {
            LatencyUtil.SetDevice(pLatencyTester);
            LogText("LATENCY TESTER connected\n");
        }        
    }
    
    return result;
}

void HMDState::ProcessLatencyTest2(unsigned char rgbColorOut[3], double startTime)
{
    // Check create.
    if (!(EnabledHmdCaps & ovrHmdCap_LatencyTest))
        return;

    if (pLatencyTesterDisplay && !LatencyUtil2.HasDisplayDevice())
    {
        if (!pLatencyTesterDisplay->IsConnected())
        {
            LatencyUtil2.SetDisplayDevice(NULL);
        }
    }
    else if (AddLatencyTestDisplayCount > 0)
    {
        // This might have some unlikely race condition issue
        // which could cause us to miss a device...
        AddLatencyTestDisplayCount = 0;

        pLatencyTesterDisplay = *GlobalState::pInstance->GetManager()->
                                 EnumerateDevices<LatencyTestDevice>().CreateDevice();
        if (pLatencyTesterDisplay)
        {
            LatencyUtil2.SetDisplayDevice(pLatencyTesterDisplay);
        }
    }

    if (LatencyUtil2.HasDevice() && pSensor && pSensor->IsConnected())
    {
        LatencyUtil2.BeginTest(startTime);

        Color colorToDisplay;
        LatencyTest2Active = LatencyUtil2.DisplayScreenColor(colorToDisplay);
        rgbColorOut[0] = colorToDisplay.R;
        rgbColorOut[1] = colorToDisplay.G;
        rgbColorOut[2] = colorToDisplay.B;
    }
    else
    {
        LatencyTest2Active = false;
    }
}

//-------------------------------------------------------------------------------------
// *** Rendering

bool HMDState::ConfigureRendering(ovrEyeRenderDesc eyeRenderDescOut[2],
                                  const ovrFovPort eyeFovIn[2],
                                  const ovrRenderAPIConfig* apiConfig,                                  
                                  unsigned distortionCaps)
{
    ThreadChecker::Scope checkScope(&RenderAPIThreadChecker, "ovrHmd_ConfigureRendering");

    // null -> shut down.
    if (!apiConfig)
    {
        if (pRenderer)
            pRenderer.Clear();        
        RenderingConfigured = false; 
        return true;
    }

    if (pRenderer &&
        (apiConfig->Header.API != pRenderer->GetRenderAPI()))
    {
        // Shutdown old renderer.
        if (pRenderer)
            pRenderer.Clear();
    }


    // Step 1: do basic setup configuration
    RenderState.setupRenderDesc(eyeRenderDescOut, eyeFovIn);
    RenderState.EnabledHmdCaps = EnabledHmdCaps;     // This is a copy... Any cleaner way?
    RenderState.DistortionCaps = distortionCaps;

    TimeManager.ResetFrameTiming(0,
                                 (EnabledHmdCaps & ovrHmdCap_DynamicPrediction) ? true : false,
                                 true);

    LastFrameTimeSeconds = 0.0f;

    // Set RenderingConfigured early to avoid ASSERTs in renderer initialization.
    RenderingConfigured = true;

    if (!pRenderer)
    {
        pRenderer = *DistortionRenderer::APICreateRegistry
                        [apiConfig->Header.API](this, TimeManager, RenderState);
    }

    if (!pRenderer ||
        !pRenderer->Initialize(apiConfig, distortionCaps))
    {
        RenderingConfigured = false;
        return false;
    }    

    return true;
}



ovrPosef HMDState::BeginEyeRender(ovrEyeType eye)
{
    // Debug checks.
    checkBeginFrameScope("ovrHmd_BeginEyeRender");
    ThreadChecker::Scope checkScope(&RenderAPIThreadChecker, "ovrHmd_BeginEyeRender");

    // Unknown eyeId provided in ovrHmd_BeginEyeRender
    OVR_ASSERT_LOG(eye == ovrEye_Left || eye == ovrEye_Right,
                   ("ovrHmd_BeginEyeRender eyeId out of range."));     
    OVR_ASSERT_LOG(EyeRenderActive[eye] == false,
                   ("Multiple calls to ovrHmd_BeginEyeRender for the same eye."));

    EyeRenderActive[eye] = true;
    
    // Only process latency tester for drawing the left eye (assumes left eye is drawn first)
    if (pRenderer && eye == 0)
    {
        LatencyTestActive = ProcessLatencyTest(LatencyTestDrawColor);
    }

    return ovrHmd_GetEyePose(this, eye);
}


void HMDState::EndEyeRender(ovrEyeType eye, ovrPosef renderPose, ovrTexture* eyeTexture)
{
    // Debug checks.
    checkBeginFrameScope("ovrHmd_EndEyeRender");
    ThreadChecker::Scope checkScope(&RenderAPIThreadChecker, "ovrHmd_EndEyeRender");

    if (!EyeRenderActive[eye])
    {
        OVR_ASSERT_LOG(false,
                       ("ovrHmd_EndEyeRender called without ovrHmd_BeginEyeRender."));
        return;
    }

    RenderState.EyeRenderPoses[eye] = renderPose;

    if (pRenderer)
        pRenderer->SubmitEye(eye, eyeTexture);

    EyeRenderActive[eye] = false;
}

}} // namespace OVR::CAPI