// Copyright (C) 2010  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"

#include "tester.h"
#include <dlib/memory_manager_stateless.h>
#include <dlib/array2d.h>

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.matrix4");

    void matrix_test (
    )
    /*!
        ensures
            - runs tests on the matrix stuff compliance with the specs
    !*/
    {
        print_spinner();

        {
            matrix<double,3,3> m = round(10*randm(3,3));
            matrix<double,3,1> v = round(10*randm(3,1));

            DLIB_TEST(equal( m*diagm(v) , m*tmp(diagm(v)) ));
            DLIB_TEST(equal( scale_columns(m,v) , m*tmp(diagm(v)) ));

            DLIB_TEST(equal( diagm(v)*m , tmp(diagm(v))*m ));
            DLIB_TEST(equal( scale_rows(m,v) , tmp(diagm(v))*m ));
        }

        {
            matrix<double,3,3> m = round(10*randm(3,3));
            matrix<double,1,3> v = round(10*randm(1,3));

            DLIB_TEST(equal( m*diagm(v) , m*tmp(diagm(v)) ));
            DLIB_TEST(equal( scale_columns(m,v) , m*tmp(diagm(v)) ));

            DLIB_TEST(equal( diagm(v)*m , tmp(diagm(v))*m ));
            DLIB_TEST(equal( scale_rows(m,v) , tmp(diagm(v))*m ));
        }

        {
            matrix<double> m = round(10*randm(3,3));
            matrix<double,1,3> v = round(10*randm(1,3));

            DLIB_TEST(equal( m*diagm(v) , m*tmp(diagm(v)) ));
            DLIB_TEST(equal( scale_columns(m,v) , m*tmp(diagm(v)) ));

            DLIB_TEST(equal( diagm(v)*m , tmp(diagm(v))*m ));
            DLIB_TEST(equal( scale_rows(m,v) , tmp(diagm(v))*m ));
        }

        {
            matrix<double> m = round(10*randm(3,3));
            matrix<double,0,3> v = round(10*randm(1,3));

            DLIB_TEST(equal( m*diagm(v) , m*tmp(diagm(v)) ));
            DLIB_TEST(equal( scale_columns(m,v) , m*tmp(diagm(v)) ));

            DLIB_TEST(equal( diagm(v)*m , tmp(diagm(v))*m ));
            DLIB_TEST(equal( scale_rows(m,v) , tmp(diagm(v))*m ));
        }

        {
            matrix<double> m = round(10*randm(3,3));
            matrix<double,1,0> v = round(10*randm(1,3));

            DLIB_TEST(equal( m*diagm(v) , m*tmp(diagm(v)) ));
            DLIB_TEST(equal( scale_columns(m,v) , m*tmp(diagm(v)) ));

            DLIB_TEST(equal( diagm(v)*m , tmp(diagm(v))*m ));
            DLIB_TEST(equal( scale_rows(m,v) , tmp(diagm(v))*m ));
        }

        {
            matrix<double> m = round(10*randm(3,3));
            matrix<double,3,0> v = round(10*randm(3,1));

            DLIB_TEST(equal( m*diagm(v) , m*tmp(diagm(v)) ));
            DLIB_TEST(equal( scale_columns(m,v) , m*tmp(diagm(v)) ));

            DLIB_TEST(equal( diagm(v)*m , tmp(diagm(v))*m ));
            DLIB_TEST(equal( scale_rows(m,v) , tmp(diagm(v))*m ));
        }

        {
            matrix<double> m = round(10*randm(3,3));
            matrix<double,0,1> v = round(10*randm(3,1));

            DLIB_TEST(equal( m*diagm(v) , m*tmp(diagm(v)) ));
            DLIB_TEST(equal( scale_columns(m,v) , m*tmp(diagm(v)) ));

            DLIB_TEST(equal( diagm(v)*m , tmp(diagm(v))*m ));
            DLIB_TEST(equal( scale_rows(m,v) , tmp(diagm(v))*m ));
        }

        {
            matrix<double,3,3> m = round(10*randm(3,3));
            matrix<double,3,0> v = round(10*randm(3,1));

            DLIB_TEST(equal( m*diagm(v) , m*tmp(diagm(v)) ));
            DLIB_TEST(equal( scale_columns(m,v) , m*tmp(diagm(v)) ));

            DLIB_TEST(equal( diagm(v)*m , tmp(diagm(v))*m ));
            DLIB_TEST(equal( scale_rows(m,v) , tmp(diagm(v))*m ));
        }

        {
            matrix<double,3,3> m = round(10*randm(3,3));
            matrix<double,0,1> v = round(10*randm(3,1));

            DLIB_TEST(equal( m*diagm(v) , m*tmp(diagm(v)) ));
            DLIB_TEST(equal( scale_columns(m,v) , m*tmp(diagm(v)) ));

            DLIB_TEST(equal( diagm(v)*m , tmp(diagm(v))*m ));
            DLIB_TEST(equal( scale_rows(m,v) , tmp(diagm(v))*m ));
        }

        {
            matrix<double,3,5> m = round(10*randm(3,5));
            matrix<double,0,1> v1 = round(10*randm(5,1));
            matrix<double,0,1> v2 = round(10*randm(3,1));

            DLIB_TEST(equal( m*diagm(v1) , m*tmp(diagm(v1)) ));
            DLIB_TEST(equal( scale_columns(m,v1) , m*tmp(diagm(v1)) ));

            DLIB_TEST(equal( diagm(v2)*m , tmp(diagm(v2))*m ));
            DLIB_TEST(equal( scale_rows(m,v2) , tmp(diagm(v2))*m ));
        }

        {
            matrix<double,3,5> m = round(10*randm(3,5));
            matrix<double,5,1> v1 = round(10*randm(5,1));
            matrix<double,3,1> v2 = round(10*randm(3,1));

            DLIB_TEST(equal( m*diagm(v1) , m*tmp(diagm(v1)) ));
            DLIB_TEST(equal( scale_columns(m,v1) , m*tmp(diagm(v1)) ));

            DLIB_TEST(equal( diagm(v2)*m , tmp(diagm(v2))*m ));
            DLIB_TEST(equal( scale_rows(m,v2) , tmp(diagm(v2))*m ));
        }

    }

    void test_stuff()
    {
        print_spinner();

        {
            matrix<double> m(3,3), lr(3,3), ud(3,3);

            m = 1,2,3,
                4,5,6,
                7,8,9;

            lr = 3,2,1,
                 6,5,4,
                 9,8,7;

            ud = 7,8,9,
                 4,5,6,
                 1,2,3;

            DLIB_TEST(lr == fliplr(m));
            DLIB_TEST(ud == flipud(m));
        }
        {
            matrix<double> m(3,2), lr(3,2), ud(3,2);

            m = 1,2,
                3,4,
                5,6;

            lr = 2,1,
                 4,3,
                 6,5;

            ud = 5,6,
                 3,4,
                 1,2;

            DLIB_TEST(lr == fliplr(m));
            DLIB_TEST(ud == flipud(m));
        }

        {
            matrix<int> a, b;

            a = matrix_cast<int>(round(10*randm(3,3)));
            b = a;

            b *= b;
            DLIB_TEST(b == a*a);
        }

        {
            matrix<double> m(2,3), m2(2,3);

            m = 1,2,3,
                4,5,6;

            m2 = 3,4,5,
                 6,7,8;

            DLIB_TEST(m + 2 == m2);
            DLIB_TEST(2 + m == m2);

            m += 2;
            DLIB_TEST(m == m2);
            m -= 2;

            m2 = 0,1,2,
                 3,4,5;

            DLIB_TEST(m - 1 == m2);

            m -= 1;
            DLIB_TEST(m == m2);
            m += 1;

            m2 = 5,4,3,
                 2,1,0;

            DLIB_TEST(6 - m == m2);
        }

        {
            matrix<float> m(2,3), m2(2,3);

            m = 1,2,3,
                4,5,6;

            m2 = 3,4,5,
                 6,7,8;

            DLIB_TEST(m + 2 == m2);
            DLIB_TEST(2 + m == m2);

            m += 2;
            DLIB_TEST(m == m2);
            m -= 2;

            m2 = 0,1,2,
                 3,4,5;

            DLIB_TEST(m - 1 == m2);

            m -= 1;
            DLIB_TEST(m == m2);
            m += 1;

            m2 = 5,4,3,
                 2,1,0;

            DLIB_TEST(6 - m == m2);
        }

        {
            matrix<int> m(2,3), m2(2,3);

            m = 1,2,3,
                4,5,6;

            m2 = 3,4,5,
                 6,7,8;

            DLIB_TEST(m + 2 == m2);
            DLIB_TEST(2 + m == m2);

            m += 2;
            DLIB_TEST(m == m2);
            m -= 2;

            m2 = 0,1,2,
                 3,4,5;

            DLIB_TEST(m - 1 == m2);

            m -= 1;
            DLIB_TEST(m == m2);
            m += 1;

            m2 = 5,4,3,
                 2,1,0;

            DLIB_TEST(6 - m == m2);
        }

        {
            matrix<int,2,3> m, m2;

            m = 1,2,3,
                4,5,6;

            m2 = 3,4,5,
                 6,7,8;

            DLIB_TEST(m + 2 == m2);
            DLIB_TEST(2 + m == m2);

            m += 2;
            DLIB_TEST(m == m2);
            m -= 2;

            m2 = 0,1,2,
                 3,4,5;

            DLIB_TEST(m - 1 == m2);

            m -= 1;
            DLIB_TEST(m == m2);
            m += 1;

            m2 = 5,4,3,
                 2,1,0;

            DLIB_TEST(6 - m == m2);
        }

        {
            matrix<double> m(2,3), m2(3,2);

            m = 1,2,3,
                4,5,6;

            m2 = 2,5,
                 3,6,
                 4,7;

            DLIB_TEST(trans(m+1) == m2);
            DLIB_TEST(trans(m)+1 == m2);
            DLIB_TEST(1+trans(m) == m2);
            DLIB_TEST(1+m-1 == m);

            m = trans(m+1);
            DLIB_TEST(m == m2);
            m = trans(m-1);
            DLIB_TEST(trans(m+1) == m2);
            m = trans(m)+1;
            DLIB_TEST(m == m2);
        }

        {
            matrix<double> d(3,1), di(3,1);
            matrix<double> m(3,3);

            m = 1,2,3,
                4,5,6,
                7,8,9;

            d = 1,2,3;

            di = 1, 1/2.0, 1/3.0;

            DLIB_TEST(inv(diagm(d)) == diagm(di));
            DLIB_TEST(pinv(diagm(d)) == diagm(di));
            DLIB_TEST(inv(diagm(d))*m == tmp(diagm(di))*m);
            DLIB_TEST(m*inv(diagm(d)) == m*tmp(diagm(di)));

            DLIB_TEST(equal(inv(diagm(d)) + m , tmp(diagm(di)) + m));
            DLIB_TEST(equal(m + inv(diagm(d)) , tmp(diagm(di)) + m));

            DLIB_TEST((m + identity_matrix<double>(3) == m + tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + identity_matrix<double,3>() == m + tmp(identity_matrix<double,3>())));
            DLIB_TEST((m + 2*identity_matrix<double>(3) == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + 2*identity_matrix<double,3>() == m + 2*tmp(identity_matrix<double,3>())));
            DLIB_TEST((m + identity_matrix<double>(3)*2 == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + identity_matrix<double,3>()*2 == m + 2*tmp(identity_matrix<double,3>())));

            DLIB_TEST((identity_matrix<double>(3) + m == m + tmp(identity_matrix<double>(3))));
            DLIB_TEST((identity_matrix<double,3>() + m == m + tmp(identity_matrix<double,3>())));
            DLIB_TEST((2*identity_matrix<double>(3) + m == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((2*identity_matrix<double,3>() + m == m + 2*tmp(identity_matrix<double,3>())));

        }
        {
            matrix<double,3,1> d(3,1), di(3,1);
            matrix<double,3,3> m(3,3);

            m = 1,2,3,
                4,5,6,
                7,8,9;

            d = 1,2,3;

            di = 1, 1/2.0, 1/3.0;

            DLIB_TEST(equal(inv(diagm(d)) , diagm(di)));
            DLIB_TEST(equal(inv(diagm(d)) , diagm(di)));
            DLIB_TEST(equal(inv(diagm(d))*m , tmp(diagm(di))*m));
            DLIB_TEST(equal(m*inv(diagm(d)) , m*tmp(diagm(di))));

            DLIB_TEST_MSG(equal(inv(diagm(d)) + m , tmp(diagm(di)) + m),
                          (inv(diagm(d)) + m) - (tmp(diagm(di)) + m) );
            DLIB_TEST(equal(m + inv(diagm(d)) , tmp(diagm(di)) + m));

            DLIB_TEST((m + identity_matrix<double>(3) == m + tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + identity_matrix<double,3>() == m + tmp(identity_matrix<double,3>())));
            DLIB_TEST((m + 2*identity_matrix<double>(3) == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + 2*identity_matrix<double,3>() == m + 2*tmp(identity_matrix<double,3>())));
            DLIB_TEST((m + identity_matrix<double>(3)*2 == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + identity_matrix<double,3>()*2 == m + 2*tmp(identity_matrix<double,3>())));

            DLIB_TEST((identity_matrix<double>(3) + m == m + tmp(identity_matrix<double>(3))));
            DLIB_TEST((identity_matrix<double,3>() + m == m + tmp(identity_matrix<double,3>())));
            DLIB_TEST((2*identity_matrix<double>(3) + m == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((2*identity_matrix<double,3>() + m == m + 2*tmp(identity_matrix<double,3>())));
        }

        {
            matrix<double,1,3> d(1,3), di(1,3);
            matrix<double,3,3> m(3,3);

            m = 1,2,3,
                4,5,6,
                7,8,9;

            d = 1,2,3;

            di = 1, 1/2.0, 1/3.0;

            DLIB_TEST(equal(inv(diagm(d)) , diagm(di)));
            DLIB_TEST(equal(inv(diagm(d)) , diagm(di)));
            DLIB_TEST(equal(inv(diagm(d))*m , tmp(diagm(di))*m));
            DLIB_TEST(equal(m*inv(diagm(d)) , m*tmp(diagm(di))));

            DLIB_TEST(equal(inv(diagm(d)) + m , tmp(diagm(di)) + m));
            DLIB_TEST(equal(m + inv(diagm(d)) , tmp(diagm(di)) + m));

            DLIB_TEST((m + identity_matrix<double>(3) == m + tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + identity_matrix<double,3>() == m + tmp(identity_matrix<double,3>())));
            DLIB_TEST((m + 2*identity_matrix<double>(3) == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + 2*identity_matrix<double,3>() == m + 2*tmp(identity_matrix<double,3>())));
            DLIB_TEST((m + identity_matrix<double>(3)*2 == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + identity_matrix<double,3>()*2 == m + 2*tmp(identity_matrix<double,3>())));

            DLIB_TEST((identity_matrix<double>(3) + m == m + tmp(identity_matrix<double>(3))));
            DLIB_TEST((identity_matrix<double,3>() + m == m + tmp(identity_matrix<double,3>())));
            DLIB_TEST((2*identity_matrix<double>(3) + m == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((2*identity_matrix<double,3>() + m == m + 2*tmp(identity_matrix<double,3>())));
        }

        {
            matrix<double,1,0> d(1,3), di(1,3);
            matrix<double,0,3> m(3,3);

            m = 1,2,3,
                4,5,6,
                7,8,9;

            d = 1,2,3;

            di = 1, 1/2.0, 1/3.0;

            DLIB_TEST(equal(inv(diagm(d)) , diagm(di)));
            DLIB_TEST(equal(inv(diagm(d)) , diagm(di)));
            DLIB_TEST(equal(inv(diagm(d))*m , tmp(diagm(di))*m));
            DLIB_TEST(equal(m*inv(diagm(d)) , m*tmp(diagm(di))));

            DLIB_TEST(equal(inv(diagm(d)) + m , tmp(diagm(di)) + m));
            DLIB_TEST(equal(m + inv(diagm(d)) , tmp(diagm(di)) + m));

            DLIB_TEST((m + identity_matrix<double>(3) == m + tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + identity_matrix<double,3>() == m + tmp(identity_matrix<double,3>())));
            DLIB_TEST((m + 2*identity_matrix<double>(3) == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + 2*identity_matrix<double,3>() == m + 2*tmp(identity_matrix<double,3>())));
            DLIB_TEST((m + identity_matrix<double>(3)*2 == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((m + identity_matrix<double,3>()*2 == m + 2*tmp(identity_matrix<double,3>())));

            DLIB_TEST((identity_matrix<double>(3) + m == m + tmp(identity_matrix<double>(3))));
            DLIB_TEST((identity_matrix<double,3>() + m == m + tmp(identity_matrix<double,3>())));
            DLIB_TEST((2*identity_matrix<double>(3) + m == m + 2*tmp(identity_matrix<double>(3))));
            DLIB_TEST((2*identity_matrix<double,3>() + m == m + 2*tmp(identity_matrix<double,3>())));
        }

        {
            matrix<double,3,1> d1, d2;

            d1 = 1,2,3;

            d2 = 2,3,4;

            matrix<double,3,3> ans;
            ans = 2, 0, 0,
                  0, 6, 0,
                  0, 0, 12;

            DLIB_TEST(ans == diagm(d1)*diagm(d2));
        }

        dlib::rand rnd;
        for (int i = 0; i < 1; ++i)
        {
            matrix<double> d1 = randm(4,1,rnd);
            matrix<double,5,1> d2 = randm(5,1,rnd);

            matrix<double,4,5> m = randm(4,5,rnd);

            DLIB_TEST_MSG(equal(pointwise_multiply(d1*trans(d2), m) , diagm(d1)*m*diagm(d2)),
                          pointwise_multiply(d1*trans(d2), m) - diagm(d1)*m*diagm(d2)
            );
            DLIB_TEST(equal(pointwise_multiply(d1*trans(d2), m) , diagm(d1)*(m*diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply(d1*trans(d2), m) , (diagm(d1)*m)*diagm(d2)));

            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans(reciprocal(d2)), m) , inv(diagm(d1))*m*inv(diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans(reciprocal(d2)), m) , inv(diagm(d1))*(m*inv(diagm(d2)))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans(reciprocal(d2)), m) , (inv(diagm(d1))*m)*inv(diagm(d2))));

            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans((d2)), m) , inv(diagm(d1))*m*(diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans((d2)), m) , inv(diagm(d1))*(m*(diagm(d2)))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans((d2)), m) , (inv(diagm(d1))*m)*(diagm(d2))));

            DLIB_TEST(equal(pointwise_multiply((d1)*trans(reciprocal(d2)), m) , (diagm(d1))*m*inv(diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply((d1)*trans(reciprocal(d2)), m) , (diagm(d1))*(m*inv(diagm(d2)))));
            DLIB_TEST(equal(pointwise_multiply((d1)*trans(reciprocal(d2)), m) , ((diagm(d1))*m)*inv(diagm(d2))));
        }
        for (int i = 0; i < 1; ++i)
        {
            matrix<double,4,1> d1 = randm(4,1,rnd);
            matrix<double,5,1> d2 = randm(5,1,rnd);

            matrix<double,4,5> m = randm(4,5,rnd);

            DLIB_TEST(equal(pointwise_multiply(d1*trans(d2), m) , diagm(d1)*m*diagm(d2)));
            DLIB_TEST(equal(pointwise_multiply(d1*trans(d2), m) , diagm(d1)*(m*diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply(d1*trans(d2), m) , (diagm(d1)*m)*diagm(d2)));

            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans(reciprocal(d2)), m) , inv(diagm(d1))*m*inv(diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans(reciprocal(d2)), m) , inv(diagm(d1))*(m*inv(diagm(d2)))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans(reciprocal(d2)), m) , (inv(diagm(d1))*m)*inv(diagm(d2))));

            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans((d2)), m) , inv(diagm(d1))*m*(diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans((d2)), m) , inv(diagm(d1))*(m*(diagm(d2)))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans((d2)), m) , (inv(diagm(d1))*m)*(diagm(d2))));

            DLIB_TEST(equal(pointwise_multiply((d1)*trans(reciprocal(d2)), m) , (diagm(d1))*m*inv(diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply((d1)*trans(reciprocal(d2)), m) , (diagm(d1))*(m*inv(diagm(d2)))));
            DLIB_TEST(equal(pointwise_multiply((d1)*trans(reciprocal(d2)), m) , ((diagm(d1))*m)*inv(diagm(d2))));
        }
        for (int i = 0; i < 1; ++i)
        {
            matrix<double,4,1> d1 = randm(4,1,rnd);
            matrix<double,5,1> d2 = randm(5,1,rnd);

            matrix<double,0,0> m = randm(4,5,rnd);

            DLIB_TEST(equal(pointwise_multiply(d1*trans(d2), m) , diagm(d1)*m*diagm(d2)));
            DLIB_TEST(equal(pointwise_multiply(d1*trans(d2), m) , diagm(d1)*(m*diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply(d1*trans(d2), m) , (diagm(d1)*m)*diagm(d2)));

            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans(reciprocal(d2)), m) , inv(diagm(d1))*m*inv(diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans(reciprocal(d2)), m) , inv(diagm(d1))*(m*inv(diagm(d2)))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans(reciprocal(d2)), m) , (inv(diagm(d1))*m)*inv(diagm(d2))));

            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans((d2)), m) , inv(diagm(d1))*m*(diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans((d2)), m) , inv(diagm(d1))*(m*(diagm(d2)))));
            DLIB_TEST(equal(pointwise_multiply(reciprocal(d1)*trans((d2)), m) , (inv(diagm(d1))*m)*(diagm(d2))));

            DLIB_TEST(equal(pointwise_multiply((d1)*trans(reciprocal(d2)), m) , (diagm(d1))*m*inv(diagm(d2))));
            DLIB_TEST(equal(pointwise_multiply((d1)*trans(reciprocal(d2)), m) , (diagm(d1))*(m*inv(diagm(d2)))));
            DLIB_TEST(equal(pointwise_multiply((d1)*trans(reciprocal(d2)), m) , ((diagm(d1))*m)*inv(diagm(d2))));
        }

        {
            for (int i = 0; i < 5; ++i)
            {
                matrix<double> m = randm(3,4) + 1;

                DLIB_TEST(equal(1.0/m , reciprocal(m)));
                DLIB_TEST(equal(0.0/m , zeros_matrix<double>(3,4)));
            }
        }

        {
            matrix<int> m(2,3);
            m = 1,2,3,
                4,5,6;
            matrix<int> M(2,3);
            M = m;

            DLIB_TEST(upperbound(m,6) == M);
            DLIB_TEST(upperbound(m,60) == M);
            DLIB_TEST(lowerbound(m,-2) == M);
            DLIB_TEST(lowerbound(m,0) == M);

            M = 2,2,3,
                4,5,6;
            DLIB_TEST(lowerbound(m,2) == M);

            M = 0,0,0,
                0,0,0;
            DLIB_TEST(upperbound(m,0) == M);

            M = 1,2,3,
                3,3,3;
            DLIB_TEST(upperbound(m,3) == M);
        }

        {
            matrix<double,9,5> A = randm(9,5);
            matrix<double> B = A;

            orthogonalize(A);
            orthogonalize(B);

            DLIB_TEST(equal(A,B));
        }
    }

    template <
        long D1,
        long D2,
        long D3,
        long D4
        >
    void test_conv()
    {
        dlog << LINFO << D1 << " " << D2 << " " << D3 << " " << D4;
        matrix<int,D1,D1> a(1,1);
        matrix<int,D2,D2> b(2,2);
        matrix<int,D3,D3> c(3,3);
        matrix<int,D4,D1> d(4,1);

        a = 4;

        b = 1,2,
            3,4;

        c = 1,2,3,
            4,5,6,
            7,8,9;

        d = 1,
            2,
            3,
            4;

        matrix<int> temp(4,4), temp2;
        temp =     1,    4,    7,    6,
                    7,   23,   33,   24,
                    19,   53,   63,   42,
                    21,   52,   59,   36;

        DLIB_TEST(conv(b,c) == temp);
        DLIB_TEST(conv(c,b) == temp);
        DLIB_TEST(xcorr(c,flip(b)) == temp);

        temp.set_size(2,2);
        temp =   23,   33,
                53,   63;
        DLIB_TEST(conv_same(b,c) == temp);
        DLIB_TEST(xcorr_same(b,flip(c)) == temp);

        temp2.set_size(2,2);
        temp2 = 63, 53,
                33, 23;
        DLIB_TEST(flip(temp) == temp2);
        DLIB_TEST(flip(temp) == fliplr(flipud(temp)));

        DLIB_TEST(conv_valid(b,c).nr() == 0);
        DLIB_TEST(conv_valid(b,c).nc() == 0);

        DLIB_TEST(conv_valid(c,b) == temp);
        DLIB_TEST(xcorr_valid(c,flip(b)) == temp);

        temp.set_size(1,1);
        temp =  16;

        DLIB_TEST(conv(a,a) == temp);
        DLIB_TEST(conv_same(a,a) == temp);
        DLIB_TEST(conv_valid(a,a) == temp);
        DLIB_TEST(xcorr(a,a) == temp);
        DLIB_TEST(xcorr_same(a,a) == temp);
        DLIB_TEST(xcorr_valid(a,a) == temp);

        temp.set_size(0,0);
        DLIB_TEST(conv(temp,temp).nr() == 0);
        DLIB_TEST(conv(temp,temp).nc() == 0);
        DLIB_TEST(conv_same(temp,temp).nr() == 0);
        DLIB_TEST(conv_same(temp,temp).nc() == 0);
        DLIB_TEST_MSG(conv_valid(temp,temp).nr() == 0, conv_valid(temp,temp).nr());
        DLIB_TEST(conv_valid(temp,temp).nc() == 0);
        DLIB_TEST(conv(c,temp).nr() == 0);
        DLIB_TEST(conv(c,temp).nc() == 0);
        DLIB_TEST(conv_same(c,temp).nr() == 0);
        DLIB_TEST(conv_same(c,temp).nc() == 0);
        DLIB_TEST(conv_valid(c,temp).nr() == 0);
        DLIB_TEST(conv_valid(c,temp).nc() == 0);
        DLIB_TEST(conv(temp,c).nr() == 0);
        DLIB_TEST(conv(temp,c).nc() == 0);
        DLIB_TEST(conv_same(temp,c).nr() == 0);
        DLIB_TEST(conv_same(temp,c).nc() == 0);
        DLIB_TEST(conv_valid(temp,c).nr() == 0);
        DLIB_TEST(conv_valid(temp,c).nc() == 0);

        temp.set_size(5,2);
        temp =     1,    2,
                    5,    8,
                    9,   14,
                    13,   20,
                    12,   16;
        DLIB_TEST(conv(b,d) == temp);
        DLIB_TEST(xcorr(b,flip(d)) == temp);

        temp.set_size(2,2);
        temp =    9,   14,
                13,   20;
        DLIB_TEST(conv_same(b,d) == temp);
        DLIB_TEST(xcorr_same(b,flip(d)) == temp);

        DLIB_TEST(conv_valid(b,d).nr() == 0);
        DLIB_TEST(xcorr_valid(b,flip(d)).nr() == 0);
        DLIB_TEST_MSG(conv_valid(b,d).nc() == 0, conv_valid(b,d).nc());
        DLIB_TEST(xcorr_valid(b,flip(d)).nc() == 0);

        temp.set_size(5,5);
        temp =      1,     4,    10,    12,     9,
                    8,    26,    56,    54,    36,
                    30,    84,   165,   144,    90,
                    56,   134,   236,   186,   108,
                    49,   112,   190,   144,    81;

        DLIB_TEST(conv(c,c) == temp);
        DLIB_TEST(xcorr(c,flip(c)) == temp);
        matrix<int> temp3 = c;
        temp3 = conv(temp3,c);
        DLIB_TEST(temp3 == temp);

        temp3 = c;
        temp3 = conv(c,temp3);
        DLIB_TEST(temp3 == temp);

        temp.set_size(3,3);
        temp =     26,    56,    54,
                    84,   165,   144,
                    134,   236,   186;
        DLIB_TEST(conv_same(c,c) == temp);
        DLIB_TEST(xcorr_same(c,flip(c)) == temp);
        temp3 = c;
        temp3 = conv_same(c,temp3);
        DLIB_TEST(temp3 == temp);
        temp3 = c;
        temp3 = conv_same(temp3,c);
        DLIB_TEST(temp3 == temp);

        temp.set_size(1,1);
        temp = 165;
        DLIB_TEST(conv_valid(c,c) == temp);
        DLIB_TEST(xcorr_valid(c,flip(c)) == temp);
        temp3 = c;
        temp3 = conv_valid(c,temp3);
        DLIB_TEST(temp3 == temp);
        temp3 = c;
        temp3 = conv_valid(temp3,c);
        DLIB_TEST(temp3 == temp);

        dlib::rand rnd;
        for (int i = 0; i < 3; ++i)
        {
            matrix<complex<int> > a, b;
            a = complex_matrix(matrix_cast<int>(round(20*randm(2,7,rnd))),
                               matrix_cast<int>(round(20*randm(2,7,rnd))));
            b = complex_matrix(matrix_cast<int>(round(20*randm(3,2,rnd))),
                               matrix_cast<int>(round(20*randm(3,2,rnd))));

            DLIB_TEST(xcorr(a,b)       == conv(a, flip(conj(b))));
            DLIB_TEST(xcorr_valid(a,b) == conv_valid(a, flip(conj(b))));
            DLIB_TEST(xcorr_same(a,b)  == conv_same(a, flip(conj(b))));
        }

        for (int i = 0; i < 30; ++i)
        {
            auto nr1 = rnd.get_integer_in_range(1,30);
            auto nc1 = rnd.get_integer_in_range(1,30);
            auto nr2 = rnd.get_integer_in_range(1,30);
            auto nc2 = rnd.get_integer_in_range(1,30);
            matrix<double> a, b;
            a = randm(nr1,nc1,rnd);
            b = randm(nr2,nc2,rnd);

            DLIB_TEST(max(abs(xcorr(a,b) - xcorr_fft(a,b))) < 1e-12);
        }
    }

    void test_complex()
    {
        matrix<complex<double> > a, b;

        a = complex_matrix(linspace(1,7,7), linspace(2,8,7));
        b = complex_matrix(linspace(4,10,7), linspace(2,8,7));

        DLIB_TEST(mean(a) == complex<double>(4, 5));
    }

    void test_setsubs()
    {
        {
            matrix<double> m(3,3);
            m = 0;

            set_colm(m,0) += 1;
            set_rowm(m,0) += 1;
            set_subm(m,1,1,2,2) += 5;

            matrix<double> m2(3,3);
            m2 = 2, 1, 1,
                 1, 5, 5,
                 1, 5, 5;

            DLIB_TEST(m == m2);

            set_colm(m,0) -= 1;
            set_rowm(m,0) -= 1;
            set_subm(m,1,1,2,2) -= 5;

            m2 = 0;
            DLIB_TEST(m == m2);

            matrix<double,1,3> r;
            matrix<double,3,1> c;
            matrix<double,2,2> b;
            r = 1,2,3;

            c = 2,
                3,
                4;

            b = 2,3,
                4,5;

            set_colm(m,1) += c;
            set_rowm(m,1) += r;
            set_subm(m,1,1,2,2) += b;

            m2 = 0, 2, 0,
                 1, 7, 6,
                 0, 8, 5;

            DLIB_TEST(m2 == m);

            set_colm(m,1) -= c;
            set_rowm(m,1) -= r;
            set_subm(m,1,1,2,2) -= b;

            m2 = 0;
            DLIB_TEST(m2 == m);

            // check that the code path for destructive aliasing works right.
            m = 2*identity_matrix<double>(3);
            set_colm(m,1) += m*c;
            m2 = 2, 4, 0,
                 0, 8, 0,
                 0, 8, 2;
            DLIB_TEST(m == m2);

            m = 2*identity_matrix<double>(3);
            set_colm(m,1) -= m*c;
            m2 = 2, -4, 0,
                 0, -4, 0,
                 0, -8, 2;
            DLIB_TEST(m == m2);

            m = 2*identity_matrix<double>(3);
            set_rowm(m,1) += r*m;
            m2 = 2, 0, 0,
                 2, 6, 6,
                 0, 0, 2;
            DLIB_TEST(m == m2);

            m = 2*identity_matrix<double>(3);
            set_rowm(m,1) -= r*m;
            m2 = 2, 0, 0,
                -2, -2, -6,
                 0, 0, 2;
            DLIB_TEST(m == m2);

            m = identity_matrix<double>(3);
            const rectangle rect(0,0,1,1);
            set_subm(m,rect) += subm(m,rect)*b;
            m2 = 3, 3, 0,
                 4, 6, 0,
                 0, 0, 1;
            DLIB_TEST(m == m2);

            m = identity_matrix<double>(3);
            set_subm(m,rect) -= subm(m,rect)*b;
            m2 = -1, -3, 0,
                 -4, -4, 0,
                  0, 0, 1;
            DLIB_TEST(m == m2);

        }

        {
            matrix<double,1,1> a, b;
            a = 2;
            b = 3;
            DLIB_TEST(dot(a,b) == 6);
        }
        {
            matrix<double,1,1> a;
            matrix<double,0,1> b(1);
            a = 2;
            b = 3;
            DLIB_TEST(dot(a,b) == 6);
            DLIB_TEST(dot(b,a) == 6);
        }
        {
            matrix<double,1,1> a;
            matrix<double,1,0> b(1);
            a = 2;
            b = 3;
            DLIB_TEST(dot(a,b) == 6);
            DLIB_TEST(dot(b,a) == 6);
        }
    }

    template <typename T>
    std::vector<int> tovect1(const T& m)
    {
        std::vector<int> temp;
        for (typename T::const_iterator i = m.begin(); i != m.end(); ++i)
        {
            temp.push_back(*i);
        }
        return temp;
    }

    template <typename T>
    std::vector<int> tovect2(const T& m)
    {
        std::vector<int> temp;
        for (typename T::const_iterator i = m.begin(); i != m.end(); i++)
        {
            temp.push_back(*i);
        }
        return temp;
    }

    template <typename T>
    std::vector<int> tovect3(const T& m_)
    {
        matrix<int> m(m_);
        std::vector<int> temp;
        for (matrix<int>::iterator i = m.begin(); i != m.end(); ++i)
        {
            temp.push_back(*i);
        }
        return temp;
    }

    template <typename T>
    std::vector<int> tovect4(const T& m_)
    {
        matrix<int> m(m_);
        std::vector<int> temp;
        for (matrix<int>::iterator i = m.begin(); i != m.end(); i++)
        {
            temp.push_back(*i);
        }
        return temp;
    }

    void test_iterators()
    {
        matrix<int> m(3,2);
        m = 1,2,3,
        4,5,6;

        std::vector<int> v1 = tovect1(m);
        std::vector<int> v2 = tovect2(m);
        std::vector<int> v3 = tovect3(m);
        std::vector<int> v4 = tovect4(m);

        std::vector<int> v5 = tovect1(m+m);
        std::vector<int> v6 = tovect2(m+m);
        std::vector<int> v7 = tovect3(m+m);
        std::vector<int> v8 = tovect4(m+m);

        std::vector<int> a1, a2;
        for (int i = 1; i <= 6; ++i)
        {
            a1.push_back(i);
            a2.push_back(i*2);
        }

        DLIB_TEST(max(abs(mat(v1) - mat(a1))) == 0);
        DLIB_TEST(max(abs(mat(v2) - mat(a1))) == 0);
        DLIB_TEST(max(abs(mat(v3) - mat(a1))) == 0);
        DLIB_TEST(max(abs(mat(v4) - mat(a1))) == 0);

        DLIB_TEST(max(abs(mat(v5) - mat(a2))) == 0);
        DLIB_TEST(max(abs(mat(v6) - mat(a2))) == 0);
        DLIB_TEST(max(abs(mat(v7) - mat(a2))) == 0);
        DLIB_TEST(max(abs(mat(v8) - mat(a2))) == 0);
    }

    void test_linpiece()
    {
        matrix<double,0,1> temp = linpiece(5, linspace(-1, 9, 2));
        DLIB_CASSERT(temp.size() == 1,"");
        DLIB_CASSERT(std::abs(temp(0) - 6) < 1e-13,"");

        temp = linpiece(5, linspace(-1, 9, 6));
        DLIB_CASSERT(temp.size() == 5,"");
        DLIB_CASSERT(std::abs(temp(0) - 2) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(1) - 2) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(2) - 2) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(3) - 0) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(4) - 0) < 1e-13,"");

        temp = linpiece(4, linspace(-1, 9, 6));
        DLIB_CASSERT(temp.size() == 5,"");
        DLIB_CASSERT(std::abs(temp(0) - 2) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(1) - 2) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(2) - 1) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(3) - 0) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(4) - 0) < 1e-13,"");

        temp = linpiece(40, linspace(-1, 9, 6));
        DLIB_CASSERT(temp.size() == 5,"");
        DLIB_CASSERT(std::abs(temp(0) - 2) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(1) - 2) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(2) - 2) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(3) - 2) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(4) - 2) < 1e-13,"");

        temp = linpiece(-40, linspace(-1, 9, 6));
        DLIB_CASSERT(temp.size() == 5,"");
        DLIB_CASSERT(std::abs(temp(0) - 0) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(1) - 0) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(2) - 0) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(3) - 0) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(4) - 0) < 1e-13,"");

        temp = linpiece(0, linspace(-1, 9, 6));
        DLIB_CASSERT(temp.size() == 5,"");
        DLIB_CASSERT(std::abs(temp(0) - 1) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(1) - 0) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(2) - 0) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(3) - 0) < 1e-13,"");
        DLIB_CASSERT(std::abs(temp(4) - 0) < 1e-13,"");

    }

    class matrix_tester : public tester
    {
    public:
        matrix_tester (
        ) :
            tester ("test_matrix4",
                    "Runs tests on the scale_rows and scale_columns functions.")
        {}

        void perform_test (
        )
        {
            test_iterators();
            test_setsubs();

            test_conv<0,0,0,0>();
            test_conv<1,2,3,4>();

            test_stuff();
            for (int i = 0; i < 10; ++i)
                matrix_test();

            test_complex();
            test_linpiece();
        }
    } a;

}


