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

#pragma once

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include "../my_types.h"

#include <graphene/geometry/Blend_skinning_matrices.h>
#include <graphene/geometry/bary_coord.h>

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

namespace graphene {
namespace surface_mesh {
namespace energy_matrix {

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

// Dense PCA Matrix
static
void pca_matrix(const PCA& pca, Eigen::MatrixXd& P);

// Dense PCA Mean Vector
static
void pca_mean(const PCA& pca, Eigen::VectorXd& mu);

// Skinning matrices for each vertex, decomposed into rotation and translation
static
void skinning(const graphene::character::Character& character,
              const Surface_mesh& mesh,
              const std::vector<int>& mapping,
              SpMat& R, Eigen::VectorXd& t);

// Sparse matrix with barycentric coordinates
static
void barycentric_interpolation(const std::vector<Correspondence>& correspondences,
                               const Surface_mesh& mesh,
                               const std::vector<int>& mapping,
                               SpMat& B);

// Sparse matrix for selecting landmarks
static
void selection(std::vector<Surface_mesh::Vertex>& vertices,
               size_t n_vertices,
               const std::vector<int>& mapping,
               SpMat& S);

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

static void pca_matrix(const PCA& pca, Eigen::MatrixXd& P)
{
    P = pca.P_pca_;
}

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

static void pca_mean(const PCA& pca, Eigen::VectorXd& mu)
{
    mu = pca.m_pca_;
}

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

static void skinning(const graphene::character::Character& character,
                     const Surface_mesh& mesh,
                     const std::vector<int>& mapping,
                     SpMat& R, Eigen::VectorXd& t)
{
    auto& skeleton = character.skeleton();

    size_t n_vertices = mesh.n_vertices();

    bool with_mapping = mapping.size() > 0;
    size_t num_free_vertices = 0;
    if (with_mapping)
    {
        for (auto v : mesh.vertices())
            if (mapping[v.idx()] != -1)
                num_free_vertices++;

        n_vertices = num_free_vertices;
    }

    std::vector<Tripl> triplets;
    triplets.reserve(3 * 3 * n_vertices);

    auto vdepends1 = mesh.get_vertex_property<graphene::Vec4f>("v:skin_depend");
    auto vweights1 = mesh.get_vertex_property<graphene::Vec4f>("v:skin_weight");
    auto vdepends2 = mesh.get_vertex_property<graphene::Vec4f>("v:skin_depend2");
    auto vweights2 = mesh.get_vertex_property<graphene::Vec4f>("v:skin_weight2");

    auto skinning_matrices = skeleton.skinning_matrices_;

    t.resize(3 * n_vertices);

    size_t idx = 0;

    for (auto v : mesh.vertices())
    {
        if (with_mapping && mapping[v.idx()] == -1)
            continue;

        auto blend_mat = blend_matrices(vweights1[v], vweights2[v],
                                        vdepends1[v], vdepends2[v],
                                        skinning_matrices);

        size_t row = 3 * idx;
        size_t col = 3 * idx;

        for (int i = 0; i < 3; ++i)
        {
            for (int j = 0; j < 3; ++j)
            {
                double value = blend_mat(i, j);
                triplets.push_back(Tripl(row + i, col + j, value));
            }
        }

        t(3 * idx + 0) = blend_mat(0, 3);
        t(3 * idx + 1) = blend_mat(1, 3);
        t(3 * idx + 2) = blend_mat(2, 3);

        idx++;
    }

    R.resize(n_vertices * 3, n_vertices * 3);
    R.setFromTriplets(triplets.begin(), triplets.end());
}

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

static void
barycentric_interpolation(const std::vector<Correspondence>& correspondences,
                          const std::vector<int>& mapping,
                          const Surface_mesh& mesh,
                          SpMat& B)
{
    size_t n_vertices = mesh.n_vertices();
    size_t n_correspondences = correspondences.size();

    std::vector<Tripl> triplets;

    // x,y,z * alpha,beta,gamma * n_correspondences
    triplets.reserve(3 * 3 * n_correspondences);

    bool with_mapping = mapping.size() != 0;

    size_t r = 0;
    for (size_t i = 0; i < n_correspondences; ++i)
    {
        const auto& c = correspondences[i];

if (c.constr_dir == corresp_dir_ps2mesh)
        {
            // get vertices of closest triangle
            auto fvit = mesh.vertices(c.face);

            const auto v0 = *fvit;
            const auto v1 = *(++fvit);
            const auto v2 = *(++fvit);

            size_t idx0 = v0.idx();
            size_t idx1 = v1.idx();
            size_t idx2 = v2.idx();

            if (with_mapping)
            {
                if (mapping[idx0] == -1)
                {
                    printf("[ERROR] Unmapped vertex %lu\n", idx0);
                    exit(-1);
                }
                if (mapping[idx1] == -1)
                {
                    printf("[ERROR] Unmapped vertex %lu\n", idx1);
                    exit(-1);
                }
                if (mapping[idx2] == -1)
                {
                    printf("[ERROR] Unmapped vertex %lu\n", idx2);
                    exit(-1);
                }

                idx0 = mapping[idx0];
                idx1 = mapping[idx1];
                idx2 = mapping[idx2];
            }


            const Point bary = c.on_template_bc;

            // x coordinate
            triplets.push_back(Tripl(r, 3 * idx0 + 0, bary[0]));
            triplets.push_back(Tripl(r, 3 * idx1 + 0, bary[1]));
            triplets.push_back(Tripl(r, 3 * idx2 + 0, bary[2]));
            r++;

            // y coordinate
            triplets.push_back(Tripl(r, 3 * idx0 + 1, bary[0]));
            triplets.push_back(Tripl(r, 3 * idx1 + 1, bary[1]));
            triplets.push_back(Tripl(r, 3 * idx2 + 1, bary[2]));
            r++;

            // z coordinate
            triplets.push_back(Tripl(r, 3 * idx0 + 2, bary[0]));
            triplets.push_back(Tripl(r, 3 * idx1 + 2, bary[1]));
            triplets.push_back(Tripl(r, 3 * idx2 + 2, bary[2]));
            r++;
        }
        else // corresp_dir_mesh2ps
        {
            size_t idx = c.on_template_vh.idx();

            if (with_mapping)
            {
                if (mapping[idx] == -1)
                {
                    printf("[ERROR] Unmapped vertex %lu\n", idx);
                    exit(-1);
                }

                idx = mapping[idx];
            }

            // x, y, z coordinates
            triplets.push_back(Tripl(r, 3 * idx + 0, 1.0f)); r++;
            triplets.push_back(Tripl(r, 3 * idx + 1, 1.0f)); r++;
            triplets.push_back(Tripl(r, 3 * idx + 2, 1.0f)); r++;
        }
    }

    B.resize(3 * n_correspondences, 3 * n_vertices);
    B.setFromTriplets(triplets.begin(), triplets.end());
}

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

static void selection(std::vector<Surface_mesh::Vertex>& vertices,
                      size_t n_vertices,
                      const std::vector<int>& mapping,
                      SpMat& S)
{
    size_t n_landmarks = vertices.size();
    S.resize(3 * n_landmarks, 3 * n_vertices);

    bool with_mapping = mapping.size() != 0;

    size_t r = 0;
    for (auto v : vertices)
    {
        size_t idx = v.idx();

        if (with_mapping)
        {
            if (mapping[idx] == -1)
            {
                printf("[ERROR] Specifying landmark for unmapped vertex %lu\n",
                       idx);
                exit(-1);
            }

            idx = mapping[idx];
        }

        S.insert(r, 3 * idx + 0) = 1; r++;
        S.insert(r, 3 * idx + 1) = 1; r++;
        S.insert(r, 3 * idx + 2) = 1; r++;
    }

    S.makeCompressed();
}

}
}
}

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