#include <iostream>

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/calib3d.hpp>

#include <emscripten/bind.h>
#include <emscripten/emscripten.h>

#include <stdexcept>
#include <cmath>
#include <ctime>

// -----------------------------------------------------------------
// Utilities

void randSeed(void)
{
    srand(time(0));
}

double
unifRand(void)
{
    return rand() / double(RAND_MAX);
}

typedef double TimeStamp; // in seconds
static const TimeStamp UndefinedTime = -1.0;

// -----------------------------------------------------------------

class LowPassFilter
{

    double y, a, s;
    bool initialized;

    void setAlpha(double alpha)
    {
        if (alpha <= 0.0 || alpha > 1.0)
            throw std::range_error("alpha should be in (0.0., 1.0]");
        a = alpha;
    }

public:
    LowPassFilter(double alpha, double initval = 0.0)
    {
        y = s = initval;
        setAlpha(alpha);
        initialized = false;
    }

    double filter(double value)
    {
        double result;
        if (initialized)
            result = a * value + (1.0 - a) * s;
        else
        {
            result = value;
            initialized = true;
        }
        y = value;
        s = result;
        return result;
    }

    double filterWithAlpha(double value, double alpha)
    {
        setAlpha(alpha);
        return filter(value);
    }

    bool hasLastRawValue(void)
    {
        return initialized;
    }

    double lastRawValue(void)
    {
        return y;
    }
};

// -----------------------------------------------------------------

class OneEuroFilter
{

    double freq;
    double mincutoff;
    double beta_;
    double dcutoff;
    LowPassFilter *x;
    LowPassFilter *dx;
    TimeStamp lasttime;

    double alpha(double cutoff)
    {
        double te = 1.0 / freq;
        double tau = 1.0 / (2 * M_PI * cutoff);
        return 1.0 / (1.0 + tau / te);
    }

    void setFrequency(double f)
    {
        if (f <= 0)
            throw std::range_error("freq should be >0");
        freq = f;
    }

    void setMinCutoff(double mc)
    {
        if (mc <= 0)
            throw std::range_error("mincutoff should be >0");
        mincutoff = mc;
    }

    void setBeta(double b)
    {
        beta_ = b;
    }

    void setDerivateCutoff(double dc)
    {
        if (dc <= 0)
            throw std::range_error("dcutoff should be >0");
        dcutoff = dc;
    }

public:
    OneEuroFilter(double freq,
                  double mincutoff = 1.0, double beta_ = 0.0, double dcutoff = 1.0)
    {
        setFrequency(freq);
        setMinCutoff(mincutoff);
        setBeta(beta_);
        setDerivateCutoff(dcutoff);
        x = new LowPassFilter(alpha(mincutoff));
        dx = new LowPassFilter(alpha(dcutoff));
        lasttime = UndefinedTime;
    }

    double filter(double value, TimeStamp timestamp = UndefinedTime)
    {
        // update the sampling frequency based on timestamps
        if (lasttime != UndefinedTime && timestamp != UndefinedTime)
            freq = 1.0 / (timestamp - lasttime);
        lasttime = timestamp;
        // estimate the current variation per second
        double dvalue = x->hasLastRawValue() ? (value - x->lastRawValue()) * freq : 0.0; // FIXME: 0.0 or value?
        double edvalue = dx->filterWithAlpha(dvalue, alpha(dcutoff));
        // use it to update the cutoff frequency
        double cutoff = mincutoff + beta_ * fabs(edvalue);
        // filter the given value
        return x->filterWithAlpha(value, alpha(cutoff));
    }

    ~OneEuroFilter(void)
    {
        delete x;
        delete dx;
    }
};
using namespace emscripten;
using namespace cv;

struct Pt2f
{
    float x;
    float y;
};

class MTR_FACE
{
protected:
    int img_width;
    int img_height;
    int v_width;
    int v_height;

    int *js_data;
    std::vector<unsigned char> local_buffer;

    Mat local_data;
    Mat cameraMatrix;

    std::vector<double> final_vec;