/******************************************************************
 *
 * Round for C++
 *
 * Copyright (C) Satoshi Konno 2015
 *
 * This is licensed under BSD-style license, see file COPYING.
 *
 ******************************************************************/

#include <boost/test/unit_test.hpp>

#include <stdlib.h>
#include <round/common/Random.h>
#include <string.h>

using namespace Round;

BOOST_AUTO_TEST_SUITE(common)

BOOST_AUTO_TEST_CASE(RandomTest01) {
  Random rand;

  int randVal = rand.rand();
  BOOST_CHECK(0 <= randVal);
  BOOST_CHECK(randVal <= RAND_MAX);
}

BOOST_AUTO_TEST_CASE(RandomTest02) {
  int minRange = 1;
  int maxRange = 10;

  Random randRange(minRange, maxRange);

  int randVal = randRange.rand();
  BOOST_CHECK(minRange <= randVal);
  BOOST_CHECK(randVal <= maxRange);
}

BOOST_AUTO_TEST_CASE(RandomTest03) {
  int minRange = 10;
  int maxRange = 1;

  Random randRange(minRange, maxRange);
  std::swap(minRange, maxRange);

  int randVal = randRange.rand();
  BOOST_CHECK(minRange <= randVal);
  BOOST_CHECK(randVal <= maxRange);
}

