﻿using UnityEngine;
using System.Collections.Generic;

using OpenHMDNet;
using System;
using System.IO;

public class OpenHMDCamera : MonoBehaviour {

    OpenHMD openHMD;
    Device selectedDevice;

    public Camera currentCamera { get; set; }

	static OpenHMDCamera(){
		String currentPath = Environment.GetEnvironmentVariable("PATH", EnvironmentVariableTarget.Process);
		String dllPath = Environment.CurrentDirectory + Path.DirectorySeparatorChar + "Assets" + Path.DirectorySeparatorChar + "Plugins";
		if(currentPath.Contains(dllPath) == false)
		{
			Environment.SetEnvironmentVariable("PATH", currentPath + Path.PathSeparator + dllPath, EnvironmentVariableTarget.Process);
		}
	}

    Matrix4x4 floatArrayToMatrix(float[] array)
    {
        Matrix4x4 m = new Matrix4x4();
        m[0, 0] = array[0];
        m[1, 0] = array[1];
        m[2, 0] = array[2];
        m[3, 0] = array[3];
        m[0, 1] = array[4];
        m[1, 1] = array[5];
        m[2, 1] = array[6];
        m[3, 1] = array[7];
        m[0, 2] = array[8];
        m[1, 2] = array[9];
        m[2, 2] = array[10];
        m[3, 2] = array[11];
        m[0, 3] = array[12];
        m[1, 3] = array[13];
        m[2, 3] = array[14];
        m[3, 3] = array[15];
        return m;
    }

	Matrix4x4 convertHandness(Matrix4x4 m) //Converts the OpenGL projection matrix to right hand
	{
		m.m20 *= -1f;
		m.m21 *= -1f;
		m.m22 *= -1f;
		m.m23 *= -1f;

		return m;
	}

	// Use this for initialization
	void Start () {
        if (gameObject.name != "OpenHMD-Camera")
        {
            Debug.Log("This script should be run with the OpenHMDCamera prefab, stopping script");
            this.enabled = false;
        }

		openHMD = new OpenHMD ();

		openHMD.openContext ();
		List<Device> devices = openHMD.getDevices ();
		selectedDevice = devices [0];
		openHMD.openDevice (selectedDevice);
	}
	
	// Update is called once per frame
	void Update () {
 		openHMD.update ();
		//float[] rotation = openHMD.getFloatInformation(selectedDevice, 4, OpenHMD.ohmd_float_value.OHMD_ROTATION_QUAT);
        float[] leftProjectionMatrix = openHMD.getFloatInformation(selectedDevice, 16, OpenHMD.ohmd_float_value.OHMD_LEFT_EYE_GL_PROJECTION_MATRIX);
        float[] rightProjectionMatrix = openHMD.getFloatInformation(selectedDevice, 16, OpenHMD.ohmd_float_value.OHMD_RIGHT_EYE_GL_PROJECTION_MATRIX);
        float[] leftWorldViewMatrix = openHMD.getFloatInformation(selectedDevice, 16, OpenHMD.ohmd_float_value.OHMD_LEFT_EYE_GL_MODELVIEW_MATRIX);
        float[] rightWorldViewMatrix = openHMD.getFloatInformation(selectedDevice, 16, OpenHMD.ohmd_float_value.OHMD_RIGHT_EYE_GL_MODELVIEW_MATRIX);

        Camera openhmdCam = gameObject.GetComponent<Camera>();//gameObject.transform.Find("OpenHMD-Camera").GetComponent<Camera>();
        //Quaternion quat = new Quaternion(rotation[0], rotation[1], -rotation[2], rotation[3]);
        Matrix4x4 leftProjMatrix = floatArrayToMatrix(leftProjectionMatrix);
        Matrix4x4 rightProjMatrix = floatArrayToMatrix(rightProjectionMatrix);
        Matrix4x4 leftViewMatrix = floatArrayToMatrix(leftWorldViewMatrix);
        Matrix4x4 rightViewMatrix = floatArrayToMatrix(rightWorldViewMatrix);

		leftViewMatrix = leftViewMatrix.inverse;
		rightViewMatrix = rightViewMatrix.inverse;

		openhmdCam.SetStereoProjectionMatrix(Camera.StereoscopicEye.Left, leftProjMatrix);
		openhmdCam.SetStereoProjectionMatrix(Camera.StereoscopicEye.Right, rightProjMatrix);
		openhmdCam.SetStereoViewMatrix(Camera.StereoscopicEye.Left, convertHandness(leftViewMatrix)*transform.worldToLocalMatrix);
		openhmdCam.SetStereoViewMatrix(Camera.StereoscopicEye.Right, convertHandness(rightViewMatrix)*transform.worldToLocalMatrix);
    }

    void OnApplicationQuit() {
		openHMD.closeContext ();
	}
}
