//========= Copyright Valve Corporation ============//
#define VR_API_EXPORT 1

#if defined(_WIN32)
#define DYNAMIC_LIB_EXT ".dll"
#elif defined(__APPLE__)
#define DYNAMIC_LIB_EXT ".dylib"
#else
#define DYNAMIC_LIB_EXT ".so"
#endif

#include "openvr.h"
#include "ivrclientcore.h"
#include <vrcore/pathtools_public.h>
#include <vrcore/sharedlibtools_public.h>
#include <vrcore/envvartools_public.h>
#include "hmderrors_public.h"
#include <vrcore/strtools_public.h>
#include <vrcore/vrpathregistry_public.h>
#include <mutex>

using vr::EVRInitError;
using vr::IVRSystem;
using vr::IVRClientCore;
using vr::VRInitError_None;
using vr::TrackedDevicePose_t;
using vr::IVRRawIMUData;
using vr::ImuSample_t;
using vr::k_unMaxTrackedDeviceCount;
using vr::k_unTrackedDeviceIndex_Hmd;
using vr::TrackingUniverseStanding;
using vr::OffScale_AccelX;
using vr::OffScale_AccelY;
using vr::OffScale_AccelZ;
using vr::OffScale_GyroX;
using vr::OffScale_GyroY;
using vr::OffScale_GyroZ;

// figure out how to import from the VR API dll
#if defined(_WIN32)

#if !defined(OPENVR_BUILD_STATIC)
#define VR_EXPORT_INTERFACE extern "C" __declspec( dllexport )
#else
#define VR_EXPORT_INTERFACE extern "C"
#endif

#elif defined(__GNUC__) || defined(COMPILER_GCC) || defined(__APPLE__)

#define VR_EXPORT_INTERFACE extern "C" __attribute__((visibility("default")))

#else
#error "Unsupported Platform."
#endif

namespace vr
{

class CRawIMUData : public IVRRawIMUData
{
public:
    bool GetRawGyroscopeData(float* x, float* y, float* z)
    {
        if (!m_bHasNewData) return false;
        *x = (float)m_latestSample.vGyro.v[0];
        *y = (float)m_latestSample.vGyro.v[1];
        *z = (float)m_latestSample.vGyro.v[2];
        return true;
    }
    
    bool GetRawAccelerometerData(float* x, float* y, float* z)
    {
        if (!m_bHasNewData) return false;
        *x = (float)m_latestSample.vAccel.v[0];
        *y = (float)m_latestSample.vAccel.v[1];
        *z = (float)m_latestSample.vAccel.v[2];
        return true;
    }
    
    bool GetIMUSampleTimestamp(double* timestamp)
    {
        if (!m_bHasNewData) return false;
        *timestamp = m_latestSample.fSampleTime;
        return true;
    }
    
    bool GetIMUSample(ImuSample_t* pSample)
    {
        if (!m_bHasNewData || !pSample) return false;
        *pSample = m_latestSample;
        return true;
    }