BOOST_AUTO_TEST_CASE(RandomTest04) {
  Random rand;

  int randValue1 = rand.rand();
  int randValue2 = rand.rand();

  int minRange = std::min(randValue1, randValue2);
  int maxRange = std::max(randValue1, randValue2);

  Random randRange(minRange, maxRange);

  int randVal = randRange.rand();
  BOOST_CHECK(minRange <= randVal);
  BOOST_CHECK(randVal <= maxRange);
}

BOOST_AUTO_TEST_CASE(RandomTest05) {
  int minRange = 1;
  int maxRange = 10000000;

  Random randRange1(minRange, maxRange);
  Random randRange2(minRange, maxRange);

  BOOST_CHECK(randRange1.rand() != randRange2.rand());
  BOOST_CHECK(randRange1.rand() != randRange1.rand());
  BOOST_CHECK(randRange2.rand() != randRange2.rand());
}

BOOST_AUTO_TEST_SUITE_END()


/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2009-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_TOOLBOX_LINALG_FUNCTIONS_EXPM_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_FUNCTIONS_EXPM_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <nt2/sdk/meta/size_as.hpp>
#include <nt2/sdk/meta/value_as.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <nt2/sdk/memory/container.hpp>
#include <nt2/sdk/meta/tieable_hierarchy.hpp>
#include <nt2/core/container/dsl/value_type.hpp>