    std::vector<cv::Point3d> object_pts;
    // std::vector<cv::Point3d> reprojectsrc;
    std::vector<cv::Point2d> image_pts;

    // result
    cv::Mat rotation_vec; // 3 x 1
    // cv::Mat rotation_mat;                           //3 x 3 R
    cv::Mat translation_vec; // 3 x 1 T
    // cv::Mat pose_mat;
    // cv::Mat euler_angle;

public:
    MTR_FACE(int img_width, int img_height) : img_width(img_width), img_height(img_height)
    {
        cameraMatrix = (Mat_<double>(3, 3) << img_width, 0, img_width / 2, 0, img_width, img_height / 2, 0, 0, 1);

        /*
        UL_________UR
        |__o__|__o__|
        |____CN_____|        <----- face
        BL___CC____BR
        */

        object_pts.push_back(cv::Point3d(0, 0, 0));        // center nose
        object_pts.push_back(cv::Point3d(120, -110, 90));  // left eye left corner
        object_pts.push_back(cv::Point3d(-120, -110, 90)); // right eye right corner
        object_pts.push_back(cv::Point3d(-60, 60, 70));    // mouth right
        object_pts.push_back(cv::Point3d(60, 60, 70));     // mouth left
        object_pts.push_back(cv::Point3d(0, 150, 50));     // center chin

        std::cout << "More Than Real Face Tracker - v0.2: copyright STEM Tecnologia e Des. de Software LTDA. This software is property of More Than Real - www.morethanreal.io" << std::endl;
    }

    val processPointsFloat2(Pt2f CN, Pt2f UL, Pt2f UR, Pt2f BR, Pt2f BL, Pt2f CC);

    double freq = 5.0;
    double mincutoff = 0.8;
    double beta_x = 0.01;
    double dcutoff = 0.5;

    OneEuroFilter f1x = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f1y = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f2x = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f2y = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f3x = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f3y = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f4x = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f4y = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f5x = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f5y = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f6x = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f6y = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f7x = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f7y = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f8x = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
    OneEuroFilter f8y = OneEuroFilter(freq, mincutoff, beta_x, dcutoff);
};

val MTR_FACE::processPointsFloat2(Pt2f CN, Pt2f UL, Pt2f UR, Pt2f BR, Pt2f BL, Pt2f CC)
{
    image_pts.clear();
    image_pts.push_back(cv::Point2d(CN.x, CN.y));
    image_pts.push_back(cv::Point2d(UL.x, UL.y));
    image_pts.push_back(cv::Point2d(UR.x, UR.y));
    image_pts.push_back(cv::Point2d(BR.x, BR.y));
    image_pts.push_back(cv::Point2d(BL.x, BL.y));
    image_pts.push_back(cv::Point2d(CC.x, CC.y));
    // std::cout << image_pts[0] << std::endl;

    // calc pose
    solvePnP(object_pts, image_pts, cameraMatrix, Mat(), rotation_vec, translation_vec, false, SOLVEPNP_SQPNP);
    // std::cout << translation_vec << std::endl;

    // SEND DATA
    final_vec.clear();

    // rvec
    Mat R;
    Rodrigues(rotation_vec, R);

    for (size_t i = 0; i < 3; i++)
    {
        final_vec.push_back(R.at<double>(i, 0));
        final_vec.push_back(R.at<double>(i, 1));
        final_vec.push_back(R.at<double>(i, 2));
    }

    // tvec
    for (size_t i = 0; i < 3; i++)
    {
        final_vec.push_back(translation_vec.at<double>(i));
    }

    // return final_vec;
    return val(typed_memory_view(final_vec.size(), (double *)final_vec.data()));
}

EMSCRIPTEN_BINDINGS(mtr_xr_face)
{
    class_<Mat>("Mat");
    value_array<Pt2f>("Pt2f")
        .element(&Pt2f::x)
        .element(&Pt2f::y);
    class_<MTR_FACE>("MTR_FACE")
        .constructor<int, int>()
        .function("processPointsFloat2", &MTR_FACE::processPointsFloat2);
}