    void UpdateIMUData(const ImuSample_t &sample)
    {
        m_latestSample = sample;
        m_bHasNewData = true;
    }

private:
    ImuSample_t m_latestSample;
    bool m_bHasNewData = false;
};

static void *g_pVRModule = NULL;
static IVRClientCore *g_pHmdSystem = NULL;
static IVRSystem *g_pVRSystem = NULL;  // Track the IVRSystem interface
static CRawIMUData g_rawIMUData;  // Global instance of IMU interceptor
static std::recursive_mutex g_mutexSystem;


typedef void* (*VRClientCoreFactoryFn)(const char *pInterfaceName, int *pReturnCode);

static uint32_t g_nVRToken = 0;

uint32_t VR_GetInitToken()
{
	return g_nVRToken;
}

EVRInitError VR_LoadHmdSystemInternal();
void CleanupInternalInterfaces();


uint32_t VR_InitInternal2( EVRInitError *peError, vr::EVRApplicationType eApplicationType, const char *pStartupInfo )
{
	std::lock_guard<std::recursive_mutex> lock( g_mutexSystem );

	EVRInitError err = VR_LoadHmdSystemInternal();
	if ( err == vr::VRInitError_None )
	{
		err = g_pHmdSystem->Init( eApplicationType, pStartupInfo );
	}

	if ( peError )
		*peError = err;

	if ( err != VRInitError_None )
	{
		SharedLib_Unload( g_pVRModule );
		g_pHmdSystem = NULL;
		g_pVRModule = NULL;

		return 0;
	}

	return ++g_nVRToken;
}

VR_INTERFACE uint32_t VR_CALLTYPE VR_InitInternal( EVRInitError *peError, EVRApplicationType eApplicationType );

uint32_t VR_InitInternal( EVRInitError *peError, vr::EVRApplicationType eApplicationType )
{
	return VR_InitInternal2( peError, eApplicationType, nullptr );
}

void VR_ShutdownInternal()
{
	std::lock_guard<std::recursive_mutex> lock( g_mutexSystem );

#if !defined( VR_API_PUBLIC )
	CleanupInternalInterfaces();
#endif

	if ( g_pHmdSystem )
	{
		g_pHmdSystem->Cleanup();
		g_pHmdSystem = NULL;
	}

	if ( g_pVRModule )
	{
		SharedLib_Unload( g_pVRModule );
		g_pVRModule = NULL;
	}

	++g_nVRToken;
}

EVRInitError VR_LoadHmdSystemInternal()
{
	std::string sRuntimePath, sConfigPath, sLogPath;

	bool bReadPathRegistry = CVRPathRegistry_Public::GetPaths( &sRuntimePath, &sConfigPath, &sLogPath, NULL, NULL );
	if( !bReadPathRegistry )
	{
		return vr::VRInitError_Init_PathRegistryNotFound;
	}

	// figure out where we're going to look for vrclient.dll
	// see if the specified path actually exists.
	if( !Path_IsDirectory( sRuntimePath ) )
	{
		return vr::VRInitError_Init_InstallationNotFound;
	}

	// Because we don't have a way to select debug vs. release yet we'll just
	// use debug if it's there
#if defined( WIN32 ) || defined( LINUX32 )
	std::string sTestPath = Path_Join( sRuntimePath, "bin" );
#else
	std::string sTestPath = Path_Join( sRuntimePath, "bin", PLATSUBDIR );
#endif
	if( !Path_IsDirectory( sTestPath ) )
	{
		return vr::VRInitError_Init_InstallationCorrupt;
	}

#if defined( WIN64 )
	std::string sDLLPath = Path_Join( sTestPath, std::string("vrclient_x64").append(DYNAMIC_LIB_EXT) );
#else
	std::string sDLLPath = Path_Join( sTestPath, std::string("vrclient").append(DYNAMIC_LIB_EXT) );
#endif

	// only look in the override
	void *pMod = SharedLib_Load( sDLLPath.c_str() );
	// nothing more to do if we can't load the DLL
	if( !pMod )
	{
		return vr::VRInitError_Init_VRClientDLLNotFound;
	}

	VRClientCoreFactoryFn fnFactory = ( VRClientCoreFactoryFn )( SharedLib_GetFunction( pMod, "VRClientCoreFactory" ) );
	if( !fnFactory )
	{
		SharedLib_Unload( pMod );
		return vr::VRInitError_Init_FactoryNotFound;
	}

	int nReturnCode = 0;
	g_pHmdSystem = static_cast< IVRClientCore * > ( fnFactory( vr::IVRClientCore_Version, &nReturnCode ) );
	if( !g_pHmdSystem )
	{
		SharedLib_Unload( pMod );
		return vr::VRInitError_Init_InterfaceNotFound;
	}

	g_pVRModule = pMod;
	return VRInitError_None;
}


// Function to extract IMU data from pose
static void ExtractIMUDataFromPose(const TrackedDevicePose_t& pose, double fPredictedSecondsFromNow, ImuSample_t& imuData)
{
    imuData.vAccel.v[0] = pose.vVelocity.v[0];
    imuData.vAccel.v[1] = pose.vVelocity.v[1];
    imuData.vAccel.v[2] = pose.vVelocity.v[2];
    
    imuData.vGyro.v[0] = pose.vAngularVelocity.v[0];
    imuData.vGyro.v[1] = pose.vAngularVelocity.v[1];
    imuData.vGyro.v[2] = pose.vAngularVelocity.v[2];
    
    // Use the prediction time as sample time
    imuData.fSampleTime = fPredictedSecondsFromNow;
    
    // Set flags if velocity or angular velocity magnitudes are suspiciously large
    imuData.unOffScaleFlags = 0;
    const float kMaxVelocity = 10.0f; // m/s
    const float kMaxAngularVelocity = 20.0f; // rad/s
    
    float velMagSq = pose.vVelocity.v[0] * pose.vVelocity.v[0] +
                     pose.vVelocity.v[1] * pose.vVelocity.v[1] +
                     pose.vVelocity.v[2] * pose.vVelocity.v[2];
                     
    float angVelMagSq = pose.vAngularVelocity.v[0] * pose.vAngularVelocity.v[0] +
                        pose.vAngularVelocity.v[1] * pose.vAngularVelocity.v[1] +
                        pose.vAngularVelocity.v[2] * pose.vAngularVelocity.v[2];
                        
    if (velMagSq > kMaxVelocity * kMaxVelocity)
    {
        imuData.unOffScaleFlags |= OffScale_AccelX | OffScale_AccelY | OffScale_AccelZ;
    }
    
    if (angVelMagSq > kMaxAngularVelocity * kMaxAngularVelocity)
    {
        imuData.unOffScaleFlags |= OffScale_GyroX | OffScale_GyroY | OffScale_GyroZ;
    }
}

void *VR_GetGenericInterface(const char *pchInterfaceVersion, EVRInitError *peError)
{
	std::lock_guard<std::recursive_mutex> lock( g_mutexSystem );

	// Check if requesting IMU interceptor
	if (pchInterfaceVersion && strcmp(pchInterfaceVersion, vr::IVRRawIMUData_Version) == 0)
	{
		if (peError)
			*peError = VRInitError_None;
		return static_cast<vr::IVRRawIMUData*>(&g_rawIMUData);
	}

	if (!g_pHmdSystem)
	{
		if (peError)
			*peError = vr::VRInitError_Init_NotInitialized;
		return NULL;
	}

	void* pInterface = g_pHmdSystem->GetGenericInterface(pchInterfaceVersion, peError);
	
	// If requesting IVRSystem, store it to intercept pose updates
	if (pInterface && pchInterfaceVersion && strcmp(pchInterfaceVersion, vr::IVRSystem_Version) == 0)
	{
		g_pVRSystem = static_cast<vr::IVRSystem*>(pInterface);
		
		// Get initial pose data
		TrackedDevicePose_t poses[vr::k_unMaxTrackedDeviceCount];
		g_pVRSystem->GetDeviceToAbsoluteTrackingPose(
			vr::TrackingUniverseStanding,
			0.0f, // Now
			poses,
			vr::k_unMaxTrackedDeviceCount
		);

		if (poses[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid)
		{
			ImuSample_t imuData;
			ExtractIMUDataFromPose(poses[vr::k_unTrackedDeviceIndex_Hmd], 0.0, imuData);
			g_rawIMUData.UpdateIMUData(imuData);
		}
	}

	return pInterface;
}

bool VR_IsInterfaceVersionValid(const char *pchInterfaceVersion)
{
	std::lock_guard<std::recursive_mutex> lock( g_mutexSystem );

	if (!g_pHmdSystem)
	{
		return false;
	}

	return g_pHmdSystem->IsInterfaceVersionValid(pchInterfaceVersion) == VRInitError_None;
}

bool VR_IsHmdPresent()
{
	std::lock_guard<std::recursive_mutex> lock( g_mutexSystem );

	if( g_pHmdSystem )
	{
		// if we're already initialized, just call through
		return g_pHmdSystem->BIsHmdPresent();
	}
	else
	{
		// otherwise we need to do a bit more work
		EVRInitError err = VR_LoadHmdSystemInternal();
		if( err != VRInitError_None )
			return false;

		bool bHasHmd = g_pHmdSystem->BIsHmdPresent();

		g_pHmdSystem = NULL;
		SharedLib_Unload( g_pVRModule );
		g_pVRModule = NULL;

		return bHasHmd;
	}
}

/** Returns true if the OpenVR runtime is installed. */
bool VR_IsRuntimeInstalled()
{
	std::lock_guard<std::recursive_mutex> lock( g_mutexSystem );

	if( g_pHmdSystem )
	{
		// if we're already initialized, OpenVR is obviously installed
		return true;
	}
	else
	{
		// otherwise we need to do a bit more work
		std::string sRuntimePath, sConfigPath, sLogPath;

		bool bReadPathRegistry = CVRPathRegistry_Public::GetPaths( &sRuntimePath, &sConfigPath, &sLogPath, NULL, NULL );
		if( !bReadPathRegistry )
		{
			return false;
		}

		// figure out where we're going to look for vrclient.dll
		// see if the specified path actually exists.
		if( !Path_IsDirectory( sRuntimePath ) )
		{
			return false;
		}

		// the installation may be corrupt in some way, but it certainly looks installed
		return true;
	}
}


// -------------------------------------------------------------------------------
// Purpose: This is the old Runtime Path interface that is no longer exported in the
//			latest header. We still want to export it from the DLL, though, so updating
//			to a new DLL doesn't break old compiled code. This version was not thread 
//			safe and could change the buffer pointer to by a previous result on a 
//			subsequent call
// -------------------------------------------------------------------------------
VR_EXPORT_INTERFACE const char *VR_CALLTYPE VR_RuntimePath();

/** Returns where OpenVR runtime is installed. */
const char *VR_RuntimePath()
{
	static char rchBuffer[1024];
	uint32_t unRequiredSize;
	if ( VR_GetRuntimePath( rchBuffer, sizeof( rchBuffer ), &unRequiredSize ) && unRequiredSize < sizeof( rchBuffer ) )
	{
		return rchBuffer;
	}
	else
	{
		return nullptr;
	}
}


/** Returns where OpenVR runtime is installed. */
bool VR_GetRuntimePath( char *pchPathBuffer, uint32_t unBufferSize, uint32_t *punRequiredBufferSize )
{
	// otherwise we need to do a bit more work
	std::string sRuntimePath;

	*punRequiredBufferSize = 0;

	bool bReadPathRegistry = CVRPathRegistry_Public::GetPaths( &sRuntimePath, nullptr, nullptr, nullptr, nullptr );
	if ( !bReadPathRegistry )
	{
		return false;
	}

	// figure out where we're going to look for vrclient.dll
	// see if the specified path actually exists.
	if ( !Path_IsDirectory( sRuntimePath ) )
	{
		return false;
	}

	*punRequiredBufferSize = (uint32_t)sRuntimePath.size() + 1;
	if ( sRuntimePath.size() >= unBufferSize )
	{
		*pchPathBuffer = '\0';
	}
	else
	{
		strcpy_safe( pchPathBuffer, unBufferSize, sRuntimePath.c_str() );
	}

	return true;
}


/** Returns the symbol version of an HMD error. */
const char *VR_GetVRInitErrorAsSymbol( EVRInitError error )
{
	std::lock_guard<std::recursive_mutex> lock( g_mutexSystem );

	if( g_pHmdSystem )
		return g_pHmdSystem->GetIDForVRInitError( error );
	else
		return GetIDForVRInitError( error );
}


/** Returns the english string version of an HMD error. */
const char *VR_GetVRInitErrorAsEnglishDescription( EVRInitError error )
{
	std::lock_guard<std::recursive_mutex> lock( g_mutexSystem );

	if ( g_pHmdSystem )
		return g_pHmdSystem->GetEnglishStringForHmdError( error );
	else
		return GetEnglishStringForHmdError( error );
}


VR_INTERFACE const char *VR_CALLTYPE VR_GetStringForHmdError( vr::EVRInitError error );

/** Returns the english string version of an HMD error. */
const char *VR_GetStringForHmdError( EVRInitError error )
{
	return VR_GetVRInitErrorAsEnglishDescription( error );
}

}