//
//  Faceplus.cpp
//  interface
//
//  Created by Andrzej Kapolka on 4/8/14.
//  Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
//

#ifdef HAVE_FACEPLUS
#include <faceplus.h>
#endif

#include "Application.h"
#include "Faceplus.h"
#include "renderer/FBXReader.h"

Faceplus::Faceplus() :
    _enabled(false),
    _active(false) {

#ifdef HAVE_FACEPLUS
    // these are ignored--any values will do
    faceplus_log_in("username", "password");
#endif
}

Faceplus::~Faceplus() {
    setEnabled(false);
}

void Faceplus::init() {
    connect(Application::getInstance()->getFaceshift(), SIGNAL(connectionStateChanged()), SLOT(updateEnabled()));
    updateEnabled();
}

#ifdef HAVE_FACEPLUS
static QMultiHash<QByteArray, QPair<int, float> > createChannelNameMap() {
    QMultiHash<QByteArray, QPair<QByteArray, float> > blendshapeMap;
    blendshapeMap.insert("EyeBlink_L", QPair<QByteArray, float>("Mix::Blink_Left", 1.0f));
    blendshapeMap.insert("EyeBlink_R", QPair<QByteArray, float>("Mix::Blink_Right", 1.0f));
    blendshapeMap.insert("BrowsD_L", QPair<QByteArray, float>("Mix::BrowsDown_Left", 1.0f));
    blendshapeMap.insert("BrowsD_R", QPair<QByteArray, float>("Mix::BrowsDown_Right", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::BrowsIn_Left", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::BrowsIn_Right", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::BrowsOuterLower_Left", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::BrowsOuterLower_Right", 1.0f));
    blendshapeMap.insert("BrowsU_L", QPair<QByteArray, float>("Mix::BrowsUp_Left", 5.0f));
    blendshapeMap.insert("BrowsU_R", QPair<QByteArray, float>("Mix::BrowsUp_Right", 5.0f));
    blendshapeMap.insert("EyeOpen_L", QPair<QByteArray, float>("Mix::EyesWide_Left", 1.0f));
    blendshapeMap.insert("EyeOpen_R", QPair<QByteArray, float>("Mix::EyesWide_Right", 1.0f));
    blendshapeMap.insert("MouthFrown_L", QPair<QByteArray, float>("Mix::Frown_Left", 1.0f));
    blendshapeMap.insert("MouthFrown_R", QPair<QByteArray, float>("Mix::Frown_Right", 1.0f));
    blendshapeMap.insert("JawLeft", QPair<QByteArray, float>("Mix::Jaw_RotateY_Left", 1.0f));
    blendshapeMap.insert("JawRight", QPair<QByteArray, float>("Mix::Jaw_RotateY_Right", 1.0f));
    blendshapeMap.insert("LipsLowerDown", QPair<QByteArray, float>("Mix::LowerLipDown_Left", 0.5f));
    blendshapeMap.insert("LipsLowerDown", QPair<QByteArray, float>("Mix::LowerLipDown_Right", 0.5f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::LowerLipIn", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::LowerLipOut", 1.0f));
    blendshapeMap.insert("MouthLeft", QPair<QByteArray, float>("Mix::Midmouth_Left", 1.0f));
    blendshapeMap.insert("MouthRight", QPair<QByteArray, float>("Mix::Midmouth_Right", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::MouthDown", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::MouthNarrow_Left", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::MouthNarrow_Right", 1.0f));
    blendshapeMap.insert("JawOpen", QPair<QByteArray, float>("Mix::MouthOpen", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::MouthUp", 1.0f));
    blendshapeMap.insert("LipsPucker", QPair<QByteArray, float>("Mix::MouthWhistle_NarrowAdjust_Left", 0.5f));
    blendshapeMap.insert("LipsPucker", QPair<QByteArray, float>("Mix::MouthWhistle_NarrowAdjust_Right", 0.5f));
    blendshapeMap.insert("Sneer", QPair<QByteArray, float>("Mix::NoseScrunch_Left", 0.5f));
    blendshapeMap.insert("Sneer", QPair<QByteArray, float>("Mix::NoseScrunch_Right", 0.5f));
    blendshapeMap.insert("MouthSmile_L", QPair<QByteArray, float>("Mix::Smile_Left", 1.0f));
    blendshapeMap.insert("MouthSmile_R", QPair<QByteArray, float>("Mix::Smile_Right", 1.0f));
    blendshapeMap.insert("EyeSquint_L", QPair<QByteArray, float>("Mix::Squint_Left", 1.0f));
    blendshapeMap.insert("EyeSquint_R", QPair<QByteArray, float>("Mix::Squint_Right", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::UpperLipIn", 1.0f));
    blendshapeMap.insert("...", QPair<QByteArray, float>("Mix::UpperLipOut", 1.0f));
    blendshapeMap.insert("LipsUpperUp", QPair<QByteArray, float>("Mix::UpperLipUp_Left", 0.5f));
    blendshapeMap.insert("LipsUpperUp", QPair<QByteArray, float>("Mix::UpperLipUp_Right", 0.5f));
    
    QMultiHash<QByteArray, QPair<int, float> > channelNameMap;
    for (int i = 0;; i++) {
        QByteArray blendshape = FACESHIFT_BLENDSHAPES[i];
        if (blendshape.isEmpty()) {
            break;
        }
        for (QMultiHash<QByteArray, QPair<QByteArray, float> >::const_iterator it = blendshapeMap.constFind(blendshape);
                it != blendshapeMap.constEnd() && it.key() == blendshape; it++) {
            channelNameMap.insert(it.value().first, QPair<int, float>(i, it.value().second));
        }
    }
    
    return channelNameMap;
}

static const QMultiHash<QByteArray, QPair<int, float> >& getChannelNameMap() {
    static QMultiHash<QByteArray, QPair<int, float> > channelNameMap = createChannelNameMap();
    return channelNameMap;
}
#endif

void Faceplus::update() {
#ifdef HAVE_FACEPLUS
    if (!_active) {
        return;
    }
    if (!(_active = faceplus_synchronous_track() && faceplus_current_output_vector(_outputVector.data()))) {
        return;
    }
    _headRotation = glm::quat(glm::radians(glm::vec3(-_outputVector.at(_headRotationIndices[0]),
        _outputVector.at(_headRotationIndices[1]), -_outputVector.at(_headRotationIndices[2]))));
    _estimatedEyePitch = (_outputVector.at(_leftEyeRotationIndices[0]) + _outputVector.at(_rightEyeRotationIndices[0])) * -0.5f;
    _estimatedEyeYaw = (_outputVector.at(_leftEyeRotationIndices[1]) + _outputVector.at(_rightEyeRotationIndices[1])) * 0.5f;
    
    qFill(_blendshapeCoefficients.begin(), _blendshapeCoefficients.end(), 0.0f);
    for (int i = 0; i < _outputVector.size(); i++) {
        for (QMultiHash<int, QPair<int, float> >::const_iterator it = _channelIndexMap.constFind(i);
                it != _channelIndexMap.constEnd() && it.key() == i; it++) {
            _blendshapeCoefficients[it.value().first] += _outputVector.at(i) * it.value().second;
        }
    }
#endif
}

void Faceplus::reset() {
}

void Faceplus::updateEnabled() {
    setEnabled(Menu::getInstance()->isOptionChecked(MenuOption::Faceplus) &&
        !(Menu::getInstance()->isOptionChecked(MenuOption::Faceshift) &&
            Application::getInstance()->getFaceshift()->isConnectedOrConnecting()));
}

void Faceplus::setEnabled(bool enabled) {
#ifdef HAVE_FACEPLUS
    if (_enabled == enabled) {
        return;
    }
    if ((_enabled = enabled)) {
        if (faceplus_init("VGA")) {
            qDebug() << "Faceplus initialized.";
            _active = true;
            
            int channelCount = faceplus_output_channels_count();
            _outputVector.resize(channelCount);
            
            int maxIndex = -1;
            _channelIndexMap.clear();
            for (int i = 0; i < channelCount; i++) {
                QByteArray name = faceplus_output_channel_name(i);
                if (name == "Head_Joint::Rotation_X") {
                    _headRotationIndices[0] = i;
                
                } else if (name == "Head_Joint::Rotation_Y") {
                    _headRotationIndices[1] = i;
                
                } else if (name == "Head_Joint::Rotation_Z") {
                    _headRotationIndices[2] = i;
                
                } else if (name == "Left_Eye_Joint::Rotation_X") {
                    _leftEyeRotationIndices[0] = i;
                
                } else if (name == "Left_Eye_Joint::Rotation_Y") {
                    _leftEyeRotationIndices[1] = i;
                
                } else if (name == "Right_Eye_Joint::Rotation_X") {
                    _rightEyeRotationIndices[0] = i;
                
                } else if (name == "Right_Eye_Joint::Rotation_Y") {
                    _rightEyeRotationIndices[1] = i;
                }
                for (QMultiHash<QByteArray, QPair<int, float> >::const_iterator it = getChannelNameMap().constFind(name);
                        it != getChannelNameMap().constEnd() && it.key() == name; it++) {
                    _channelIndexMap.insert(i, it.value());
                    maxIndex = qMax(maxIndex, it.value().first);
                }
            }
            _blendshapeCoefficients.resize(maxIndex + 1);
        }
    } else if (faceplus_teardown()) {
        qDebug() << "Faceplus torn down.";
        _active = false;
    }
#endif
}