#include <ACGL/HardwareSupport/SimpleRiftController.hh>
#include <ACGL/OpenGL/Creator/ShaderProgramCreator.hh>

#ifdef ACGL_USE_OCULUS_RIFT

using namespace OVR;
using namespace ACGL;
using namespace ACGL::Utils;
using namespace ACGL::Scene;
using namespace ACGL::HardwareSupport;
using namespace std;


SimpleRiftController::SimpleRiftController( uint32_t _riftnumber, bool _performAutomaticMagneticCalibration )
{
    mSuccessfulConnected = false;
    mDistortionScaleFactor = 1.0f;

    // default values from the first devkit:
    mORHMDInfo.HResolution = 1280;
    mORHMDInfo.VResolution =  800;
    mORHMDInfo.HScreenSize = 0.14976f;
    mORHMDInfo.VScreenSize = 0.0935f;
    mORHMDInfo.VScreenCenter = mORHMDInfo.VScreenSize*0.5f;
    mORHMDInfo.DistortionK[0]        = 1.0f;
    mORHMDInfo.DistortionK[1]        = 0.22f;
    mORHMDInfo.DistortionK[2]        = 0.24f;
    mORHMDInfo.EyeToScreenDistance   = 0.041f;
    mORHMDInfo.ChromaAbCorrection[0] = 0.996f;
    mORHMDInfo.ChromaAbCorrection[1] = -0.004f;
    mORHMDInfo.ChromaAbCorrection[2] = 1.014f;
    mORHMDInfo.ChromaAbCorrection[3] = 0.0f;
    mORHMDInfo.LensSeparationDistance = 0.0635f;
    mORHMDInfo.InterpupillaryDistance = 0.064f;
    mORHMDInfo.DisplayDeviceName[0] = 0;

    mUseDistortion = true;
    mUseChromaticAberation = true;

    mPredictionTime = -1.0f;

    mCamera = SharedHMDCamera(); // set to NULL
    attachCamera( SharedHMDCamera( new HMDCamera() ) ); // attach a blank camera
    // set a good default viewport:
    glm::uvec2 viewport = getPhysicalScreenResolution();
    mCamera->resize( viewport.x/2, viewport.y );
    updateCameraFoV(); // indirectly based on the viewport

    debug() << "try to connect to Oculus Rift via SDK " << OVR_VERSION_STRING << endl;

    if (_riftnumber != 0) {
        error() << "opening any other Rift than ID 0 is not supported yet! Trying to open Rift nr 0" << endl;
    }

    System::Init( Log::ConfigureDefaultLog( LogMask_All) );

    mORManager = *DeviceManager::Create();
    if (!mORManager) {
        error() << "could not create a Rift Device Manager" << endl;
        return;
    }

    mORDevice = *mORManager->EnumerateDevices<HMDDevice>().CreateDevice();
    if (!mORDevice) {
        error() << "could not create a Rift device" << endl;
        return;
    }

    debug() << "found HMD" << endl;

    if (!mORDevice->GetDeviceInfo( &mORHMDInfo )) {
        error() << "could not get HMD device info" << endl;
        return;
    }

    mORSensor = *mORDevice->GetSensor();
    if (!mORSensor) {
        error() << "could not get sensor of HMD" << endl;
#ifdef __linux
        error() << "do you have read/write permissions of /dev/hidraw* ?" << endl;
#endif
        return;
    }

    mORSensorFusion.AttachToSensor( mORSensor );

    mSuccessfulConnected = true;

    if (_performAutomaticMagneticCalibration) {
        startMagneticCalibration();
    }
}

SimpleRiftController::~SimpleRiftController()
{
    // setting the reference counted pointers to NULL will call the object destructors:
    debug() << "disconnecting from Oculus Rift..." << endl;

    mORSensor  = NULL;
    mORDevice  = NULL;
    mORManager = NULL;
    mORSensorFusion.AttachToSensor( NULL );
    System::Destroy(); // Oculus Rift
}