namespace nt2 { namespace tag
  {
    /*!
     * \brief Define the tag expm_ of functor expm
     *        in namespace nt2::tag for toolbox algebra
    **/
    struct expm_ :   ext::tieable_<expm_>
    {
      typedef ext::tieable_<expm_>  parent;
    };
  }
  /**
   * @brief compute exponential of a matricial expression
   *
   * expm(a0) must not be confused with exp(a0) that computes on an
   * elementwise basis the powers of the elements of matrix a0.
   *
   * a0  can be a any square matricial expression
   *
   * @param  a0  Matrix expression or scalar
   *
   * @return a matrix containing e^a1
   **/

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::expm_, expm, 1)

}

namespace nt2 { namespace ext
{
  template<class Domain, int N, class Expr>
  struct  size_of<tag::expm_,Domain,N,Expr>
  {
    typedef typename boost::proto::result_of::child_c<Expr&,0>::value_type  c0_t;
    typedef typename c0_t::extent_type                               result_type;
    BOOST_FORCEINLINE result_type operator()(Expr& e) const
    {
      BOOST_ASSERT_MSG(issquare(boost::proto::child_c<0>(e)),
                       "expm needs a square matrix expression");

      return nt2::extent(boost::proto::child_c<0>(e));
    }
  };

  template<class Domain, int N, class Expr>
  struct  value_type<tag::expm_,Domain,N,Expr>
        : meta::value_as<Expr,0>
  {};
} }
#endif


//
// Created by a.kiryanenko on 3/10/20.
//

#include <iostream>
#include "../SpuUltraGraphAdapter.h"
#include "../SpuUltraGraphProperty.h"
#include <boost/graph/graphviz.hpp>

using namespace std;
using namespace SPU_GRAPH;
using namespace boost;

// Класс для печати свойств вершины для graphvis
class vertex_property_writer {
    const SpuUltraGraph &_g;
public:
    vertex_property_writer(SpuUltraGraph &g) : _g(g) {}
    void operator()(std::ostream& out, const SpuUltraGraph::vertex_descriptor &v) const {
        out << "[label=\"#" << v << "\"]";
    }
};

// Класс для печати свойств ребра для graphvis
class edge_property_writer {
    const SpuUltraGraph &_g;
public:
    edge_property_writer(SpuUltraGraph &g) : _g(g) {}
    void operator()(std::ostream& out, const SpuUltraGraph::edge_descriptor &e) const {
        auto weight = get(edge_weight, _g, e);
        out << "[label=\"#" << get(edge_index, _g, e) << "\",xlabel=\"W" << weight << "\",weight=" << weight << "]";
    }
};

int main()
{
    SpuUltraGraph graph;

    graph.add_vertex(1);
    graph.add_vertex(2);
    graph.add_vertex(3);
    graph.add_vertex(4);
    graph.add_vertex(5);
    graph.add_vertex(6);

    graph.add_edge(graph.get_free_edge_descriptor(2), 1, 2);
    graph.add_edge(graph.get_free_edge_descriptor(1), 1, 3);
    graph.add_edge(graph.get_free_edge_descriptor(8), 1, 6);
    graph.add_edge(graph.get_free_edge_descriptor(1), 2, 2);
    graph.add_edge(graph.get_free_edge_descriptor(1), 2, 3);
    graph.add_edge(graph.get_free_edge_descriptor(2), 2, 4);
    graph.add_edge(graph.get_free_edge_descriptor(5), 3, 4);
    graph.add_edge(graph.get_free_edge_descriptor(6), 3, 5);
    graph.add_edge(graph.get_free_edge_descriptor(3), 4, 6);
    graph.add_edge(graph.get_free_edge_descriptor(7), 5, 6);

    // Распечатать в консоль граф в формате graphviz
    vertex_property_writer vpw(graph);
    edge_property_writer epw(graph);
    write_graphviz(cout, graph, vpw, epw);
    // С помощью GraphViz создадим svg изображение графа
    std::ofstream f("graph.dot");
    boost::write_graphviz(f, graph, vpw, epw);
    f.close();
    system("dot graph.dot -Kcirco -Tsvg -o graph.svg");
    return 0;
}


//  Boost rational.hpp header file  ------------------------------------------//

//  (C) Copyright Paul Moore 1999. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or
//  implied warranty, and with no claim as to its suitability for any purpose.

//  See http://www.boost.org/libs/rational for documentation.

//  Credits:
//  Thanks to the boost mailing list in general for useful comments.
//  Particular contributions included:
//    Andrew D Jewell, for reminding me to take care to avoid overflow
//    Ed Brey, for many comments, including picking up on some dreadful typos
//    Stephen Silver contributed the test suite and comments on user-defined
//    IntType
//    Nickolay Mladenov, for the implementation of operator+=

//  Revision History
//  28 Sep 02  Use _left versions of operators from operators.hpp
//  05 Jul 01  Recode gcd(), avoiding std::swap (Helmut Zeisel)
//  03 Mar 01  Workarounds for Intel C++ 5.0 (David Abrahams)
//  05 Feb 01  Update operator>> to tighten up input syntax
//  05 Feb 01  Final tidy up of gcd code prior to the new release
//  27 Jan 01  Recode abs() without relying on abs(IntType)
//  21 Jan 01  Include Nickolay Mladenov's operator+= algorithm,
//             tidy up a number of areas, use newer features of operators.hpp
//             (reduces space overhead to zero), add operator!,
//             introduce explicit mixed-mode arithmetic operations
//  12 Jan 01  Include fixes to handle a user-defined IntType better
//  19 Nov 00  Throw on divide by zero in operator /= (John (EBo) David)
//  23 Jun 00  Incorporate changes from Mark Rodgers for Borland C++
//  22 Jun 00  Change _MSC_VER to BOOST_MSVC so other compilers are not
//             affected (Beman Dawes)
//   6 Mar 00  Fix operator-= normalization, #include <string> (Jens Maurer)
//  14 Dec 99  Modifications based on comments from the boost list
//  09 Dec 99  Initial Version (Paul Moore)

#ifndef BOOST_RATIONAL_HPP
#define BOOST_RATIONAL_HPP

#include <iostream>              // for std::istream and std::ostream
#include <iomanip>               // for std::noskipws
#include <stdexcept>             // for std::domain_error
#include <string>                // for std::string implicit constructor
#include <boost/operators.hpp>   // for boost::addable etc
#include <cstdlib>               // for std::abs
#include <boost/call_traits.hpp> // for boost::call_traits
#include <boost/config.hpp>      // for BOOST_NO_STDC_NAMESPACE, BOOST_MSVC

