//== INCLUDES ===================================================================


#include "Eyes_correction_helper.h"

#include <graphene/surface_mesh/algorithms/surface_mesh_tools/misc.h>
#include <graphene/geometry/registration.h>
#include <graphene/surface_mesh/algorithms/templatefitting/utility/my_helper.h>


namespace graphene {
namespace eyes_correction_helper {

using namespace surface_mesh;

bool correct_eye_joints(character::Skeleton* skeleton,
                        Surface_mesh* base_mesh,
                        Mat4f M_left, Mat4f M_right,
                        const std::string& s_eyejoint_left,
                        const std::string& s_eyejoint_right)
{
    auto undef_leye_pos = base_mesh->get_mesh_property<Vec3f>("m:undef_leye_pos");
    auto undef_reye_pos = base_mesh->get_mesh_property<Vec3f>("m:undef_reye_pos");

    if (!undef_leye_pos || !undef_reye_pos)
    {
        std::cerr << "correct_eyes: [ERROR] Cannot find undeformed eye joint positions." << std::endl;
        return false;
    }

    const Vec3f undeformed_left_eye_joint_pos  = undef_leye_pos[0];
    const Vec3f undeformed_right_eye_joint_pos = undef_reye_pos[0];

    const Vec3f new_lefteye_joint_pos  = affine_transform(M_left, undeformed_left_eye_joint_pos);
    const Vec3f new_righteye_joint_pos = affine_transform(M_right, undeformed_right_eye_joint_pos);

    character::Joint* left_eye_joint  = skeleton->get_joint(s_eyejoint_left.c_str());
    character::Joint* right_eye_joint = skeleton->get_joint(s_eyejoint_right.c_str());
    if (!left_eye_joint || !right_eye_joint)
    {
        std::cerr << "correct_eyes: [ERROR] Cannot find eye joints." << std::endl;
        return false;
    }

    character::Joint* left_eye_joint_parent  = left_eye_joint->parent_;
    character::Joint* right_eye_joint_parent = right_eye_joint->parent_;

    Vec3f offset_r = new_righteye_joint_pos - right_eye_joint_parent->get_global_translation();
    Vec3f offset_l = new_lefteye_joint_pos  - left_eye_joint_parent->get_global_translation();

    offset_r = (Mat3f(right_eye_joint_parent->global_inv_)) * offset_r;
    offset_l = (Mat3f(left_eye_joint_parent->global_inv_)) * offset_l;

    //left_eye_joint->local_ = Mat4f::identity();
    left_eye_joint->local_[12] = offset_l[0];
    left_eye_joint->local_[13] = offset_l[1];
    left_eye_joint->local_[14] = offset_l[2];
    left_eye_joint->bind_pose_local_ = left_eye_joint->local_;

    //right_eye_joint->local_ = Mat4f::identity();
    right_eye_joint->local_[12] = offset_r[0];
    right_eye_joint->local_[13] = offset_r[1];
    right_eye_joint->local_[14] = offset_r[2];
    right_eye_joint->bind_pose_local_ = right_eye_joint->local_;

    skeleton->init();

    return true;
}

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

bool correct_eyes(character::Character &inout_character, const Eye_info &eye_info)
{
    // prepare character ------------------------------------------------------

    graphene::character::Skeleton& skeleton = inout_character.skeleton();

    // prepare skins ------------------------------------------------------

    const std::vector<graphene::character::Surface_mesh_skin*>& skins = inout_character.skins();

    graphene::character::Surface_mesh_skin
            *base_mesh=0,
            *eyemesh_left=0,
            *eyemesh_right=0,
            *eyetrans_left=0,
            *eyetrans_right=0,
            *eyegland_left=0,
            *eyegland_right=0
            ;

    for (size_t i = 0; i < skins.size(); ++i)
    {
        auto skin_id = skins[i]->mesh_property<std::string>("m:id");

        if ( skin_id[0] == eye_info.s_bodymesh )
        {
            base_mesh = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyemesh_left )
        {
            eyemesh_left = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyemesh_right )
        {
            eyemesh_right = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyetrans_left )
        {
            eyetrans_left = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyetrans_right )
        {
            eyetrans_right = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyegland_left )
        {
            eyegland_left = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyegland_right )
        {
            eyegland_right = skins[i];
        }
    }

    if (!base_mesh)
    {
        std::cerr << "correct_eyes: [ERROR] Cannot correct eyes. No base mesh found!" << std::endl;
        return false;
    }



    // correct eyes --------------------------------------------------------

    std::vector<unsigned int> selection_left_vec;
    if (!graphene::surface_mesh::load_selection_from_file(selection_left_vec, eye_info.fn_sel_eyeregion_left))
    {
        std::cerr << "correct_eyes: [ERROR] Can't open selection (left eye). Filename: \"" << eye_info.fn_sel_eyeregion_left << "\"." << std::endl;
        return false;
    }
    std::vector<unsigned int> selection_right_vec;
    if (!graphene::surface_mesh::load_selection_from_file(selection_right_vec, eye_info.fn_sel_eyeregion_right))
    {
        std::cerr << "correct_eyes: [ERROR] Can't open selection (right eye). Filename: \"" << eye_info.fn_sel_eyeregion_right << "\"." << std::endl;
        return false;
    }

    Surface_mesh::Vertex_property<Point> undef_points = base_mesh->get_vertex_property<Point>("v:undef_point");
    if (!undef_points)
    {
        std::cerr << "correct_eyes: [ERROR] No undeformed mesh vertices found." << std::endl;
        return false;
    }

    std::vector<Point> src_points_left;
    std::vector<Point> tar_points_left;
    std::vector<Point> src_points_right;
    std::vector<Point> tar_points_right;
    for ( unsigned int i = 0; i < selection_left_vec.size(); ++i)
    {
        Surface_mesh::Vertex v(selection_left_vec[i]);

        src_points_left.push_back(undef_points[v]);
        tar_points_left.push_back(base_mesh->position(v));
    }
    for ( unsigned int i = 0; i < selection_right_vec.size(); ++i)
    {
        Surface_mesh::Vertex v(selection_right_vec[i]);

        src_points_right.push_back(undef_points[v]);
        tar_points_right.push_back(base_mesh->position(v));

        // src_points_both.push_back(mesh_source_undeformed.position(v));
        // tar_points_both.push_back(mesh_target_undeformed.position(v));
    }


    // similarity transformation ----------------------------------------------------------------

    //get transformation in order to transform both eyes according to deformation of template

    // old version:
    //ATTENTION: this performs a minimal depth correction. If you find a more versatile solution for this, implement it!
//    const Mat4f M_registration_eyes_left  = Mat4f::translate(Vec3f(0.0f,0.0f,-0.0005f)) * graphene::geometry::registration(src_points_left, tar_points_left, graphene::geometry::CONFORMAL_REGISTRATION, NULL, false); // NULL means we are not interested in a relative change; false means we also optimize rotation; note: we also optimize translation and scaling

//    const Mat4f M_registration_eyes_right = Mat4f::translate(Vec3f(0.0f,0.0f,-0.0005f)) * graphene::geometry::registration(src_points_right, tar_points_right, graphene::geometry::CONFORMAL_REGISTRATION, NULL, false); // NULL means we are not interested in a relative change; false means we also optimize rotation; note: we also optimize translation and scaling

    // new version:
    Mat4f M_registration_eyes_left;
    Mat4f M_registration_eyes_right;
    const bool ok_register_both_eyes = register_both_eyes(M_registration_eyes_left, M_registration_eyes_right, src_points_left, tar_points_left, src_points_right, tar_points_right);
    if (!ok_register_both_eyes)
    {
        std::cerr << "correct_eyes: [ERROR] Could not register both eyes." << std::endl;
        return false;
    }

    //transform meshes

    if (eyemesh_left != nullptr && eyemesh_right != nullptr)
    {
        transform_mesh(*eyemesh_left , M_registration_eyes_left);
        eyemesh_left->copy_unskinned_properties();
        transform_mesh(*eyemesh_right, M_registration_eyes_right);
        eyemesh_right->copy_unskinned_properties();
    }
    else
    {
        std::cerr << "correct_eyes: [ERROR] Could not correct eye balls. Eye ball meshes not found!" << std::endl;
    }

    if (eyegland_left != nullptr && eyegland_right != nullptr)
    {
        transform_mesh(*eyegland_left , M_registration_eyes_left);
        eyegland_left->copy_unskinned_properties();
        transform_mesh(*eyegland_right, M_registration_eyes_right);
        eyegland_right->copy_unskinned_properties();
    }
    else
    {
        std::cerr << "correct_eyes: [ERROR] Could not correct eye glands. Gland meshes not found!" << std::endl;
    }

    if (eyetrans_left != nullptr && eyetrans_right != nullptr)
    {
        transform_mesh(*eyetrans_left , M_registration_eyes_left);
        eyetrans_left->copy_unskinned_properties();
        transform_mesh(*eyetrans_right, M_registration_eyes_right);
        eyetrans_right->copy_unskinned_properties();
    }
    else
    {
        std::cerr << "correct_eyes: [ERROR] Could not correct transparent eye sphere. Eye sphere meshes not found!" << std::endl;
    }

    //transform eye joint positions
    if (!correct_eye_joints(&skeleton, base_mesh,
                            M_registration_eyes_left,
                            M_registration_eyes_right,
                            eye_info.s_eyejoint_left,
                            eye_info.s_eyejoint_right))
    {
        return false;
    }


    return true;
}

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

bool correct_eyes(character::Character& inout_character,
                  const Combined_eyes_info& eye_info)
{
    using graphene::surface_mesh::load_selection_from_file;
    using graphene::geometry::registration;

    //
    // Prepare data
    //
    auto& skeleton = inout_character.skeleton();
    const auto& skins = inout_character.skins();

    graphene::character::Surface_mesh_skin* body_mesh = nullptr;
    graphene::character::Surface_mesh_skin* eye_mesh = nullptr;
    graphene::character::Surface_mesh_skin* eyetrans_mesh = nullptr;

    for (size_t i = 0; i < skins.size(); ++i)
    {
        auto skin_id = skins[i]->mesh_property<std::string>("m:id");

        if ( skin_id[0] == eye_info.s_bodymesh )
        {
            body_mesh = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyemesh)
        {
            eye_mesh = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyetransmesh)
        {
            eyetrans_mesh = skins[i];
        }
    }

    if (!body_mesh || !eye_mesh)
    {
        std::cerr << "correct_eyes: [ERROR] Cannot correct eyes. Base or eye mesh not found!" << std::endl;
        return false;
    }

    if (!eyetrans_mesh)
    {
        std::cerr << "correct_eyes: [WARNING] No trans mesh found! Only correcting regular eye mesh" << std::endl;
    }

    std::vector<unsigned int> body_mesh_left_eyeregion;
    std::vector<unsigned int> body_mesh_right_eyeregion;
    std::vector<unsigned int> eye_mesh_left_vertices;
    std::vector<unsigned int> eye_mesh_right_vertices;

    if (!load_selection_from_file(body_mesh_left_eyeregion,
                                  eye_info.fn_sel_body_mesh_eyeregion_left))
    {
        std::cerr << "correct_eyes: [ERROR] Can't open selection (left eye). Filename: \""
                  << eye_info.fn_sel_body_mesh_eyeregion_left << "\"." << std::endl;
        return false;
    }

    if (!load_selection_from_file(body_mesh_right_eyeregion,
                                  eye_info.fn_sel_body_mesh_eyeregion_right))
    {
        std::cerr << "correct_eyes: [ERROR] Can't open selection (right eye). Filename: \""
                  << eye_info.fn_sel_body_mesh_eyeregion_right << "\"." << std::endl;
        return false;
    }

    if (!load_selection_from_file(eye_mesh_left_vertices,
                                  eye_info.fn_sel_eye_mesh_vertices_left))
    {
        std::cerr << "correct_eyes: [ERROR] Can't open selection (left eye). Filename: \""
                  << eye_info.fn_sel_eye_mesh_vertices_left << "\"." << std::endl;
        return false;
    }

    if (!load_selection_from_file(eye_mesh_right_vertices,
                                  eye_info.fn_sel_eye_mesh_vertices_right))
    {
        std::cerr << "correct_eyes: [ERROR] Can't open selection (right eye). Filename: \""
                  << eye_info.fn_sel_eye_mesh_vertices_right << "\"." << std::endl;
        return false;
    }

    auto undef_points = body_mesh->get_vertex_property<Point>("v:undef_point");
    if (!undef_points)
    {
        std::cerr << "correct_eyes: [ERROR] No undeformed mesh vertices found." << std::endl;
        return false;
    }

    //
    // Compute transform from undeformed to deformed eyeregions
    //
    std::vector<Point> src_points_left;
    std::vector<Point> tar_points_left;
    std::vector<Point> src_points_right;
    std::vector<Point> tar_points_right;

    for ( unsigned int i = 0; i < body_mesh_left_eyeregion.size(); ++i)
    {
        Surface_mesh::Vertex v(body_mesh_left_eyeregion[i]);

        src_points_left.push_back(undef_points[v]);
        tar_points_left.push_back(body_mesh->position(v));
    }

    for ( unsigned int i = 0; i < body_mesh_right_eyeregion.size(); ++i)
    {
        Surface_mesh::Vertex v(body_mesh_right_eyeregion[i]);

        src_points_right.push_back(undef_points[v]);
        tar_points_right.push_back(body_mesh->position(v));
    }

    Mat4f M_left; // =
        //registration(src_points_left, tar_points_left,
        //             graphene::geometry::CONFORMAL_REGISTRATION,
        //             nullptr, true);

    Mat4f M_right; // =
        //registration(src_points_right, tar_points_right,
        //             graphene::geometry::CONFORMAL_REGISTRATION,
        //             nullptr, true);

    register_both_eyes(M_left, M_right, src_points_left, tar_points_left,
                       src_points_right, tar_points_right);

    auto eye_points = eye_mesh->vertex_property<Point>("v:point");

    // Left eye
    for (unsigned int i = 0; i < eye_mesh_left_vertices.size(); ++i)
    {
        Surface_mesh::Vertex v(eye_mesh_left_vertices[i]);

        Vec3f& p = eye_points[v];
        p = affine_transform(M_left, p);

        if (eyetrans_mesh)
        {
            Vec3f& p2 = eyetrans_mesh->position(v);
            p2 = affine_transform(M_left, p2);
        }
    }

    // Right eye
    for (unsigned int i = 0; i < eye_mesh_right_vertices.size(); ++i)
    {
        Surface_mesh::Vertex v(eye_mesh_right_vertices[i]);

        Vec3f& p = eye_points[v];
        p = affine_transform(M_right, p);

        if (eyetrans_mesh)
        {
            Vec3f& p2 = eyetrans_mesh->position(v);
            p2 = affine_transform(M_right, p2);
        }
    }

    // Update blendshape targets
    for (auto* blendshape_set : inout_character.blendshapes())
    {
        if (blendshape_set->base() == eye_mesh ||
            blendshape_set->base() == eyetrans_mesh)
        {
            for (auto* target : blendshape_set->targets())
            {
                transform_mesh(*target, eye_mesh_left_vertices, M_left);
                transform_mesh(*target, eye_mesh_right_vertices, M_right);
            }
        }
    }

    eye_mesh->update_vertex_normals();
    eye_mesh->copy_unskinned_properties();

    if (eyetrans_mesh)
    {
        eyetrans_mesh->update_vertex_normals();
        eyetrans_mesh->copy_unskinned_properties();
    }

    if (!correct_eye_joints(&skeleton, body_mesh,
                            M_left, M_right,
                            eye_info.s_eyejoint_left,
                            eye_info.s_eyejoint_right))
    {
        return false;
    }

    return true;
}

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

bool correct_eyelids(character::Character& inout_character,
                     const Eye_info& eye_info,
                     const std::string& fn_sel_eyecontour_left,
                     const std::string& fn_sel_eyecontour_right)
{
    // prepare skins ------------------------------------------------------

    const std::vector<graphene::character::Surface_mesh_skin*>& skins = inout_character.skins();

    graphene::character::Surface_mesh_skin
            *base_mesh=0,
            *eyetrans_left=0,
            *eyetrans_right=0
            ;

    for (size_t i = 0; i < skins.size(); ++i)
    {
        auto skin_id = skins[i]->mesh_property<std::string>("m:id");

        if ( skin_id[0] == eye_info.s_bodymesh )
        {
            base_mesh = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyetrans_left )
        {
            eyetrans_left = skins[i];
        }
        else if ( skin_id[0] == eye_info.s_eyetrans_right )
        {
            eyetrans_right = skins[i];
        }
    }

    if (!base_mesh || !eyetrans_left || !eyetrans_right)
    {
        std::cerr << "correct_eyelids: [ERROR] Cannot correct eyelids. No base mesh or eyes found!" << std::endl;
        return false;
    }

    //load template contour selections
    //right eye
    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > contour_righteye
        = base_mesh->mesh_property< std::vector<Surface_mesh::Vertex> >("m:contour_righteye");
    std::vector<Surface_mesh::Vertex>& contour_points_right_eye = contour_righteye[0];
    contour_points_right_eye.clear();
    //left eye
    Surface_mesh::Mesh_property< std::vector<Surface_mesh::Vertex> > contour_lefteye
        = base_mesh->mesh_property< std::vector<Surface_mesh::Vertex> >("m:contour_lefteye");
    std::vector<Surface_mesh::Vertex>& contour_points_left_eye = contour_lefteye[0];
    contour_points_left_eye.clear();
    //load both selections
    if (
            ! load_selection_from_file(contour_points_left_eye, fn_sel_eyecontour_left)
            ||
            ! load_selection_from_file(contour_points_right_eye, fn_sel_eyecontour_right)
            )
    {
        std::cerr << "eyes_correction_helper::correct_eyelids: [ERROR] Unable to correct eyelids." << std::endl;
        return false;
    }

    Surface_mesh::Mesh_property< std::vector<Point> > contour_targets_lefteye_prop
        = base_mesh->mesh_property< std::vector<Point> >("m:contour_targets_lefteye");
    Surface_mesh::Mesh_property< std::vector<Point> > contour_targets_righteye_prop
        = base_mesh->mesh_property< std::vector<Point> >("m:contour_targets_righteye");
    std::vector<Point>& contour_targets_lefteye  = contour_targets_lefteye_prop[0];
    std::vector<Point>& contour_targets_righteye = contour_targets_righteye_prop[0];
    contour_targets_lefteye.clear();
    contour_targets_righteye.clear();

    // kd tree of eye mesh
    const graphene::surface_mesh::Triangle_kD_tree mesh_eyetrans_left_kd( *eyetrans_left );
    const graphene::surface_mesh::Triangle_kD_tree mesh_eyetrans_right_kd( *eyetrans_right );

    // right eye
    for (auto v : contour_points_right_eye)
    {
        // find closest point
        const graphene::Point& point = base_mesh->position(v);
        const auto nn_tri = mesh_eyetrans_right_kd.nearest( point );
        const graphene::Point x_nearest = nn_tri.nearest;
        contour_targets_righteye.push_back(x_nearest + Point(0,0,0.0005));
    }

    // left eye
    for (auto v : contour_points_left_eye)
    {
        // find closest point
        const graphene::Point& point = base_mesh->position(v);
        const auto nn_tri = mesh_eyetrans_left_kd.nearest( point );
        const graphene::Point x_nearest = nn_tri.nearest;
        contour_targets_lefteye.push_back(x_nearest + Point(0,0,0.0005));
    }

    return true;
}

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

bool register_both_eyes(Mat4f& M_registration_eyes_left,
                        Mat4f& M_registration_eyes_right,
                        const std::vector<Point>& src_left,
                        const std::vector<Point>& dst_left,
                        const std::vector<Point>& src_right,
                        const std::vector<Point>& dst_right)
{
    M_registration_eyes_left = Mat4f::identity();
    M_registration_eyes_right = Mat4f::identity();

    const int n_left  = src_left.size();
    const int n_right = src_right.size();

    // compute barycenters
    Vector<Scalar,3> scog_left(0.0,0.0,0.0), dcog_left(0.0,0.0,0.0);
    for (int i=0; i<n_left; ++i)
    {
        scog_left += src_left[i];
        dcog_left += dst_left[i];
    }
    scog_left /= (Scalar) n_left;
    dcog_left /= (Scalar) n_left;

    Vector<Scalar,3> scog_right(0.0,0.0,0.0), dcog_right(0.0,0.0,0.0);
    for (int i=0; i<n_right; ++i)
    {
        scog_right += src_right[i];
        dcog_right += dst_right[i];
    }
    scog_right /= (Scalar) n_right;
    dcog_right /= (Scalar) n_right;

    const bool use_rotation = true;
    if ( use_rotation )
    {
        // rotation left eye
        {
            // build matrix
            Mat4d M;
            {
                double  xx(0.0), xy(0.0), xz(0.0), yx(0.0), yy(0.0), yz(0.0), zx(0.0), zy(0.0), zz(0.0);
                Vector<Scalar,3>   sp, dp;

                for (int i=0; i<n_left; ++i)
                {
                    sp = src_left[i]; sp -= scog_left;
                    dp = dst_left[i]; dp -= dcog_left;
                    xx += sp[0] * dp[0];
                    xy += sp[0] * dp[1];
                    xz += sp[0] * dp[2];
                    yx += sp[1] * dp[0];
                    yy += sp[1] * dp[1];
                    yz += sp[1] * dp[2];
                    zx += sp[2] * dp[0];
                    zy += sp[2] * dp[1];
                    zz += sp[2] * dp[2];
                }

                M(0,0) =  xx + yy + zz;
                M(1,1) =  xx - yy - zz;
                M(2,2) = -xx + yy - zz;
                M(3,3) = -xx - yy + zz;
                M(1,0) = M(0,1) = yz - zy;
                M(2,0) = M(0,2) = zx - xz;
                M(2,1) = M(1,2) = xy + yx;
                M(3,0) = M(0,3) = xy - yx;
                M(3,1) = M(1,3) = zx + xz;
                M(3,2) = M(2,3) = yz + zy;
            }


            // symmetric eigendecomposition
            Mat4d   V = Mat4d::identity();
            unsigned int iter(50);
            {
                int     i, j, k;
                double  theta, t, c, s, ss, g, h, tau, tM;

                while (--iter)
                {
                    // find largest off-diagonal element
                    i=0; j=1; ss=fabs(M(0,1));
                    if ( (s=fabs(M(0,2))) > ss) { ss=s; i=0; j=2; }
                    if ( (s=fabs(M(0,3))) > ss) { ss=s; i=0; j=3; }
                    if ( (s=fabs(M(1,2))) > ss) { ss=s; i=1; j=2; }
                    if ( (s=fabs(M(1,3))) > ss) { ss=s; i=1; j=3; }
                    if ( (s=fabs(M(2,3))) > ss) { ss=s; i=2; j=3; }

                    // converged?
                    if (ss < 1e-10) break;

                    // compute Jacobi rotation
                    theta = 0.5 * (M(j,j) - M(i,i)) / M(i,j);
                    t     = (theta<0.0 ? -1.0 : 1.0) / (fabs(theta) + sqrt(1.0+theta*theta));
                    c     = 1.0 / sqrt(1.0 + t*t);
                    s     = t*c;
                    tau   = s/(1.0+c);
                    tM    = t*M(i,j);

#define rot(a, s, t, i, j, k, l)                                        \
                    { g=a(i,j); h=a(k,l); a(i,j)=g-s*(h+g*t); a(k,l)=h+s*(g-h*t); }

                    M(i,j)  = 0.0;
                    for (k=  0; k<i; ++k)  rot(M, s, tau, k, i, k, j);
                    for (k=i+1; k<j; ++k)  rot(M, s, tau, i, k, k, j);
                    for (k=j+1; k<4; ++k)  rot(M, s, tau, i, k, j, k);
                    for (k=  0; k<4; ++k)  rot(V, s, tau, k, i, k, j);
                    M(i,i) -= tM;
                    M(j,j) += tM;
                }
            }


            // did it work?
            if (!iter)
            {
                std::cerr << "registration: Jacobi did not converge\n";
                return false;
            }


            // eigenvector wrt largest eigenvalue -> quaternion
            Vec4d q;
            {
                int imax=0;
                double s, ss = M(imax,imax);
                if ( (s=M(1,1)) > ss) { ss=s; imax=1; }
                if ( (s=M(2,2)) > ss) { ss=s; imax=2; }
                if ( (s=M(3,3)) > ss) { ss=s; imax=3; }
                q = Vec4d( V(0,imax), V(1,imax), V(2,imax), V(3,imax) );
                q.normalize();
            }


            // rotation part
            Scalar
                ww(q[0]*q[0]), xx(q[1]*q[1]), yy(q[2]*q[2]), zz(q[3]*q[3]),
                wx(q[0]*q[1]), wy(q[0]*q[2]), wz(q[0]*q[3]),
                xy(q[1]*q[2]), xz(q[1]*q[3]), yz(q[2]*q[3]);
//        Mat4<Scalar> T;
            M_registration_eyes_left(0,0) = ww + xx - yy - zz;
            M_registration_eyes_left(1,0) = 2.0*(xy + wz);
            M_registration_eyes_left(2,0) = 2.0*(xz - wy);
            M_registration_eyes_left(3,0) = 0.0;
            M_registration_eyes_left(0,1) = 2.0*(xy - wz);
            M_registration_eyes_left(1,1) = ww - xx + yy - zz;
            M_registration_eyes_left(2,1) = 2.0*(yz + wx);
            M_registration_eyes_left(3,1) = 0.0;
            M_registration_eyes_left(0,2) = 2.0*(xz + wy);
            M_registration_eyes_left(1,2) = 2.0*(yz - wx);
            M_registration_eyes_left(2,2) = ww - xx - yy + zz;
            M_registration_eyes_left(3,2) = 0.0;
        }


        // rotation right eye
        {
            // build matrix
            Mat4d M;
            {
                double  xx(0.0), xy(0.0), xz(0.0), yx(0.0), yy(0.0), yz(0.0), zx(0.0), zy(0.0), zz(0.0);
                Vector<Scalar,3>   sp, dp;

                for (int i=0; i<n_right; ++i)
                {
                    sp = src_right[i]; sp -= scog_right;
                    dp = dst_right[i]; dp -= dcog_right;
                    xx += sp[0] * dp[0];
                    xy += sp[0] * dp[1];
                    xz += sp[0] * dp[2];
                    yx += sp[1] * dp[0];
                    yy += sp[1] * dp[1];
                    yz += sp[1] * dp[2];
                    zx += sp[2] * dp[0];
                    zy += sp[2] * dp[1];
                    zz += sp[2] * dp[2];
                }

                M(0,0) =  xx + yy + zz;
                M(1,1) =  xx - yy - zz;
                M(2,2) = -xx + yy - zz;
                M(3,3) = -xx - yy + zz;
                M(1,0) = M(0,1) = yz - zy;
                M(2,0) = M(0,2) = zx - xz;
                M(2,1) = M(1,2) = xy + yx;
                M(3,0) = M(0,3) = xy - yx;
                M(3,1) = M(1,3) = zx + xz;
                M(3,2) = M(2,3) = yz + zy;
            }


            // symmetric eigendecomposition
            Mat4d   V = Mat4d::identity();
            unsigned int iter(50);
            {
                int     i, j, k;
                double  theta, t, c, s, ss, g, h, tau, tM;

                while (--iter)
                {
                    // find largest off-diagonal element
                    i=0; j=1; ss=fabs(M(0,1));
                    if ( (s=fabs(M(0,2))) > ss) { ss=s; i=0; j=2; }
                    if ( (s=fabs(M(0,3))) > ss) { ss=s; i=0; j=3; }
                    if ( (s=fabs(M(1,2))) > ss) { ss=s; i=1; j=2; }
                    if ( (s=fabs(M(1,3))) > ss) { ss=s; i=1; j=3; }
                    if ( (s=fabs(M(2,3))) > ss) { ss=s; i=2; j=3; }

                    // converged?
                    if (ss < 1e-10) break;

                    // compute Jacobi rotation
                    theta = 0.5 * (M(j,j) - M(i,i)) / M(i,j);
                    t     = (theta<0.0 ? -1.0 : 1.0) / (fabs(theta) + sqrt(1.0+theta*theta));
                    c     = 1.0 / sqrt(1.0 + t*t);
                    s     = t*c;
                    tau   = s/(1.0+c);
                    tM    = t*M(i,j);

#define rot(a, s, t, i, j, k, l)                                        \
                    { g=a(i,j); h=a(k,l); a(i,j)=g-s*(h+g*t); a(k,l)=h+s*(g-h*t); }

                    M(i,j)  = 0.0;
                    for (k=  0; k<i; ++k)  rot(M, s, tau, k, i, k, j);
                    for (k=i+1; k<j; ++k)  rot(M, s, tau, i, k, k, j);
                    for (k=j+1; k<4; ++k)  rot(M, s, tau, i, k, j, k);
                    for (k=  0; k<4; ++k)  rot(V, s, tau, k, i, k, j);
                    M(i,i) -= tM;
                    M(j,j) += tM;
                }
            }


            // did it work?
            if (!iter)
            {
                std::cerr << "registration: Jacobi did not converge\n";
                return false;
            }


            // eigenvector wrt largest eigenvalue -> quaternion
            Vec4d q;
            {
                int imax=0;
                double s, ss = M(imax,imax);
                if ( (s=M(1,1)) > ss) { ss=s; imax=1; }
                if ( (s=M(2,2)) > ss) { ss=s; imax=2; }
                if ( (s=M(3,3)) > ss) { ss=s; imax=3; }
                q = Vec4d( V(0,imax), V(1,imax), V(2,imax), V(3,imax) );
                q.normalize();
            }


            // rotation part
            Scalar
                ww(q[0]*q[0]), xx(q[1]*q[1]), yy(q[2]*q[2]), zz(q[3]*q[3]),
                wx(q[0]*q[1]), wy(q[0]*q[2]), wz(q[0]*q[3]),
                xy(q[1]*q[2]), xz(q[1]*q[3]), yz(q[2]*q[3]);
//        Mat4<Scalar> T;
            M_registration_eyes_right(0,0) = ww + xx - yy - zz;
            M_registration_eyes_right(1,0) = 2.0*(xy + wz);
            M_registration_eyes_right(2,0) = 2.0*(xz - wy);
            M_registration_eyes_right(3,0) = 0.0;
            M_registration_eyes_right(0,1) = 2.0*(xy - wz);
            M_registration_eyes_right(1,1) = ww - xx + yy - zz;
            M_registration_eyes_right(2,1) = 2.0*(yz + wx);
            M_registration_eyes_right(3,1) = 0.0;
            M_registration_eyes_right(0,2) = 2.0*(xz + wy);
            M_registration_eyes_right(1,2) = 2.0*(yz - wx);
            M_registration_eyes_right(2,2) = ww - xx - yy + zz;
            M_registration_eyes_right(3,2) = 0.0;
        }
    }


    if (true)
    {
        const bool scale_separate = false;
        if ( scale_separate )
        {
            // scaling
            Scalar  nom_left(0), denom_left(0);
            Vector<Scalar,3>  sp_left, dp_left;
            for (int i=0; i<n_left; ++i)
            {
                sp_left = src_left[i]; sp_left -= scog_left;
                dp_left = dst_left[i]; dp_left -= dcog_left;

                sp_left = linear_transform(M_registration_eyes_left, sp_left);

                nom_left   += dot(sp_left,dp_left);
                denom_left += dot(sp_left,sp_left);
            }
            const Scalar scaling_left = nom_left / denom_left;
            Scalar  nom_right(0), denom_right(0);
            Vector<Scalar,3>  sp_right, dp_right;
            for (int i=0; i<n_right; ++i)
            {
                sp_right = src_right[i]; sp_right -= scog_right;
                dp_right = dst_right[i]; dp_right -= dcog_right;

                sp_right = linear_transform(M_registration_eyes_right, sp_right);

                nom_right   += dot(sp_right,dp_right);
                denom_right += dot(sp_right,sp_right);
            }
            const Scalar scaling_right = nom_right / denom_right;


            for (int i=0; i<3; ++i)
            {
                for (int j=0; j<3; ++j)
                {
                    M_registration_eyes_left(i,j)  *= scaling_left;
                    M_registration_eyes_right(i,j) *= scaling_right;
                }
            }
        }
        else // smartly combined scaling
        {
            // scaling
            Scalar  nom_left(0), denom_left(0);
            Vector<Scalar,3>  sp_left, dp_left;
            for (int i=0; i<n_left; ++i)
            {
                sp_left = src_left[i]; sp_left -= scog_left;
                dp_left = dst_left[i]; dp_left -= dcog_left;

                sp_left = linear_transform(M_registration_eyes_left, sp_left);

                nom_left   += dot(sp_left,dp_left);
                denom_left += dot(sp_left,sp_left);
            }
            const Scalar scaling_left = nom_left / denom_left;
            Scalar  nom_right(0), denom_right(0);
            Vector<Scalar,3>  sp_right, dp_right;
            for (int i=0; i<n_right; ++i)
            {
                sp_right = src_right[i]; sp_right -= scog_right;
                dp_right = dst_right[i]; dp_right -= dcog_right;

                sp_right = linear_transform(M_registration_eyes_right, sp_right);

                nom_right   += dot(sp_right,dp_right);
                denom_right += dot(sp_right,sp_right);
            }
            const Scalar scaling_right = nom_right / denom_right;

            // std::cerr << "scaling_left: " << scaling_left << std::endl;
            // std::cerr << "scaling_right: " << scaling_right << std::endl;

            const Scalar diam_minimum_m   = 0.02174; // avg -15%
            const Scalar diam_maximum_m   = 0.02875; // avg +15%
            const Scalar diam_template_m  = 0.0285; // template is 2.85 cm; average is 2.5 cm
            // std::cerr << "diam_template_m: " << diam_template_m << std::endl;

            const Scalar diam_lefteye_fitted_m = diam_template_m * scaling_left;
            // std::cerr << "diam_lefteye_fitted_m: " << diam_lefteye_fitted_m << std::endl;
            const Scalar diam_righteye_fitted_m = diam_template_m * scaling_right;
            // std::cerr << "diam_righteye_fitted_m: " << diam_righteye_fitted_m << std::endl;

            const bool valid_lefteye  = (diam_minimum_m <= diam_lefteye_fitted_m  && diam_lefteye_fitted_m <= diam_maximum_m)  ? true : false;
            const bool valid_righteye = (diam_minimum_m <= diam_righteye_fitted_m && diam_righteye_fitted_m <= diam_maximum_m) ? true : false;

            const Scalar diam_lefteye_corrected_m = std::max( std::min(diam_lefteye_fitted_m, diam_maximum_m), diam_minimum_m ); // to be within valid range
            // std::cerr << "diam_lefteye_corrected_m: " << diam_lefteye_corrected_m << std::endl;
            const Scalar diam_righteye_corrected_m = std::max( std::min(diam_righteye_fitted_m, diam_maximum_m), diam_minimum_m ); // to be within valid range
            // std::cerr << "diam_righteye_corrected_m: " << diam_righteye_corrected_m << std::endl;

            const Scalar scaling_lefteye_corrected = scaling_left * (diam_lefteye_corrected_m / diam_lefteye_fitted_m);
            // std::cerr << "scaling_lefteye_corrected: " << scaling_lefteye_corrected << std::endl;
            const Scalar scaling_righteye_corrected = scaling_right * (diam_righteye_corrected_m / diam_righteye_fitted_m);
            // std::cerr << "scaling_righteye_corrected: " << scaling_righteye_corrected << std::endl;

            Scalar scaling_total;
            if (valid_lefteye && valid_righteye)
            {
                scaling_total = 0.5 * (scaling_lefteye_corrected + scaling_righteye_corrected);
            }
            else if (valid_lefteye && !valid_righteye)
            {
                scaling_total = scaling_lefteye_corrected;
            }
            else if (!valid_lefteye && valid_righteye)
            {
                scaling_total = scaling_righteye_corrected;
            }
            else
            {
                scaling_total = 0.5 * (scaling_lefteye_corrected + scaling_righteye_corrected);
            }
            // std::cerr << "scaling_total: " << scaling_total << std::endl;

            for (int i=0; i<3; ++i)
            {
                for (int j=0; j<3; ++j)
                {
                    M_registration_eyes_left(i,j)  *= scaling_total;
                    M_registration_eyes_right(i,j) *= scaling_total;
                }
            }
        }
    }


    // translation part
    M_registration_eyes_left(0,3) = dcog_left[0] - M_registration_eyes_left(0,0)*scog_left[0] - M_registration_eyes_left(0,1)*scog_left[1] - M_registration_eyes_left(0,2)*scog_left[2];
    M_registration_eyes_left(1,3) = dcog_left[1] - M_registration_eyes_left(1,0)*scog_left[0] - M_registration_eyes_left(1,1)*scog_left[1] - M_registration_eyes_left(1,2)*scog_left[2];
    M_registration_eyes_left(2,3) = dcog_left[2] - M_registration_eyes_left(2,0)*scog_left[0] - M_registration_eyes_left(2,1)*scog_left[1] - M_registration_eyes_left(2,2)*scog_left[2];
    M_registration_eyes_left(3,3) = 1.0;

    M_registration_eyes_right(0,3) = dcog_right[0] - M_registration_eyes_right(0,0)*scog_right[0] - M_registration_eyes_right(0,1)*scog_right[1] - M_registration_eyes_right(0,2)*scog_right[2];
    M_registration_eyes_right(1,3) = dcog_right[1] - M_registration_eyes_right(1,0)*scog_right[0] - M_registration_eyes_right(1,1)*scog_right[1] - M_registration_eyes_right(1,2)*scog_right[2];
    M_registration_eyes_right(2,3) = dcog_right[2] - M_registration_eyes_right(2,0)*scog_right[0] - M_registration_eyes_right(2,1)*scog_right[1] - M_registration_eyes_right(2,2)*scog_right[2];
    M_registration_eyes_right(3,3) = 1.0;


    // slightly translate eyes back
    M_registration_eyes_left = Mat4f::translate(Vec3f(0.0f,0.0f,-0.0005f)) * M_registration_eyes_left;
    M_registration_eyes_right = Mat4f::translate(Vec3f(0.0f,0.0f,-0.0005f)) * M_registration_eyes_right;

    return true;
}

//=============================================================================
} // namespace surface_mesh
} // namespace graphene
//=============================================================================