void SimpleRiftController::startMagneticCalibration()
{
    deactivateMagneticDriftCorrection(); // in case there was an old calibration already
    mMagneticCalibration.BeginAutoCalibration( mORSensorFusion );
}

bool SimpleRiftController::magneticCalibrationDone()
{
    return mMagneticCalibration.IsCalibrated();
}

void SimpleRiftController::deactivateMagneticDriftCorrection()
{
    mMagneticCalibration.ClearCalibration( mORSensorFusion );
}

void SimpleRiftController::setPrediction( float _seconds )
{
    if ( _seconds >= 0.0f ) {
        mORSensorFusion.SetPrediction( _seconds );
    } else {
        mORSensorFusion.SetPrediction( 0.0f, false );
    }
    mPredictionTime = _seconds;
}


glm::mat4 SimpleRiftController::getProjectionMatrixFromCamera()
{
    if (!mCamera) return glm::mat4();
    return mCamera->getProjectionMatrix();
}

glm::mat3 riftMatrixToGLM( const Matrix4f &_mat4 )
{
    glm::mat3 glmMat3;

    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            glmMat3[i][j] = _mat4.M[i][j];
        }
    }

    return glmMat3;
}

void SimpleRiftController::attachCamera( ACGL::Scene::SharedHMDCamera _camera )
{
    mCamera = _camera;
    mCamera->setInterpupillaryDistance( mORHMDInfo.InterpupillaryDistance );
    updateCameraFoV();

    float viewCenterH = mORHMDInfo.HScreenSize * 0.25f;
    float eyeProjectionShift = viewCenterH - mORHMDInfo.LensSeparationDistance * 0.5f;
    float offsetX = 4.0f * eyeProjectionShift / mORHMDInfo.HScreenSize;
    mCamera->setProjectionCenterOffset( glm::vec2(offsetX, 0.0f));

    mCamera->setNeckToEyeVerticalDistance(   0.12f );
    mCamera->setNeckToEyeHorizontalDistance( 0.08f );
}

glm::mat3 SimpleRiftController::getCurrentRotation()
{
    // magnetic calibration:
    if (mMagneticCalibration.IsAutoCalibrating()) {
        mMagneticCalibration.UpdateAutoCalibration( mORSensorFusion );

        if (mMagneticCalibration.IsCalibrated()) {
            Vector3f mc = mMagneticCalibration.GetMagCenter();
            debug() << "mag center " << mc.x << " " << mc.y << " " << mc.z << " found, enabling yaw correction" << endl;
            mORSensorFusion.SetYawCorrectionEnabled( true );
        }
    }

    // update orientation:
    Quatf q;
    if (mPredictionTime > 0.0f) {
        q = mORSensorFusion.GetPredictedOrientation();
    } else {
        q = mORSensorFusion.GetOrientation();
    }

    q.Normalize();

    //debug() << "Rift orientation: " << q.x << " " << q.y << " " << q.z << " " << q.w << endl;

    q = mInverseNeutralRotation * q;

    if (mORSensorFusion.IsYawCorrectionEnabled()) {
        //debug() << "yaw corrected" << endl;
    }
    if (mMagneticCalibration.IsCalibrated()) {
        //debug() << "calibrated" << endl;
    }

    Matrix4f orientation( q );
    return riftMatrixToGLM( orientation );
}

void SimpleRiftController::setNeutralPosition()
{
    mInverseNeutralRotation = mORSensorFusion.GetOrientation();
    mInverseNeutralRotation.Normalize();
    mInverseNeutralRotation.x *= -1.0;
    mInverseNeutralRotation.y *= -1.0;
    mInverseNeutralRotation.z *= -1.0;
}

void SimpleRiftController::setNeutralYaw()
{
    mInverseNeutralRotation = OVR::Quatf();
    mORSensorFusion.Reset();
}

void SimpleRiftController::updateCamera()
{
    if (!mSuccessfulConnected) return;

    mCamera->setHMDRotation( getCurrentRotation() );
}