namespace boost {

// Note: We use n and m as temporaries in this function, so there is no value
// in using const IntType& as we would only need to make a copy anyway...
template <typename IntType>
IntType gcd(IntType n, IntType m)
{
    // Avoid repeated construction
    IntType zero(0);

    // This is abs() - given the existence of broken compilers with Koenig
    // lookup issues and other problems, I code this explicitly. (Remember,
    // IntType may be a user-defined type).
    if (n < zero)
        n = -n;
    if (m < zero)
        m = -m;

    // As n and m are now positive, we can be sure that %= returns a
    // positive value (the standard guarantees this for built-in types,
    // and we require it of user-defined types).
    for(;;) {
      if(m == zero)
        return n;
      n %= m;
      if(n == zero)
        return m;
      m %= n;
    }
}

template <typename IntType>
IntType lcm(IntType n, IntType m)
{
    // Avoid repeated construction
    IntType zero(0);

    if (n == zero || m == zero)
        return zero;

    n /= gcd(n, m);
    n *= m;

    if (n < zero)
        n = -n;
    return n;
}

class bad_rational : public std::domain_error
{
public:
    explicit bad_rational() : std::domain_error("bad rational: zero denominator") {}
};

template <typename IntType>
class rational;

template <typename IntType>
rational<IntType> abs(const rational<IntType>& r);

template <typename IntType>
class rational :
    less_than_comparable < rational<IntType>,
    equality_comparable < rational<IntType>,
    less_than_comparable2 < rational<IntType>, IntType,
    equality_comparable2 < rational<IntType>, IntType,
    addable < rational<IntType>,
    subtractable < rational<IntType>,
    multipliable < rational<IntType>,
    dividable < rational<IntType>,
    addable2 < rational<IntType>, IntType,
    subtractable2 < rational<IntType>, IntType,
    subtractable2_left < rational<IntType>, IntType,
    multipliable2 < rational<IntType>, IntType,
    dividable2 < rational<IntType>, IntType,
    dividable2_left < rational<IntType>, IntType,
    incrementable < rational<IntType>,
    decrementable < rational<IntType>
    > > > > > > > > > > > > > > > >
{
    typedef IntType int_type;
    typedef typename boost::call_traits<IntType>::param_type param_type;

public:
    rational() : num(0), den(1) {}
    rational(param_type n) : num(n), den(1) {}
    rational(param_type n, param_type d) : num(n), den(d) { normalize(); }

    // Default copy constructor and assignment are fine

    // Add assignment from IntType
    rational& operator=(param_type n) { return assign(n, 1); }

    // Assign in place
    rational& assign(param_type n, param_type d);

    // Access to representation
    IntType numerator() const { return num; }
    IntType denominator() const { return den; }

    // Arithmetic assignment operators
    rational& operator+= (const rational& r);
    rational& operator-= (const rational& r);
    rational& operator*= (const rational& r);
    rational& operator/= (const rational& r);

    rational& operator+= (param_type i);
    rational& operator-= (param_type i);
    rational& operator*= (param_type i);
    rational& operator/= (param_type i);

    // Increment and decrement
    const rational& operator++();
    const rational& operator--();

    // Operator not
    bool operator!() const { return !num; }

    // Comparison operators
    bool operator< (const rational& r) const;
    bool operator== (const rational& r) const;

    bool operator< (param_type i) const;
    bool operator> (param_type i) const;
    bool operator== (param_type i) const;

private:
    // Implementation - numerator and denominator (normalized).
    // Other possibilities - separate whole-part, or sign, fields?
    IntType num;
    IntType den;

    // Representation note: Fractions are kept in normalized form at all
    // times. normalized form is defined as gcd(num,den) == 1 and den > 0.
    // In particular, note that the implementation of abs() below relies
    // on den always being positive.
    void normalize();
};

// Assign in place
template <typename IntType>
inline rational<IntType>& rational<IntType>::assign(param_type n, param_type d)
{
    num = n;
    den = d;
    normalize();
    return *this;
}

// Unary plus and minus
template <typename IntType>
inline rational<IntType> operator+ (const rational<IntType>& r)
{
    return r;
}

template <typename IntType>
inline rational<IntType> operator- (const rational<IntType>& r)
{
    return rational<IntType>(-r.numerator(), r.denominator());
}

// Arithmetic assignment operators
template <typename IntType>
rational<IntType>& rational<IntType>::operator+= (const rational<IntType>& r)
{
    // This calculation avoids overflow, and minimises the number of expensive
    // calculations. Thanks to Nickolay Mladenov for this algorithm.
    //
    // Proof:
    // We have to compute a/b + c/d, where gcd(a,b)=1 and gcd(b,c)=1.
    // Let g = gcd(b,d), and b = b1*g, d=d1*g. Then gcd(b1,d1)=1
    //
    // The result is (a*d1 + c*b1) / (b1*d1*g).
    // Now we have to normalize this ratio.
    // Let's assume h | gcd((a*d1 + c*b1), (b1*d1*g)), and h > 1
    // If h | b1 then gcd(h,d1)=1 and hence h|(a*d1+c*b1) => h|a.
    // But since gcd(a,b1)=1 we have h=1.
    // Similarly h|d1 leads to h=1.
    // So we have that h | gcd((a*d1 + c*b1) , (b1*d1*g)) => h|g
    // Finally we have gcd((a*d1 + c*b1), (b1*d1*g)) = gcd((a*d1 + c*b1), g)
    // Which proves that instead of normalizing the result, it is better to
    // divide num and den by gcd((a*d1 + c*b1), g)

    // Protect against self-modification
    IntType r_num = r.num;
    IntType r_den = r.den;

    IntType g = gcd(den, r_den);
    den /= g;  // = b1 from the calculations above
    num = num * (r_den / g) + r_num * den;
    g = gcd(num, g);
    num /= g;
    den *= r_den/g;

    return *this;
}

template <typename IntType>
rational<IntType>& rational<IntType>::operator-= (const rational<IntType>& r)
{
    // Protect against self-modification
    IntType r_num = r.num;
    IntType r_den = r.den;

    // This calculation avoids overflow, and minimises the number of expensive
    // calculations. It corresponds exactly to the += case above
    IntType g = gcd(den, r_den);
    den /= g;
    num = num * (r_den / g) - r_num * den;
    g = gcd(num, g);
    num /= g;
    den *= r_den/g;

    return *this;
}

template <typename IntType>
rational<IntType>& rational<IntType>::operator*= (const rational<IntType>& r)
{
    // Protect against self-modification
    IntType r_num = r.num;
    IntType r_den = r.den;

    // Avoid overflow and preserve normalization
    IntType gcd1 = gcd<IntType>(num, r_den);
    IntType gcd2 = gcd<IntType>(r_num, den);
    num = (num/gcd1) * (r_num/gcd2);
    den = (den/gcd2) * (r_den/gcd1);
    return *this;
}

template <typename IntType>
rational<IntType>& rational<IntType>::operator/= (const rational<IntType>& r)
{
    // Protect against self-modification
    IntType r_num = r.num;
    IntType r_den = r.den;

    // Avoid repeated construction
    IntType zero(0);

    // Trap division by zero
    if (r_num == zero)
        throw bad_rational();
    if (num == zero)
        return *this;

    // Avoid overflow and preserve normalization
    IntType gcd1 = gcd<IntType>(num, r_num);
    IntType gcd2 = gcd<IntType>(r_den, den);
    num = (num/gcd1) * (r_den/gcd2);
    den = (den/gcd2) * (r_num/gcd1);

    if (den < zero) {
        num = -num;
        den = -den;
    }
    return *this;
}

// Mixed-mode operators
template <typename IntType>
inline rational<IntType>&
rational<IntType>::operator+= (param_type i)
{
    return operator+= (rational<IntType>(i));
}

template <typename IntType>
inline rational<IntType>&
rational<IntType>::operator-= (param_type i)
{
    return operator-= (rational<IntType>(i));
}

template <typename IntType>
inline rational<IntType>&
rational<IntType>::operator*= (param_type i)
{
    return operator*= (rational<IntType>(i));
}

template <typename IntType>
inline rational<IntType>&
rational<IntType>::operator/= (param_type i)
{
    return operator/= (rational<IntType>(i));
}

// Increment and decrement
template <typename IntType>
inline const rational<IntType>& rational<IntType>::operator++()
{
    // This can never denormalise the fraction
    num += den;
    return *this;
}

template <typename IntType>
inline const rational<IntType>& rational<IntType>::operator--()
{
    // This can never denormalise the fraction
    num -= den;
    return *this;
}

// Comparison operators
template <typename IntType>
bool rational<IntType>::operator< (const rational<IntType>& r) const
{
    // Avoid repeated construction
    IntType zero(0);

    // If the two values have different signs, we don't need to do the
    // expensive calculations below. We take advantage here of the fact
    // that the denominator is always positive.
    if (num < zero && r.num >= zero) // -ve < +ve
        return true;
    if (num >= zero && r.num <= zero) // +ve or zero is not < -ve or zero
        return false;

    // Avoid overflow
    IntType gcd1 = gcd<IntType>(num, r.num);
    IntType gcd2 = gcd<IntType>(r.den, den);
    return (num/gcd1) * (r.den/gcd2) < (den/gcd2) * (r.num/gcd1);
}

template <typename IntType>
bool rational<IntType>::operator< (param_type i) const
{
    // Avoid repeated construction
    IntType zero(0);

    // If the two values have different signs, we don't need to do the
    // expensive calculations below. We take advantage here of the fact
    // that the denominator is always positive.
    if (num < zero && i >= zero) // -ve < +ve
        return true;
    if (num >= zero && i <= zero) // +ve or zero is not < -ve or zero
        return false;

    // Now, use the fact that n/d truncates towards zero as long as n and d
    // are both positive.
    // Divide instead of multiplying to avoid overflow issues. Of course,
    // division may be slower, but accuracy is more important than speed...
    if (num > zero)
        return (num/den) < i;
    else
        return -i < (-num/den);
}

template <typename IntType>
bool rational<IntType>::operator> (param_type i) const
{
    // Trap equality first
    if (num == i && den == IntType(1))
        return false;

    // Otherwise, we can use operator<
    return !operator<(i);
}

template <typename IntType>
inline bool rational<IntType>::operator== (const rational<IntType>& r) const
{
    return ((num == r.num) && (den == r.den));
}

template <typename IntType>
inline bool rational<IntType>::operator== (param_type i) const
{
    return ((den == IntType(1)) && (num == i));
}

// Normalisation
template <typename IntType>
void rational<IntType>::normalize()
{
    // Avoid repeated construction
    IntType zero(0);

    if (den == zero)
        throw bad_rational();

    // Handle the case of zero separately, to avoid division by zero
    if (num == zero) {
        den = IntType(1);
        return;
    }

    IntType g = gcd<IntType>(num, den);

    num /= g;
    den /= g;

    // Ensure that the denominator is positive
    if (den < zero) {
        num = -num;
        den = -den;
    }
}

namespace detail {

    // A utility class to reset the format flags for an istream at end
    // of scope, even in case of exceptions
    struct resetter {
        resetter(std::istream& is) : is_(is), f_(is.flags()) {}
        ~resetter() { is_.flags(f_); }
        std::istream& is_;
        std::istream::fmtflags f_;      // old GNU c++ lib has no ios_base
    };

}

// Input and output
template <typename IntType>
std::istream& operator>> (std::istream& is, rational<IntType>& r)
{
    IntType n = IntType(0), d = IntType(1);
    char c = 0;
    detail::resetter sentry(is);

    is >> n;
    c = is.get();

    if (c != '/')
        is.clear(std::istream::badbit);  // old GNU c++ lib has no ios_base

#if !defined(__GNUC__) || (defined(__GNUC__) && (__GNUC__ >= 3)) || defined __SGI_STL_PORT
    is >> std::noskipws;
#else
    is.unsetf(ios::skipws); // compiles, but seems to have no effect.
#endif
    is >> d;

    if (is)
        r.assign(n, d);

    return is;
}

// Add manipulators for output format?
template <typename IntType>
std::ostream& operator<< (std::ostream& os, const rational<IntType>& r)
{
    os << r.numerator() << '/' << r.denominator();
    return os;
}

// Type conversion
template <typename T, typename IntType>
inline T rational_cast(const rational<IntType>& src)
{
    return static_cast<T>(src.numerator())/src.denominator();
}

// Do not use any abs() defined on IntType - it isn't worth it, given the
// difficulties involved (Koenig lookup required, there may not *be* an abs()
// defined, etc etc).
template <typename IntType>
inline rational<IntType> abs(const rational<IntType>& r)
{
    if (r.numerator() >= IntType(0))
        return r;

    return rational<IntType>(-r.numerator(), r.denominator());
}

} // namespace boost

#endif  // BOOST_RATIONAL_HPP


#include "mesh.h"

#include <array>
#include <algorithm>
#include <fstream>
#include <limits>
#include <random>

#include <igl/readOBJ.h>
#include <igl/writeOBJ.h>
#include <igl/grad.h>
#include <igl/cotmatrix.h>
#include <igl/colormap.h>
#include <igl/eigs.h>
#include <igl/unproject_onto_mesh.h>
#include <igl/adjacency_matrix.h>
#include <igl/triangle_triangle_adjacency.h>
#include <igl/per_vertex_normals.h>
#include <igl/arap.h>
#include <igl/boundary_loop.h>
#include <igl/harmonic.h>
#include <igl/map_vertices_to_circle.h>

