UE Eyetracking

#pragma once

#include <mutex>

#include "Defines.h"
#include "DoubleBuffered.h"
#include "Eyetracker.h"

#include "SRanipal.h"
#include "SRanipal_Eye.h"
#include "SRanipal_Enums.h"
#pragma comment(lib, "SRanipal.lib")

namespace ISTHapticWrapper
{

class Collisions;

void newCalibration();
void newEyeData(ViveSR::anipal::Eye::EyeData_v2 const &eyeData);
std::string ConvertErrorCode(int error);

enum AccuracyState {
	Dot0 = 0,
	Dot1 = 1,
	Dot2 = 2,
	Dot3 = 3,
	Dot4 = 4,
	Dot5 = 5,
	Dot6 = 6,
	Dot7 = 7,
	Dot8 = 8,
	End = 9
};

enum EyeHitKind {
	None = 0,
	Tooth = 1,
	Bur = 2,
	BurHandpiece = 3,
	Mirror = 4,
	MirrorHandpiece = 5,
	MirroredNone = 6,
	MirroredTooth = 7,
	MirroredBur = 8,
	MirroredBurHandpiece = 9,
	AccuracyPlane = 10
};

static std::string EyeHitKindToStr( EyeHitKind kind )
{
	switch(kind) {
	default:
	case EyeHitKind::None: return "None"; break;
	case EyeHitKind::Tooth: return "Tooth"; break;
	case EyeHitKind::Bur: return "Bur"; break;
	case EyeHitKind::BurHandpiece: return "BurHandpiece"; break;
	case EyeHitKind::Mirror: return "Mirror"; break;
	case EyeHitKind::MirrorHandpiece: return "MirrorHandpiece"; break;
	case EyeHitKind::MirroredNone: return "MirroredNone"; break;
	case EyeHitKind::MirroredTooth: return "MirroredTooth"; break;
	case EyeHitKind::MirroredBur: return "MirroredBur"; break;
	case EyeHitKind::MirroredBurHandpiece: return "MirroredBurHandpiece"; break;
	case EyeHitKind::AccuracyPlane: return "AccuracyPlane"; break;
	}
}

struct EyeData {
	ViveSR::anipal::Eye::EyeData_v2 verboseData;
	pd::Vec3<float> gazeOriginWorld;
	pd::Vec3<float> gazeOriginWorldRaw;
	pd::Vec3<float> gazeDirWorld;
	pd::Vec3<float> gazeDirWorldRaw;
	pd::Vec3<float> mirroredGazeOriginWorld;
	pd::Vec3<float> mirroredGazeDirWorld;
	pd::Vec3<float> gazeHitPointWorld;
	EyeHitKind hitKind;
	pd::Vec3<float> gazeHitPointTooth; // Only set when hitting the tooth
};

class Eyetracker
{
public:
	         Eyetracker( Collisions *clsns );
	virtual ~Eyetracker();

	// Threading
	void pause();
	void unpause();
	void shutdown();
	void join();

	float getFPS();

    void togglePaused() { m_isPaused ? unpause() : pause(); };
	void calibrate();
	void startCheck();
	void setHmdPose( pd::Matrix4<float> M_WfHMD );
	void setVrZoom( float zoomFactor );

    bool m_firstFrame = true;
    DoubleBuffered<EyeData> m_eyeData;

	// Static transformations
	DoubleBuffered<pd::Matrix4<float>> m_M_DfBur = pd::Matrix4<float>::IDENTITY();
	DoubleBuffered<pd::Matrix4<float>> m_M_DfMirror = pd::Matrix4<float>::IDENTITY();
	DoubleBuffered<pd::Matrix4<float>> m_M_BurfD = pd::Matrix4<float>::IDENTITY();
	DoubleBuffered<pd::Matrix4<float>> m_M_MirrorfD = pd::Matrix4<float>::IDENTITY();

	// Dynamic auxillary transformations
	pd::Matrix4<float> m_M_WfDevice[2] = { pd::Matrix4<float>::IDENTITY(), pd::Matrix4<float>::IDENTITY() };
	bool m_deviceButtonPressed[2] = { false, false };

	// Dynamic object transformations
	pd::Matrix4<float> m_M_WfBur = pd::Matrix4<float>::IDENTITY();
	pd::Matrix4<float> m_M_WfMirror = pd::Matrix4<float>::IDENTITY();
	pd::Matrix4<float> m_M_WfTooth = pd::Matrix4<float>::IDENTITY();
	pd::Matrix4<float> m_M_BurfW = pd::Matrix4<float>::IDENTITY();
	pd::Matrix4<float> m_M_MirrorfW = pd::Matrix4<float>::IDENTITY();
	pd::Matrix4<float> m_M_ToothfW = pd::Matrix4<float>::IDENTITY();

	// VR HMD
    DoubleBuffered<pd::Matrix4<float>> m_M_WfHmd = pd::Matrix4<float>::IDENTITY();
    DoubleBuffered<pd::Matrix4<float>> m_M_HmdfW = pd::Matrix4<float>::IDENTITY();
    DoubleBuffered<pd::Vec3<float>> m_WfHmd_Pos = pd::Vec3<float>::ZERO();
    DoubleBuffered<pd::Matrix4<float>> m_WfHmd_Rot = pd::Matrix4<float>::IDENTITY();
	// VR Zoom
    DoubleBuffered<float> m_vrZoomFactor = 1;
	DoubleBuffered<float> m_worldToMeters = 100;

	// Accuracy Check
	bool m_accuracyChecking = false;
	DoubleBuffered<pd::Vec3<float>> m_currentDot = pd::Vec3<float>::ZERO();

private:
	void asyncTask();
	void run();
	void nextFrame();
	void processEyeData(ViveSR::anipal::Eye::EyeData_v2 const &eyeData);

	Collisions *m_clsns = nullptr;

	// FPS
    std::ofstream m_log;
	unsigned long long m_framesFps = 0;
	std::chrono::high_resolution_clock::time_point m_fpsStartTime = std::chrono::high_resolution_clock::now();
	float m_fps = 0;

	// Threading
	std::thread *m_thread = nullptr;
	bool m_isReady = false;
	bool m_isPaused = true;
	bool m_shutdown = false;
	bool m_exited = false;
	bool m_trackingIsOn = false;
	std::mutex m_pauseMutex;
	std::condition_variable m_resumeCondition;
	unsigned long long m_frames = 0;
	std::chrono::time_point<std::chrono::high_resolution_clock> m_startTime = std::chrono::high_resolution_clock::now();
	std::chrono::time_point<std::chrono::high_resolution_clock> m_frameTime = std::chrono::high_resolution_clock::now();
	bool m_justCalibrated = false;
	bool m_calibrated = false;
	bool m_eyesDetected = false;
	int m_eyesLostCount = 1;

	// Accuracy Checking
	std::chrono::time_point<std::chrono::high_resolution_clock> m_accuracyStartTime;
	AccuracyState m_accuracyState = AccuracyState::Dot0;
	std::vector<float> m_accuracyErrors[9];
	float m_accuracyError = 0;
};

}