void SimpleRiftController::updateCameraFoV()
{
    float percievedHalfRTDist = (mORHMDInfo.VScreenSize / 2) * mDistortionScaleFactor;
    float vfov = 2.0f * atan( percievedHalfRTDist/mORHMDInfo.EyeToScreenDistance );
    vfov = ACGL::Math::Functions::calcRadToDeg( vfov );

    mCamera->setVerticalFieldOfView( vfov );
    debug() << "update VFoV: " << vfov << endl;
}

glm::uvec2 SimpleRiftController::getPhysicalScreenResolution()
{
    return glm::uvec2( mORHMDInfo.HResolution, mORHMDInfo.VResolution );
}

void SimpleRiftController::setDistortionScaleFactor( float _f ) {
    if (mDistortionScaleFactor == _f) return;
    ACGL::Utils::debug() << "set distortion scale " << _f << std::endl;
    mDistortionScaleFactor = _f;
    updateCameraFoV();
}

glm::vec4 SimpleRiftController::getShaderValue( int v )
{
    //
    // note that this isn't the virtul camera viewport (the size the offscreen rendering is performed in)
    // but the physical size of the Rifts screen:
    glm::vec2 windowSize = glm::vec2( getPhysicalScreenResolution() );

    glm::vec2 viewport = windowSize;
    viewport.x /= 2;

    glm::vec2 viewportPosL = glm::vec2(          0, 0 );
    glm::vec2 viewportPosR = glm::vec2( viewport.x, 0 );
    glm::vec2 viewportSize = glm::vec2( viewport.x, viewport.y ); // viewport of one eye

    float w  = float(viewportSize.x) / float(windowSize.x);
    float h  = float(viewportSize.y) / float(windowSize.y);
    float xl = float(viewportPosL.x) / float(windowSize.x);
    float yl = float(viewportPosL.y) / float(windowSize.y);

    float xr = float(viewportPosR.x) / float(windowSize.x);
    float yr = float(viewportPosR.y) / float(windowSize.y);

    // both eyes have the same aspect ratio: hals the windowsize as the image was rendered side by side
    float aspectRatio =  (0.5 * windowSize.x) / windowSize.y;

    float lensOffset        = mORHMDInfo.LensSeparationDistance * 0.5f;
    float lensShift         = mORHMDInfo.HScreenSize * 0.25f - lensOffset;
    float lensViewportShift = 4.0f * lensShift / mORHMDInfo.HScreenSize;
    float lensViewportShiftL =  lensViewportShift;
    float lensViewportShiftR = -lensViewportShift;

    glm::vec4 lensCenter;
    lensCenter.x = xl + (w + lensViewportShiftL * 0.5f)*0.5f;
    lensCenter.y = yl + h*0.5f;
    lensCenter.z = xr + (w + lensViewportShiftR * 0.5f)*0.5f;
    lensCenter.w = yr + h*0.5f;

    glm::vec4 screenCenter;
    screenCenter.x = xl + w*0.5f;
    screenCenter.y = yl + h*0.5f;
    screenCenter.z = xr + w*0.5f;
    screenCenter.w = yr + h*0.5f;

    glm::vec4 scale;
    scale.x = (w/2);
    scale.y = (h/2) * aspectRatio;
    scale /= mDistortionScaleFactor;

    glm::vec4 scaleIn;
    scaleIn.x = (2/w);
    scaleIn.y = (2/h) / aspectRatio;

    if (v == 0) return lensCenter;
    if (v == 1) return screenCenter;
    if (v == 2) return scale;
    return scaleIn;
}

glm::vec4 SimpleRiftController::getLensCenter()
{
    return getShaderValue(0);
}

glm::vec4 SimpleRiftController::getScreenCenter()
{
    return getShaderValue(1);
}

glm::vec4 SimpleRiftController::getScale()
{
    return getShaderValue(2);
}

glm::vec4 SimpleRiftController::getScaleIn()
{
    return getShaderValue(3);
}