#include <Eigen/Eigenvalues>

#define STB_IMAGE_WRITE_IMPLEMENTATION
#include <stb/stb_image.h>
#include <stb/stb_image_write.h>

#include <geometry/patch.h>
#include <matching/geodesic_fan.h>
#include <shape_signatures/heat_kernel_signature.h>
#include <matching/surface_stroke.h>
#include <matching/parameterization/curve_unrolling.h>

struct MeshInstancer : public Mesh, std::enable_shared_from_this<Mesh> {
	MeshInstancer(std::string obj_path) : Mesh(obj_path) { }
	MeshInstancer(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F): Mesh(V, F) { }
};

Mesh::Mesh(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F) :
	_V(Eigen::MatrixXd::Zero(0, 0)),
	_TC(Eigen::MatrixXd::Zero(0, 0)),
	_N(Eigen::MatrixXd::Zero(0, 0)),
	_F(Eigen::MatrixXi::Zero(0, 0)),
	_FTC(Eigen::MatrixXi::Zero(0, 0)),
	_FN(Eigen::MatrixXd::Zero(0, 0)),
	_VN(Eigen::MatrixXd::Zero(0, 0)),
	_adj(Eigen::SparseMatrix<int>(0, 0)),
	_base_colors(Eigen::MatrixXd::Zero(0, 0)),
	_face_colors(Eigen::MatrixXd::Zero(0, 0)),
	_loaded(false),
	_model_name(""),
	_data_dir(""),
	_selection_patch(nullptr),
	_displayed_points(Eigen::MatrixXd::Zero(0, 6)) {

	std::get<0>(_color_texture) = std::make_shared<ColorChannel>();
	std::get<1>(_color_texture) = std::make_shared<ColorChannel>();
	std::get<2>(_color_texture) = std::make_shared<ColorChannel>();
	std::get<3>(_color_texture) = std::make_shared<ColorChannel>();

	_V = V;
	_F = F;

	// Compute normals
	igl::per_vertex_normals(_V, _F, _VN);
	igl::per_face_normals(_V, _F, _FN);

	_VN.rowwise().normalize();
	_FN.rowwise().normalize();

	// Create blank color map
	_base_colors = Eigen::MatrixXd::Constant(_F.rows(), 3, 1.0);
	_face_colors = _base_colors;

	// Create adjacency matrix
	igl::adjacency_matrix(_F, _adj);
	igl::triangle_triangle_adjacency(_F, _tri_adj);

	_VC = _V;
	_FC = _F;
	_face_colors_C = _face_colors;
}

Mesh::Mesh(std::string obj_path) :
	_V(Eigen::MatrixXd::Zero(0, 0)),
	_TC(Eigen::MatrixXd::Zero(0, 0)),
	_N(Eigen::MatrixXd::Zero(0, 0)),
	_F(Eigen::MatrixXi::Zero(0, 0)),
	_FTC(Eigen::MatrixXi::Zero(0, 0)),
	_FN(Eigen::MatrixXd::Zero(0, 0)),
	_VN(Eigen::MatrixXd::Zero(0, 0)),
	_adj(Eigen::SparseMatrix<int>(0,0)),
	_base_colors(Eigen::MatrixXd::Zero(0, 0)),
	_face_colors(Eigen::MatrixXd::Zero(0, 0)),
	_loaded(false),
	_model_name(""),
	_data_dir(""),
	_selection_patch(nullptr),
	_displayed_points(Eigen::MatrixXd::Zero(0,6)) {

	std::get<0>(_color_texture) = std::make_shared<ColorChannel>();
	std::get<1>(_color_texture) = std::make_shared<ColorChannel>();
	std::get<2>(_color_texture) = std::make_shared<ColorChannel>();
	std::get<3>(_color_texture) = std::make_shared<ColorChannel>();

	if (!(_loaded = load_obj(obj_path))) {
		//throw std::invalid_argument("Could not load model [ " + obj_path + " ]!\n");
		return;
	}

	_model_path = obj_path;

	// Scale Vertices to unit cube
	double scale_factor = _V.leftCols<3>().maxCoeff() / 2.0;
	_V.leftCols<3>() /= scale_factor;
	_V.leftCols<3>().rowwise() - Eigen::Vector3d(1.0, 1.0, 1.0).transpose();

	// Create blank color map
	_base_colors = Eigen::MatrixXd::Constant(_F.rows(), 3, 1.0);
	_face_colors = _base_colors;

	// Create adjacency matrix
	igl::adjacency_matrix(_F, _adj);
	igl::triangle_triangle_adjacency(_F, _tri_adj);

	// Right now only supporting triangle meshes
	if (_F.cols() == 3) {
		// Create halfedge structure
		std::vector<trimesh::triangle_t> triangles;
		triangles.resize(_F.rows());

		for (Eigen::DenseIndex i = 0; i < _F.rows(); ++i) {
			for (Eigen::DenseIndex j = 0; j < 3; ++j) {
				triangles[i].v[j] = _F(i, j);
			}
		}

		std::vector<trimesh::edge_t> edges;
		trimesh::unordered_edges_from_triangles(triangles.size(), &triangles[0], edges);

		halfedge_mesh.build(_V.rows(), triangles.size(), &triangles[0], edges.size(), &edges[0]);
	}

	_VC = _V;
	_FC = _F;
	_face_colors_C = _face_colors;
}

Mesh::~Mesh() {
}

std::shared_ptr<Mesh> Mesh::instantiate(std::string obj_path) {
	return std::make_shared<MeshInstancer>(obj_path);
}

std::shared_ptr<Mesh> Mesh::instantiate(const Eigen::MatrixXd& V, const Eigen::MatrixXi& F) {
	return std::make_shared<MeshInstancer>(V, F);
}

const trimesh::trimesh_t& Mesh::halfedge() {
	return halfedge_mesh;
}

void Mesh::display(igl::opengl::glfw::Viewer& viewer) {
	if (_V.cols() == 3) {
		viewer.data().set_mesh(_V.block(0, 0, _V.rows(), 3), _F);

		if (std::get<0>(_color_texture)->cols() > 0 &&
			std::get<1>(_color_texture)->cols() > 0 &&
			std::get<2>(_color_texture)->cols() >0 &&
			std::get<0>(_color_texture)->cols() == std::get<1>(_color_texture)->cols() &&
			std::get<1>(_color_texture)->cols() == std::get<2>(_color_texture)->cols()) {
			if (_TC.cols() > 0 && _FTC.cols() > 0) {
				viewer.data().set_uv(_TC, _FTC);
			}

			viewer.data().set_texture(*std::get<0>(_color_texture), *std::get<1>(_color_texture), *std::get<2>(_color_texture));
			viewer.data().show_texture = true;
		} else {
			viewer.data().set_colors(_face_colors);
		}
	} else if (_V.cols() == 6) {
		viewer.data().set_mesh(_V.block(0, 0, _V.rows(), 3), _F);
		viewer.data().set_colors(_V.block(0, 3, _V.rows(), 3));
	}

	viewer.core.align_camera_center(_V.block(0, 0, _V.rows(), 3), _F);
}

bool Mesh::set_scalar_vertex_color(igl::opengl::glfw::Viewer& viewer, const Eigen::MatrixXd& color_per_vertex, igl::ColorMapType cm) {
	return set_scalar_vertex_color(viewer, color_per_vertex, color_per_vertex.minCoeff(), color_per_vertex.maxCoeff(), cm);
}

bool Mesh::set_scalar_vertex_color(igl::opengl::glfw::Viewer& viewer, const Eigen::MatrixXd& color_per_vertex, double z_min, double z_max, igl::ColorMapType cm) {
	// Defaults to use inferno for now
	Eigen::MatrixXd per_vert_colors;

	igl::colormap(cm, Eigen::VectorXd(color_per_vertex.col(0)), z_min, z_max, per_vert_colors);

	if (std::fabs(per_vert_colors.maxCoeff() - per_vert_colors.minCoeff()) > 1e-7) {
		per_vert_colors = (per_vert_colors.array() - per_vert_colors.minCoeff());
		per_vert_colors /= per_vert_colors.maxCoeff();

		double max_coeff = per_vert_colors.maxCoeff();
		double min_coeff = per_vert_colors.minCoeff();

		assert(std::fabs(per_vert_colors.maxCoeff() - 1.0) < 1e-7 && std::fabs(per_vert_colors.minCoeff()) < 1e-7);
	}

	// Sum up vertex colors into a face color
	_base_colors = Eigen::MatrixXd::Zero(_F.rows(), 3);

	for (Eigen::DenseIndex i = 0; i < _base_colors.rows(); ++i) {
		for (Eigen::DenseIndex j = 0; j < _F.cols(); ++j) {
			_base_colors.row(i) += per_vert_colors.row(_F(i, j)) / _FC.cols();
		}
	}

	refresh_colormap();

	viewer.data().set_colors(_face_colors_C);

	return true;
}

bool Mesh::select_vid_with_point(igl::opengl::glfw::Viewer& viewer, Eigen::DenseIndex vid, Eigen::Vector3d color) {
	if (_V.rows() <= vid) {
		return false;
	}

	viewer.data().points = _displayed_points;
	viewer.data().add_points(_V.block(vid, 0, 1, 3), color.transpose());

	return true;
}

bool Mesh::write_obj(std::string path) {
	return igl::writeOBJ(path, _VC, _FC);
}

bool Mesh::load_obj(std::string objPath) {
	Eigen::MatrixXi FN;

	if (!igl::readOBJ(objPath, _V, _TC, _N, _F, _FTC, FN)) {
		return false;
	}

	if (FN.rows() <= 0 || FN.cols() <= 0) {
		// Compute normals since the file didn't include them
		igl::per_vertex_normals(_V, _F, _VN);
		igl::per_face_normals(_V, _F, _FN);
	} else {
		_N.rowwise().normalize();
		_VN = Eigen::MatrixXd::Zero(_V.rows(), 3);

		for (Eigen::DenseIndex i = 0; i < _F.rows(); ++i) {
			for (Eigen::DenseIndex j = 0; j < _F.cols(); ++j) {
				_VN.row(_F(i, j)) += _N.row(FN(i, j));
			}
		}

		_FN = _N;
	}

	_VN.rowwise().normalize();
	_FN.rowwise().normalize();

	std::replace(objPath.begin(), objPath.end(), '\\', '/');
	_model_name = objPath.substr(objPath.find_last_of('/') + 1, objPath.find_last_of('.'));
	_data_dir = objPath.substr(0, objPath.find_last_of('/') + 1);

	return true;
}

bool Mesh::load_texture(std::string texture_path) {
	int w, h, n;
	unsigned char *data = stbi_load(texture_path.c_str(), &w, &h, &n, 4);
	if (data == NULL) {
		return false;
	}

	int comp = 4;

	std::get<0>(_color_texture) = std::make_shared<ColorChannel>(w, h);
	std::get<1>(_color_texture) = std::make_shared<ColorChannel>(w, h);
	std::get<2>(_color_texture) = std::make_shared<ColorChannel>(w, h);
	std::get<3>(_color_texture) = std::make_shared<ColorChannel>(w, h);

	for (unsigned y = 0; y < h; ++y) {
		for (unsigned x = 0; x < w; ++x) {
			(*std::get<0>(_color_texture))(x, h - 1 - y) = data[comp * (x + w * y) + 0];
			(*std::get<1>(_color_texture))(x, h - 1 - y) = data[comp * (x + w * y) + 1];
			(*std::get<2>(_color_texture))(x, h - 1 - y) = data[comp * (x + w * y) + 2];
			(*std::get<3>(_color_texture))(x, h - 1 - y) = data[comp * (x + w * y) + 3];
		}
	}

	stbi_image_free(data);

	return true;
}