glm::vec4 SimpleRiftController::getHmdWarpParam()
{
    glm::vec4 distortionK;
    distortionK.x = mORHMDInfo.DistortionK[0];
    distortionK.y = mORHMDInfo.DistortionK[1];
    distortionK.z = mORHMDInfo.DistortionK[2];
    distortionK.w = mORHMDInfo.DistortionK[3];
    return distortionK;
}

glm::vec4 SimpleRiftController::getChromAbParam()
{
    glm::vec4 chromaK;
    chromaK.x = mORHMDInfo.ChromaAbCorrection[0];
    chromaK.y = mORHMDInfo.ChromaAbCorrection[1];
    chromaK.z = mORHMDInfo.ChromaAbCorrection[2];
    chromaK.w = mORHMDInfo.ChromaAbCorrection[3];
    return chromaK;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// optional rendering:
//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void SimpleRiftController::renderDistorted( OpenGL::ConstSharedTexture2D _sideBySideTexture )
{
    if (!mDistortShaderSideBySide) {
        // initialize shaders:
        mDistortShaderSideBySide = OpenGL::ShaderProgramFileManager::the()->get( OpenGL::ShaderProgramCreator("RiftDistortSideBySide.fsh").andFile("RiftDistort.vsh") );
        if (!mDistortShaderSideBySide) {
            ACGL::Utils::error() << "could not load distortion shader!" << std::endl;
            return;
        }
    }

    mDistortShaderSideBySide->use();
    mDistortShaderSideBySide->setTexture("uSamplerColor", _sideBySideTexture, 0 );
    renderDistorted( mDistortShaderSideBySide );
}

void SimpleRiftController::renderDistorted( OpenGL::ConstSharedTexture2D _leftTexture, OpenGL::ConstSharedTexture2D _rightTexture )
{
    if (!mDistortShaderTwoTextures) {
        // initialize shaders:
        mDistortShaderTwoTextures = OpenGL::ShaderProgramFileManager::the()->get( OpenGL::ShaderProgramCreator("RiftDistortTwoTexture.fsh").andFile("RiftDistort.vsh") );
        if (!mDistortShaderTwoTextures) {
            ACGL::Utils::error() << "could not load distortion shader!" << std::endl;
            return;
        }
    }

    mDistortShaderTwoTextures->use();
    mDistortShaderTwoTextures->setTexture("uSamplerColorLeft",  _leftTexture,  0 );
    mDistortShaderTwoTextures->setTexture("uSamplerColorRight", _rightTexture, 1 );
    renderDistorted( mDistortShaderTwoTextures );
}

void SimpleRiftController::renderDistorted( ACGL::OpenGL::ConstSharedShaderProgram _program )
{
    // if the user defined an output size, use that, otherwise default to the Rifts size:
    glm::uvec2 windowSize = mOutputViewport;
    if (windowSize.x == 0) {
        windowSize = getPhysicalScreenResolution();
    }

    glBindFramebuffer(GL_FRAMEBUFFER, 0);
    glViewport( 0, 0, windowSize.x, windowSize.y );
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // we assume that _program is in use as this should only get called from our own functions
    _program->setUniform("uLensCenter",   getLensCenter() );
    _program->setUniform("uScreenCenter", getScreenCenter() );
    _program->setUniform("uScale",        getScale() );
    _program->setUniform("uScaleIn",      getScaleIn() );
    _program->setUniform("uHmdWarpParam", getHmdWarpParam() );
    _program->setUniform("uChromAbParam", getChromAbParam() );
    _program->setUniform("uDistort",      (int)mUseDistortion );
    _program->setUniform("uCorrectChromaticAberation", (int)mUseChromaticAberation );

    // attribute-less rendering:
    // just rendering a fullscreen quad
    OpenGL::VertexArrayObject vao;
    vao.bind(); // 'empty' VAO -> no attributes are defined
    glDrawArrays( GL_TRIANGLE_STRIP, 0, 4 ); // create 2 triangles with no attributes
}

#endif