bool Mesh::save_texture(std::string texture_path) {
	int w = std::get<0>(_color_texture)->cols();
	int h = std::get<0>(_color_texture)->rows();
	int comp = 4;

	unsigned char* data = new unsigned char[w * h * 4];

	for (int y = 0; y < h; ++y) {
		for (int x = 0; x < w; ++x) {
			int i = comp * (x + w * y);
			data[i + 0] = (*std::get<0>(_color_texture))(x, h - 1 - y);
			data[i + 1] = (*std::get<1>(_color_texture))(x, h - 1 - y);
			data[i + 2] = (*std::get<2>(_color_texture))(x, h - 1 - y);
			data[i + 3] = (*std::get<3>(_color_texture))(x, h - 1 - y);
		}
	}

	stbi_write_png(texture_path.c_str(), w, h, comp, data, 0);

	delete data;

	return true;
}

int Mesh::closest_vertex_id(int fid, Eigen::Vector3f bc) {
	int vid;

	bc.maxCoeff(&vid);

	return _F(fid, vid);
}

bool Mesh::select_faces_with_color(igl::opengl::glfw::Viewer& viewer, std::set<Eigen::DenseIndex> fids, Eigen::Vector3d color) {
	for (auto fid : fids) {
		if (_face_colors_C.rows() <= fid) {
			return false;
		}
	}

	if (_selection_patch == nullptr) {
		std::shared_ptr<Mesh> mesh = dynamic_cast<MeshInstancer*>(this)->shared_from_this();
		_selection_patch = Patch::instantiate(mesh);
	}

	for (auto fid : fids) {
		_face_colors_C.row(fid) << color.transpose();
	}

	_selection_patch->add(fids);

	viewer.data().set_colors(_face_colors_C);

	return true;
}

bool Mesh::color_faces(igl::opengl::glfw::Viewer& viewer, std::vector<int> fids, std::vector<Eigen::Vector3d> colors) {
	for (std::size_t i = 0; i < std::min(fids.size(), colors.size()); ++i) {
		if (_face_colors_C.rows() <= fids[i]) {
			continue;
		}

		_face_colors_C.row(fids[i]) << colors[i].transpose();
	}

	viewer.data().set_colors(_face_colors_C);

	return true;
}

bool Mesh::select_contiguous_face(igl::opengl::glfw::Viewer& viewer, int fid) {
	// For a face to be contiguous with a patch, the face and the patch much share at least one edge
	// The best approach I can come with given the structures at hand is to find all common vertices between the patch and face,
	// then see if any have an edge between them by checking the adjacency matrix
	if (_selection_patch == nullptr) {
		std::shared_ptr<Mesh> mesh = dynamic_cast<MeshInstancer*>(this)->shared_from_this();
		_selection_patch = Patch::instantiate(mesh);
	}

	const std::set<Eigen::DenseIndex>& vids = _selection_patch->vids();

	bool contiguous = false;

	if (vids.size() == 0) {
		contiguous = true;
	} else {
		std::vector<Eigen::DenseIndex> shared_vertices;
		for (Eigen::DenseIndex i = 0; i < _F.cols(); ++i) {
			if (vids.count(_F(fid, i)) > 0) {
				shared_vertices.push_back(_F(fid, i));
			}
		}

		for (Eigen::DenseIndex i = 0; i < shared_vertices.size(); ++i) {
			for (Eigen::DenseIndex j = 0; j < shared_vertices.size(); ++j) {
				if (i == j) {
					continue;
				}

				if (_adj.coeff(shared_vertices[i], shared_vertices[j]) != 0) {
					contiguous = true;
					break;
				}
			}

			if (contiguous) {
				break;
			}
		}
	}

	if (contiguous) {
		std::set<Eigen::DenseIndex> fids = { fid };
		select_faces_with_color(viewer, fids);
	}

	return contiguous;
}

bool Mesh::select_patch(igl::opengl::glfw::Viewer& viewer, std::shared_ptr<Patch> patch, Eigen::Vector4d color) {
	_displayed_patches.push_back(std::pair<std::shared_ptr<Patch>, Eigen::Vector4d>(patch, color));

	bool refreshed = refresh_colormap();

	viewer.data().set_colors(_face_colors);

	return refreshed;
}

bool Mesh::select_vertex(igl::opengl::glfw::Viewer& viewer, Eigen::DenseIndex vid, Eigen::Vector3d color) {
	_displayed_points.conservativeResize(_displayed_points.rows() + 1, _displayed_points.cols());

	_displayed_points.row(_displayed_points.rows() - 1) << _V.block<1, 3>(vid, 0), color.transpose();

	viewer.data().points.resize(0, 0);

	for (Eigen::DenseIndex i = 0; i < _displayed_points.rows(); ++i) {
		viewer.data().add_points(_displayed_points.block<1, 3>(i, 0), _displayed_points.block<1, 3>(i, 3));
	}

	return true;
}

bool Mesh::select_curve_cover(igl::opengl::glfw::Viewer& viewer, Eigen::DenseIndex fid) {
	if (fid < _F.rows() || fid >= _FC.rows()) {
		return false;
	}

	int cIndex = -1;

	for (int i = 0; i < _curve_face_indices.size(); ++i) {
		if (fid < _curve_face_indices[i].first) {
			break;
		}

		++cIndex;
	}

	if (cIndex < 0) {
		return false;
	}

	auto fids = _curve_face_indices[cIndex].second->fids();

	return select_faces_with_color(viewer, fids);
}

bool Mesh::deselect_all(igl::opengl::glfw::Viewer& viewer) {
	_face_colors = _base_colors;

	_displayed_patches.clear();
	_displayed_points.resize(0, 6);

	viewer.data().points.resize(0, 0);

	_VC = _V;
	_FC = _F;
	_face_colors_C = _face_colors;
	_curve_face_indices.clear();

	viewer.data().clear();
	viewer.data().set_mesh(_VC.block(0, 0, _VC.rows(), 3), _FC);
	viewer.data().set_colors(_face_colors_C);

	return true;
}

std::shared_ptr<Patch> Mesh::emit_selected_as_patch() {
	std::random_device rd;
	std::mt19937 e2(rd());
	std::uniform_real_distribution<> dist(0.4, 1);

	auto ret = _selection_patch;

	if (_selection_patch != nullptr) {
		Eigen::Vector4d color = { dist(e2), dist(e2), dist(e2), 1.0 };
		_displayed_patches.push_back(std::pair<std::shared_ptr<Patch>, Eigen::Vector4d>(_selection_patch, color));
	}

	std::shared_ptr<Mesh> mesh = dynamic_cast<MeshInstancer*>(this)->shared_from_this();
	_selection_patch = Patch::instantiate(mesh);

	refresh_colormap();

	return ret;
}

bool Mesh::match_active_selection(igl::opengl::glfw::Viewer& viewer, const std::shared_ptr<ShapeSignature> signature, double t_scale) {
	const Eigen::MatrixXd& sig_values = signature->get_signature_values();

	unsigned int rows = sig_values.rows();
	unsigned int cols = std::max(1.0, std::floor(sig_values.cols() * t_scale));
	Eigen::MatrixXd features = sig_values.block(0, 0, rows, cols);

	Eigen::MatrixXd matches;

	std::shared_ptr<Patch> user_selection = emit_selected_as_patch();
	Eigen::DenseIndex feature_vid;
	std::vector<Eigen::DenseIndex> feature_matches;
	std::vector<std::shared_ptr<Patch>> similar = user_selection->find_similar(signature, feature_vid, feature_matches);

	if (similar.size() == 0) {
		return false;
	}

	// Visualize matches
	std::random_device rd;
	std::mt19937 e2(rd());
	std::uniform_real_distribution<> dist(0.4, 1);

	unsigned int patch_nbr = 0;
	for (auto it = similar.begin(); it != similar.end(); ++it) {
		Eigen::Vector4d color = { 0.0, 0.0, dist(e2), 1.0 };

		_displayed_patches.push_back(std::pair<std::shared_ptr<Patch>, Eigen::Vector4d>(*it, color));

		Eigen::DenseIndex patch_center_vid = feature_matches[patch_nbr++];

		viewer.data().add_points(_V.block(patch_center_vid, 0, 1, 3), color.transpose());
	}

	Eigen::Vector3d feature_color = { 0.2, 0.8, 0.2 };
	viewer.data().add_points(_V.block(feature_vid, 0, 1, 3), feature_color.transpose());

	refresh_colormap();

	return true;
}

// TODO: This needs to be refactored as a separate mesh to be display -- does libigl support multi-mesh rendering?? I believe there's a pending PR.
bool Mesh::show_patch_map(igl::opengl::glfw::Viewer& viewer, const std::shared_ptr<ShapeSignature> signature) {
	//std::shared_ptr<Patch> user_selection = emit_selected_as_patch();
	Eigen::VectorXi fids = Eigen::VectorXi::LinSpaced(_F.rows(), 0, _F.rows() - 1);
	std::shared_ptr<Patch> user_selection = Patch::instantiate(dynamic_cast<MeshInstancer*>(this)->shared_from_this(), fids);

	std::vector<Eigen::Vector3d> shared;

	Eigen::DenseIndex centroid_vid = user_selection->get_centroid_vid_on_origin_mesh();

	if (centroid_vid < 0) {
		return false;
	}

	std::shared_ptr<DiscreteExponentialMap> dem = user_selection->discrete_exponential_map(centroid_vid);

	std::stringstream ss; ss << resource_dir() << "//matlab//" << name().substr(0, name().find_first_of('.')) << "_dem.m";
	dem->to_matlab(ss.str());

	Eigen::MatrixXd V_dem = dem->get_3d_vertices();
	Eigen::MatrixXi F_dem = dem->get_reindexed_faces();

	F_dem = F_dem.array() + _V.rows();

	// In case of nD-vertex data
	V_dem.conservativeResize(V_dem.rows(), _V.cols());
	V_dem = V_dem.rowwise() + _V.row(centroid_vid);

	Eigen::MatrixXd V_all(_V.rows() + V_dem.rows(), _V.cols());
	Eigen::MatrixXi F_all(_F.rows() + F_dem.rows(), _F.cols());;
	Eigen::MatrixXd C_all(_face_colors.rows() + F_dem.rows(), _face_colors.cols());

	V_all << _V, V_dem;
	F_all << _F, F_dem;
	C_all << _face_colors, Eigen::MatrixXd::Ones(F_dem.rows(), _face_colors.cols());

	viewer.data().clear();
	viewer.data().set_mesh(V_all.block(0, 0, V_all.rows(), 3), F_all);
	viewer.data().set_colors(C_all);

	return true;
}

bool Mesh::show_patch_map(igl::opengl::glfw::Viewer& viewer, bool draw_points, const std::shared_ptr<SurfaceStroke> stroke, std::shared_ptr<ShapeSignature> signature) {
	//std::shared_ptr<Patch> user_selection = stroke->cover_patch();
	//std::shared_ptr<DiscreteExponentialMap> dem = stroke->cover_patch_local_dem(user_selection);

	if (stroke->blade_points().size() <= 0) {
		return false;
	}

	std::vector<CurveUnrolling> unrolled;
	unrolled.emplace_back(CurveUnrolling(stroke));

	// Pick a random face, then an arbitrary vid in the face
	std::random_device r;
	std::default_random_engine e1(r());
	std::uniform_int_distribution<int> uniform_dist(0, stroke->origin_mesh()->faces().rows() - 1);
	Eigen::DenseIndex fid = static_cast<Eigen::DenseIndex>(uniform_dist(e1));

	unrolled.emplace_back(CurveUnrolling(stroke->origin_mesh(), unrolled[0], fid));

	Eigen::MatrixXd V = _V;
	Eigen::MatrixXi F = _F;
	auto face_colors = _face_colors;

	//std::vector<Eigen::Vector3d> shared;
	for (auto cu = unrolled.begin(); cu != unrolled.end(); ++cu) {
		Eigen::DenseIndex centroid_vid = cu->vid_map().at(0);

		if (centroid_vid < 0) {
			return false;
		}

		Eigen::MatrixXd V_dem = cu->vertices();
		Eigen::MatrixXi F_dem = cu->faces();
		Eigen::MatrixXd C_dem = Eigen::MatrixXd::Ones(F_dem.rows(), face_colors.cols());

		F_dem = F_dem.array() + V.rows();

		Eigen::Vector3d normal = cu->frame().col(2);
		Eigen::Vector3d tangent = cu->frame().col(0);
		Eigen::Vector3d bitangent = cu->frame().col(1);
		Eigen::Vector3d pt = V.row(centroid_vid).block<1, 3>(0, 0).transpose() + normal;

		// In case of nD-vertex data
		Eigen::VectorXd nd_pt = Eigen::VectorXd::Zero(V.cols());
		nd_pt.topRows(pt.size()) << pt;

		V_dem.conservativeResize(V_dem.rows(), V.cols());
		V_dem = V_dem.rowwise() + nd_pt.transpose();

		Eigen::MatrixXd V_all(V.rows() + V_dem.rows(), V.cols());
		Eigen::MatrixXi F_all(F.rows() + F_dem.rows(), F.cols());;
		Eigen::MatrixXd C_all(face_colors.rows() + F_dem.rows(), face_colors.cols());

		V_all << V, V_dem;
		F_all << F, F_dem;
		C_all << face_colors, C_dem;

		V = V_all;
		F = F_all;
		face_colors = C_all;

		// TODO: There is a bug where the appended faces draw vertices well outside of the vertex matrix
		//		 The face matrix needs to be reindexed
		// CLUE: It seems to be related to patch connectivity -- a patch must be fully connected in order to extra a DEM from it, or else it should be represented by two patches (disjoint shatter)
		assert(F_all.maxCoeff() < V_all.rows());

		viewer.data().clear();

		viewer.data().set_mesh(V_all.block(0, 0, V_all.rows(), 3), F_all);
		viewer.data().set_colors(C_all);

		/*viewer.data().add_edges(pt.transpose(), (pt + normal).transpose(), Eigen::RowVector3d(1.0, 0.0, 0.0));
		viewer.data().add_edges(pt.transpose(), (pt + tangent).transpose(), Eigen::RowVector3d(0.0, 1.0, 0.0));
		viewer.data().add_edges(pt.transpose(), (pt + bitangent).transpose(), Eigen::RowVector3d(0.0, 0.0, 1.0));*/

		cu->unrolled_stroke()->display(viewer, draw_points, cu->parameterized_mesh(), false, nd_pt.topRows<3>());
	}

	return true;
}

bool Mesh::add_geometry(igl::opengl::glfw::Viewer& viewer, Eigen::MatrixXd& V, Eigen::MatrixXi& F, std::shared_ptr<SurfaceStroke> stroke) {
	Eigen::MatrixXd C_dem = Eigen::MatrixXd::Ones(F.rows(), _face_colors_C.cols());

	F = F.array() + _VC.rows();

	// In case of nD-vertex data
	V.conservativeResize(V.rows(), _VC.cols());
	//V_dem = V_dem.rowwise() + nd_pt.transpose();

	Eigen::MatrixXd V_all(_VC.rows() + V.rows(), _VC.cols());
	Eigen::MatrixXi F_all(_FC.rows() + F.rows(), _FC.cols());;
	Eigen::MatrixXd C_all(_face_colors_C.rows() + F.rows(), _face_colors_C.cols());

	if (stroke != nullptr) {
		_curve_face_indices.push_back(std::make_pair(_FC.rows(), stroke));
	}

	V_all << _VC, V;
	F_all << _FC, F;
	C_all << _face_colors_C, C_dem;

	_VC = V_all;
	_FC = F_all;
	_face_colors_C = C_all;

	// TODO: There is a bug where the appended faces draw vertices well outside of the vertex matrix
	//		 The face matrix needs to be reindexed
	// CLUE: It seems to be related to patch connectivity -- a patch must be fully connected in order to extra a DEM from it, or else it should be represented by two patches (disjoint shatter)
	assert(F_all.maxCoeff() < V_all.rows());
	assert(C_all.rows() == F_all.rows());

	viewer.data().clear();
	viewer.data().set_mesh(V_all.block(0, 0, V_all.rows(), 3), F_all);
	viewer.data().set_colors(C_all);

	return true;
}

std::vector<std::string> Mesh::calc_obj_vertex_colors() {
	std::vector<std::vector<std::array<unsigned char, 3>>> VC;
	VC.resize(_V.rows());

	unsigned int height = std::get<0>(_color_texture)->cols();
	unsigned int width = std::get<0>(_color_texture)->rows();

	for (unsigned int i = 0; i < _FTC.rows(); ++i) {
		for (unsigned int j = 0; j < _FTC.cols(); ++j) {
			// Clamp UV [0,1]
			double u = std::min(1.0, std::max(0.0, _TC(_FTC(i, j), 0)));
			double v = std::min(1.0, std::max(0.0, _TC(_FTC(i, j), 1)));

			unsigned int imgX = static_cast<unsigned int>(u * (width - 1));
			unsigned int imgY = static_cast<unsigned int>(v * (height - 1));

			std::array<unsigned char, 3> color{
				(*std::get<0>(_color_texture))(imgY, imgX),
				(*std::get<1>(_color_texture))(imgY, imgX),
				(*std::get<2>(_color_texture))(imgY, imgX)
			};

			VC[_F(i, j)].push_back(color);
		}
	}

	std::vector<std::array<unsigned char, 3>> bVC;

	for (auto it = VC.begin(); it != VC.end(); ++it) {
		std::array<unsigned char, 3> bColor{ 0.0, 0.0, 0.0 };

		for (auto itt = it->begin(); itt != it->end(); ++itt) {
			for (unsigned int k = 0; k < itt->size(); ++k) {
				bColor[k] += (*itt)[k] / itt->size();
			}
		}

		bVC.push_back(bColor);
	}

	std::vector<std::string> vertex_color_entries;

	for (unsigned int i = 0; i < bVC.size(); ++i) {
		std::stringstream ss;
		ss.precision(6);
		ss << "v ";

		for (unsigned int j = 0; j < _V.cols(); ++j) {
			ss << std::fixed << _V(i, j) << " ";
		}

		for (unsigned int k = 0; k < bVC[i].size(); ++k) {
			ss << std::fixed << static_cast<double>(bVC[i][k]) / 255.0 << " ";
		}
		ss << std::endl;

		vertex_color_entries.push_back(ss.str());
	}

	return vertex_color_entries;
}

bool Mesh::refresh_colormap() {
	_face_colors = _base_colors;

	for (auto it = _displayed_patches.cbegin(); it != _displayed_patches.cend(); ++it) {
		const std::set<Eigen::DenseIndex>& fids = it->first->fids();

		for (auto itt = fids.cbegin(); itt != fids.cend(); ++itt) {
			_face_colors.row(*itt) << it->second.block<3, 1>(0, 0).transpose();
		}
	}

	_face_colors_C.topRows(_face_colors.rows()) << _face_colors;

	return true;
}

bool Mesh::mean_curvature(Eigen::MatrixXd& MC) {
	Eigen::SparseMatrix<double> L;
	Eigen::MatrixXd D;
	igl::cotmatrix(_V, _F, L);

	D = L * _V;
	MC = D.rowwise().norm();

	return true;
}

bool Mesh::unproject_onto_mesh(const Eigen::Vector2f& screen_point, const Eigen::Matrix4f& model, const Eigen::Matrix4f& proj, const Eigen::Vector4f& viewport, int & fid, Eigen::Vector3f& bc, bool include_extra) {
	if (include_extra) {
		Eigen::MatrixXd V_block = _VC.block(0, 0, _VC.rows(), 3);

		return igl::unproject_onto_mesh(screen_point, model, proj, viewport, V_block, _FC, fid, bc);
	} else {
		Eigen::MatrixXd V_block = _V.block(0, 0, _V.rows(), 3);

		return igl::unproject_onto_mesh(screen_point, model, proj, viewport, V_block, _F, fid, bc);
	}
}

std::shared_ptr<DiscreteExponentialMap> Mesh::arap_parameterization(Eigen::DenseIndex center) {
	// This was pulled directly from the libigl tutorial for ARAP parameterization
	Eigen::MatrixXd vertex_uv;

	if (_V.rows() >= 3) {
		Eigen::MatrixXd initial_guess;

		// Compute the initial solution for ARAP (harmonic parametrization)
		std::vector<Eigen::DenseIndex> loop;
		igl::boundary_loop(_F, loop);

		Eigen::VectorXi bnd;
		bnd.resize(loop.size());
		for (size_t i = 0; i < loop.size(); ++i)
			bnd(i) = loop[i];

		Eigen::MatrixXd bnd_uv;
		igl::map_vertices_to_circle(_V, bnd, bnd_uv);

		igl::harmonic(_V, _F, bnd, bnd_uv, 1, initial_guess);

		// Add dynamic regularization to avoid to specify boundary conditions
		igl::ARAPData arap_data;
		arap_data.with_dynamics = true;
		Eigen::VectorXi b = Eigen::VectorXi::Zero(0);
		Eigen::MatrixXd bc = Eigen::MatrixXd::Zero(0, 0);

		// Initialize ARAP
		arap_data.max_iter = 100;
		// 2 means that we're going to *solve* in 2d
		arap_precomputation(_V, _F, 2, b, arap_data);

		// Solve arap using the harmonic map as initial guess
		vertex_uv = initial_guess;

		igl::arap_solve(bc, arap_data, vertex_uv);
	}

	// Pack points for map representation
	std::map<Eigen::DenseIndex, Eigen::Vector2d> DEM_points;
	for (Eigen::DenseIndex i = 0; i < vertex_uv.rows(); ++i) {
		// Subtract center vid UV
		DEM_points.insert(std::pair<Eigen::DenseIndex, Eigen::Vector2d>(i, vertex_uv.row(i) - vertex_uv.row(center)));
	}

	// Scale UV to make it representative of edge distances
	if (DEM_points.size() > 0) {
		auto ring = one_ring(center);

		double scale = 0.0;
		for (auto vid : ring) {
			if (vid == center) {
				continue;
			}

			scale += ((_V.block<1, 3>(vid, 0) - _V.block<1, 3>(center, 0)).norm() / _TC.row(vid).norm()) / static_cast<double>(ring.size());
		}

		if (scale > std::numeric_limits<double>::epsilon()) {
			for (auto it = DEM_points.begin(); it != DEM_points.end(); ++it) {
				it->second = scale * it->second;
			}
		}
	} else {
		DEM_points.insert(std::pair<Eigen::DenseIndex, Eigen::Vector2d>(center, Eigen::Vector2d::Zero()));
	}

	// Create a TBN for the parameterization
	Eigen::Matrix3d TBN = basis_from_plane_normal(_N.row(center));

	std::shared_ptr<Patch> mesh_as_patch = Patch::instantiate(dynamic_cast<MeshInstancer*>(this)->shared_from_this(), Eigen::VectorXi::LinSpaced(_F.rows(), 0, _F.rows() - 1));

	//return std::make_shared<DiscreteExponentialMap>(center, TBN, DEM_points, dynamic_cast<MeshInstancer*>(this)->shared_from_this());
	return std::make_shared<DiscreteExponentialMap>(center, TBN, DEM_points, mesh_as_patch);
}

std::vector<Eigen::DenseIndex> Mesh::one_ring(Eigen::DenseIndex center_vid) {
	std::vector<Eigen::DenseIndex> ring;

	if (center_vid < 0 || center_vid >= _V.rows()) {
		return ring;
	}

	ring.push_back(center_vid);

	for (Eigen::SparseMatrix<int>::InnerIterator it(_adj, static_cast<int>(center_vid)); it; ++it) {
		Eigen::DenseIndex neighbor = it.row();   // neighbor vid

		ring.push_back(neighbor);
	}

	return ring;
}

Eigen::Matrix3d Mesh::basis_from_plane_normal(const Eigen::Vector3d plane_normal) {
	// Set up basis
	Eigen::Vector3d tangent = plane_normal.cross(Eigen::Vector3d::UnitX()).normalized();

	if (tangent.isZero()) {
		// Just in case we're unlucky and plane_normal is (anti)parallel to UnitX(), then it can't also be (anti)parellel UnitY()
		tangent = plane_normal.cross(Eigen::Vector3d::UnitY()).normalized();
	}

	Eigen::Vector3d bitangent = plane_normal.cross(tangent).normalized();

	Eigen::Matrix3d TBN;
	TBN << tangent, bitangent, plane_normal;

	return TBN;
}

Eigen::DenseIndex Mesh::vid_to_origin_mesh(Eigen::DenseIndex vid) {
	return vid;
}

Eigen::DenseIndex Mesh::fid_to_origin_mesh(Eigen::DenseIndex fid) {
	return fid;
}