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

#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <dlib/statistics.h>
#include <dlib/rand.h>
#include <dlib/svm.h>
#include <algorithm>
#include <dlib/matrix.h>
#include <cmath>

#include "tester.h"

namespace
{

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

    logger dlog("test.statistics");

    class statistics_tester : public tester
    {
    public:
        statistics_tester (
        ) :
            tester ("test_statistics",
                    "Runs tests on the statistics component.")
        {}

        void test_random_subset_selector ()
        {
            random_subset_selector<double> rand_set;

            for (int j = 0; j < 30; ++j)
            {
                print_spinner();

                running_stats<double> rs, rs2;

                rand_set.set_max_size(1000);

                for (double i = 0; i < 100000; ++i)
                {
                    rs.add(i);
                    rand_set.add(i);
                }

                for (unsigned long i = 0; i < rand_set.size(); ++i)
                    rs2.add(rand_set[i]);

                dlog << LDEBUG << "true mean:    " << rs.mean();
                dlog << LDEBUG << "true sampled: " << rs2.mean();
                double ratio = rs.mean()/rs2.mean();
                DLIB_TEST_MSG(0.96 < ratio  && ratio < 1.04, " ratio: " << ratio);
            }

            {
                random_subset_selector<int> r1, r2;
                r1.set_max_size(300);
                for (int i = 0; i < 4000; ++i)
                    r1.add(i);

                ostringstream sout;
                serialize(r1, sout);
                istringstream sin(sout.str());
                deserialize(r2, sin);

                DLIB_TEST(r1.size() == r2.size());
                DLIB_TEST(r1.max_size() == r2.max_size());
                DLIB_TEST(r1.next_add_accepts() == r2.next_add_accepts());
                DLIB_TEST(std::equal(r1.begin(), r1.end(), r2.begin()));

                for (int i = 0; i < 4000; ++i)
                {
                    r1.add(i);
                    r2.add(i);
                }

                DLIB_TEST(r1.size() == r2.size());
                DLIB_TEST(r1.max_size() == r2.max_size());
                DLIB_TEST(r1.next_add_accepts() == r2.next_add_accepts());
                DLIB_TEST(std::equal(r1.begin(), r1.end(), r2.begin()));
            }
        }

        void test_random_subset_selector2 ()
        {
            random_subset_selector<double> rand_set;
            DLIB_TEST(rand_set.next_add_accepts() == false);
            DLIB_TEST(rand_set.size() == 0);
            DLIB_TEST(rand_set.max_size() == 0);

            for (int j = 0; j < 30; ++j)
            {
                print_spinner();

                running_stats<double> rs, rs2;

                rand_set.set_max_size(1000);
                DLIB_TEST(rand_set.next_add_accepts() == true);

                for (double i = 0; i < 100000; ++i)
                {
                    rs.add(i);
                    if (rand_set.next_add_accepts())
                        rand_set.add(i);
                    else
                        rand_set.add();
                }

                DLIB_TEST(rand_set.size() == 1000);
                DLIB_TEST(rand_set.max_size() == 1000);

                for (unsigned long i = 0; i < rand_set.size(); ++i)
                    rs2.add(rand_set[i]);

                dlog << LDEBUG << "true mean:    " << rs.mean();
                dlog << LDEBUG << "true sampled: " << rs2.mean();
                double ratio = rs.mean()/rs2.mean();
                DLIB_TEST_MSG(0.96 < ratio  && ratio < 1.04, " ratio: " << ratio);
            }
        }

        void test_running_cross_covariance ()
        {
            running_cross_covariance<matrix<double> > rcc1, rcc2;

            matrix<double,0,1> xm, ym;
            const int num = 40;

            dlib::rand rnd;
            for (int i = 0; i < num; ++i)
            {
                matrix<double,0,1> x = randm(4,1,rnd);
                matrix<double,0,1> y = randm(4,1,rnd);

                xm += x/num;
                ym += y/num;

                if (i < 15)
                    rcc1.add(x,y);
                else
                    rcc2.add(x,y);
            }

            rnd.clear();
            matrix<double> cov;
            for (int i = 0; i < num; ++i)
            {
                matrix<double,0,1> x = randm(4,1,rnd);
                matrix<double,0,1> y = randm(4,1,rnd);
                cov += (x-xm)*trans(y-ym);
            }
            cov /= num-1;

            running_cross_covariance<matrix<double> > rcc = rcc1 + rcc2;
            DLIB_TEST(max(abs(rcc.covariance_xy()-cov)) < 1e-14);
            DLIB_TEST(max(abs(rcc.mean_x()-xm)) < 1e-14);
            DLIB_TEST(max(abs(rcc.mean_y()-ym)) < 1e-14);
        }

        std::map<unsigned long,double> dense_to_sparse (
            const matrix<double,0,1>& x
        )
        {
            std::map<unsigned long,double> temp;
            for (long i = 0; i < x.size(); ++i)
                temp[i] = x(i);
            return temp;
        }

        void test_running_cross_covariance_sparse()
        {
            running_cross_covariance<matrix<double> > rcc1, rcc2;

            running_covariance<matrix<double> > rc1, rc2;

            matrix<double,0,1> xm, ym;
            const int num = 40;

            rc1.set_dimension(4);
            rc2.set_dimension(4);

            rcc1.set_dimensions(4,5);
            rcc2.set_dimensions(4,5);

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

                xm += x/num;
                ym += y/num;

                if (i < 15)
                {
                    rcc1.add(x,dense_to_sparse(y));
                    rc1.add(x);
                }
                else if (i < 30)
                {
                    rcc2.add(dense_to_sparse(x),y);
                    rc2.add(dense_to_sparse(x));
                }
                else
                {
                    rcc2.add(dense_to_sparse(x),dense_to_sparse(y));
                    rc2.add(x);
                }
            }

            rnd.clear();
            matrix<double> cov, cov2;
            for (int i = 0; i < num; ++i)
            {
                matrix<double,0,1> x = randm(4,1,rnd);
                matrix<double,0,1> y = randm(5,1,rnd);
                cov += (x-xm)*trans(y-ym);
                cov2 += (x-xm)*trans(x-xm);
            }
            cov /= num-1;
            cov2 /= num-1;

            running_cross_covariance<matrix<double> > rcc = rcc1 + rcc2;
            DLIB_TEST_MSG(max(abs(rcc.covariance_xy()-cov)) < 1e-14, max(abs(rcc.covariance_xy()-cov)));
            DLIB_TEST(max(abs(rcc.mean_x()-xm)) < 1e-14);
            DLIB_TEST(max(abs(rcc.mean_y()-ym)) < 1e-14);

            running_covariance<matrix<double> > rc = rc1 + rc2;
            DLIB_TEST(max(abs(rc.covariance()-cov2)) < 1e-14);
            DLIB_TEST(max(abs(rc.mean()-xm)) < 1e-14);
        }

        void test_running_covariance (
        )
        {
            dlib::rand rnd;
            std::vector<matrix<double,0,1> > vects;

            running_covariance<matrix<double,0,1> > cov, cov2;
            DLIB_TEST(cov.in_vector_size() == 0);

            for (unsigned long dims = 1; dims < 5; ++dims)
            {
                for (unsigned long samps = 2; samps < 10; ++samps)
                {
                    vects.clear();
                    cov.clear();
                    DLIB_TEST(cov.in_vector_size() == 0);
                    for (unsigned long i = 0; i < samps; ++i)
                    {
                        vects.push_back(randm(dims,1,rnd));
                        cov.add(vects.back());

                    }
                    DLIB_TEST(cov.in_vector_size() == (long)dims);

                    DLIB_TEST(equal(mean(mat(vects)), cov.mean()));
                    DLIB_TEST_MSG(equal(covariance(mat(vects)), cov.covariance()),
                              max(abs(covariance(mat(vects)) - cov.covariance()))
                              << "   dims = " << dims << "   samps = " << samps
                              );
                }
            }

            for (unsigned long dims = 1; dims < 5; ++dims)
            {
                for (unsigned long samps = 2; samps < 10; ++samps)
                {
                    vects.clear();
                    cov.clear();
                    cov2.clear();
                    DLIB_TEST(cov.in_vector_size() == 0);
                    for (unsigned long i = 0; i < samps; ++i)
                    {
                        vects.push_back(randm(dims,1,rnd));
                        if ((i%2) == 0)
                            cov.add(vects.back());
                        else
                            cov2.add(vects.back());

                    }
                    DLIB_TEST((cov+cov2).in_vector_size() == (long)dims);

                    DLIB_TEST(equal(mean(mat(vects)), (cov+cov2).mean()));
                    DLIB_TEST_MSG(equal(covariance(mat(vects)), (cov+cov2).covariance()),
                              max(abs(covariance(mat(vects)) - (cov+cov2).covariance()))
                              << "   dims = " << dims << "   samps = " << samps
                              );
                }
            }

        }

        void test_running_stats()
        {
            print_spinner();

            running_stats<double> rs, rs2;

            running_scalar_covariance<double> rsc1, rsc2;
            running_scalar_covariance_decayed<double> rscd1(1000000), rscd2(1000000);

            for (double i = 0; i < 100; ++i)
            {
                rs.add(i);

                rsc1.add(i,i);
                rsc2.add(i,i);
                rsc2.add(i,-i);

                rscd1.add(i,i);
                rscd2.add(i,i);
                rscd2.add(i,-i);
            }

            // make sure the running_stats and running_scalar_covariance agree
            DLIB_TEST_MSG(std::abs(rs.mean() - rsc1.mean_x()) < 1e-10, std::abs(rs.mean() - rsc1.mean_x()));
            DLIB_TEST(std::abs(rs.mean() - rsc1.mean_y()) < 1e-10);
            DLIB_TEST(std::abs(rs.stddev() - rsc1.stddev_x()) < 1e-10);
            DLIB_TEST(std::abs(rs.stddev() - rsc1.stddev_y()) < 1e-10);
            DLIB_TEST(std::abs(rs.variance() - rsc1.variance_x()) < 1e-10);
            DLIB_TEST(std::abs(rs.variance() - rsc1.variance_y()) < 1e-10);
            DLIB_TEST(rs.current_n() == rsc1.current_n());

            DLIB_TEST(std::abs(rsc1.correlation() - 1) < 1e-10);
            DLIB_TEST(std::abs(rsc2.correlation() - 0) < 1e-10);

            const double s = 99/100.0;
            const double ss = std::sqrt(s);;
            DLIB_TEST_MSG(std::abs(rs.mean() - rscd1.mean_x()) < 1e-2, std::abs(rs.mean() - rscd1.mean_x()) << " " << rscd1.mean_x());
            DLIB_TEST(std::abs(rs.mean() - rscd1.mean_y()) < 1e-2);
            DLIB_TEST_MSG(std::abs(ss*rs.stddev() - rscd1.stddev_x()) < 1e-2, std::abs(ss*rs.stddev() - rscd1.stddev_x()));
            DLIB_TEST(std::abs(ss*rs.stddev() - rscd1.stddev_y()) < 1e-2);
            DLIB_TEST_MSG(std::abs(s*rs.variance() - rscd1.variance_x()) < 1e-2, std::abs(s*rs.variance() - rscd1.variance_x()));
            DLIB_TEST(std::abs(s*rs.variance() - rscd1.variance_y()) < 1e-2);
            DLIB_TEST(std::abs(rscd1.correlation() - 1) < 1e-2);
            DLIB_TEST(std::abs(rscd2.correlation() - 0) < 1e-2);

            // test serialization of running_stats
            ostringstream sout;
            serialize(rs, sout);
            istringstream sin(sout.str());
            deserialize(rs2, sin);
            // make sure the running_stats and running_scalar_covariance agree
            DLIB_TEST_MSG(std::abs(rs2.mean() - rsc1.mean_x()) < 1e-10, std::abs(rs2.mean() - rsc1.mean_x()));
            DLIB_TEST(std::abs(rs2.mean() - rsc1.mean_y()) < 1e-10);
            DLIB_TEST(std::abs(rs2.stddev() - rsc1.stddev_x()) < 1e-10);
            DLIB_TEST(std::abs(rs2.stddev() - rsc1.stddev_y()) < 1e-10);
            DLIB_TEST(std::abs(rs2.variance() - rsc1.variance_x()) < 1e-10);
            DLIB_TEST(std::abs(rs2.variance() - rsc1.variance_y()) < 1e-10);
            DLIB_TEST(rs2.current_n() == rsc1.current_n());

            rsc1.clear();
            rsc1.add(1, -1);
            rsc1.add(0, 0);
            rsc1.add(1, -1);
            rsc1.add(0, 0);
            rsc1.add(1, -1);
            rsc1.add(0, 0);

            DLIB_TEST(std::abs(rsc1.covariance() - -0.3) < 1e-10);
        }

        void test_skewness_and_kurtosis_1()
        {

            dlib::rand rnum;
            running_stats<double> rs1;

            double tp = 0;

            rnum.set_seed("DlibRocks");

            for(int i = 0; i< 1000000; i++)
            {
                tp = rnum.get_random_gaussian();
                rs1.add(tp);
            }

            // check the unbiased skewness and excess kurtosis of one million Gaussian
            // draws are both near_vects zero.
            DLIB_TEST(abs(rs1.skewness()) < 0.1);
            DLIB_TEST(abs(rs1.ex_kurtosis()) < 0.1);
        }

        void test_skewness_and_kurtosis_2()
        {

            string str = "DlibRocks";

            for(int j = 0; j<5 ; j++)
            {
                matrix<double,1,100000> dat;
                dlib::rand rnum;
                running_stats<double> rs1;

                double tp = 0;
                double n = 100000;
                double xb = 0;

                double sknum = 0;
                double skdenom = 0;
                double unbi_skew = 0;

                double exkurnum = 0;
                double exkurdenom = 0;
                double unbi_exkur = 0;

                random_shuffle(str.begin(), str.end());
                rnum.set_seed(str);

                for(int i = 0; i<n; i++)
                {
                    tp = rnum.get_random_gaussian();
                    rs1.add(tp);
                    dat(i)=tp;
                    xb += dat(i);
                }

                xb = xb/n;

                for(int i = 0; i < n; i++ )
                {
                    sknum += pow(dat(i) - xb,3);
                    skdenom += pow(dat(i) - xb,2);
                    exkurnum += pow(dat(i) - xb,4);
                    exkurdenom += pow(dat(i)-xb,2);
                }

                sknum = sknum/n;
                skdenom = pow(skdenom/n,1.5);
                exkurnum = exkurnum/n;
                exkurdenom = pow(exkurdenom/n,2);

                unbi_skew = sqrt(n*(n-1))/(n-2)*sknum/skdenom;
                unbi_exkur = (n-1)*((n+1)*(exkurnum/exkurdenom-3)+6)/((n-2)*(n-3));

                dlog << LINFO << "Skew Diff: " <<  unbi_skew - rs1.skewness();
                dlog << LINFO << "Kur Diff: " << unbi_exkur - rs1.ex_kurtosis();

                // Test an alternative implementation of the unbiased skewness and excess
                // kurtosis against the one in running_stats.
                DLIB_TEST(abs(unbi_skew - rs1.skewness()) < 1e-10);
                DLIB_TEST(abs(unbi_exkur - rs1.ex_kurtosis()) < 1e-10);
            }
        }

        void test_randomize_samples()
        {
            std::vector<unsigned int> t(15),u(15),v(15);

            for (unsigned long i = 0; i < t.size(); ++i)
            {
                t[i] = i;
                u[i] = i+1;
                v[i] = i+2;
            }
            randomize_samples(t,u,v);

            DLIB_TEST(t.size() == 15);
            DLIB_TEST(u.size() == 15);
            DLIB_TEST(v.size() == 15);

            for (unsigned long i = 0; i < t.size(); ++i)
            {
                const unsigned long val = t[i];
                DLIB_TEST(u[i] == val+1);
                DLIB_TEST(v[i] == val+2);
            }
        }
        void test_randomize_samples2()
        {
            dlib::matrix<int,15,1> t(15),u(15),v(15);

            for (long i = 0; i < t.size(); ++i)
            {
                t(i) = i;
                u(i) = i+1;
                v(i) = i+2;
            }
            randomize_samples(t,u,v);

            DLIB_TEST(t.size() == 15);
            DLIB_TEST(u.size() == 15);
            DLIB_TEST(v.size() == 15);

            for (long i = 0; i < t.size(); ++i)
            {
                const long val = t(i);
                DLIB_TEST(u(i) == val+1);
                DLIB_TEST(v(i) == val+2);
            }
        }

        void another_test()
        {
            std::vector<double> a;

            running_stats<double> rs1, rs2;

            for (int i = 0; i < 10; ++i)
            {
                rs1.add(i);
                a.push_back(i);
            }

            DLIB_TEST(std::abs(variance(mat(a)) - rs1.variance()) < 1e-13);
            DLIB_TEST(std::abs(stddev(mat(a)) - rs1.stddev()) < 1e-13);
            DLIB_TEST(std::abs(mean(mat(a)) - rs1.mean()) < 1e-13);

            for (int i = 10; i < 20; ++i)
            {
                rs2.add(i);
                a.push_back(i);
            }

            DLIB_TEST(std::abs(variance(mat(a)) - (rs1+rs2).variance()) < 1e-13);
            DLIB_TEST(std::abs(mean(mat(a)) - (rs1+rs2).mean()) < 1e-13);
            DLIB_TEST((rs1+rs2).current_n() == 20);

            running_scalar_covariance<double> rc1, rc2, rc3;
            dlib::rand rnd;
            for (double i = 0; i < 10; ++i)
            {
                const double a = i + rnd.get_random_gaussian();
                const double b = i + rnd.get_random_gaussian();
                rc1.add(a,b);
                rc3.add(a,b);
            }
            for (double i = 11; i < 20; ++i)
            {
                const double a = i + rnd.get_random_gaussian();
                const double b = i + rnd.get_random_gaussian();
                rc2.add(a,b);
                rc3.add(a,b);
            }

            DLIB_TEST(std::abs((rc1+rc2).mean_x() - rc3.mean_x()) < 1e-13);
            DLIB_TEST(std::abs((rc1+rc2).mean_y() - rc3.mean_y()) < 1e-13);
            DLIB_TEST_MSG(std::abs((rc1+rc2).variance_x() - rc3.variance_x()) < 1e-13, std::abs((rc1+rc2).variance_x() - rc3.variance_x()));
            DLIB_TEST(std::abs((rc1+rc2).variance_y() - rc3.variance_y()) < 1e-13);
            DLIB_TEST(std::abs((rc1+rc2).covariance() - rc3.covariance()) < 1e-13);
            DLIB_TEST((rc1+rc2).current_n() == rc3.current_n());

        }

        void test_average_precision()
        {
            std::vector<bool> items;
            DLIB_TEST(average_precision(items) == 1);
            DLIB_TEST(average_precision(items,1) == 0);

            items.push_back(true);
            DLIB_TEST(average_precision(items) == 1);
            DLIB_TEST(std::abs(average_precision(items,1) - 0.5) < 1e-14);

            items.push_back(true);
            DLIB_TEST(average_precision(items) == 1);
            DLIB_TEST(std::abs(average_precision(items,1) - 2.0/3.0) < 1e-14);

            items.push_back(false);

            DLIB_TEST(average_precision(items) == 1);
            DLIB_TEST(std::abs(average_precision(items,1) - 2.0/3.0) < 1e-14);

            items.push_back(true);

            DLIB_TEST(std::abs(average_precision(items) - (2.0+3.0/4.0)/3.0) < 1e-14);

            items.push_back(true);

            DLIB_TEST(std::abs(average_precision(items)   - (2.0 + 4.0/5.0 + 4.0/5.0)/4.0) < 1e-14);
            DLIB_TEST(std::abs(average_precision(items,1) - (2.0 + 4.0/5.0 + 4.0/5.0)/5.0) < 1e-14);
        }

        template <typename sample_type>
        void check_distance_metrics (
            const std::vector<frobmetric_training_sample<sample_type> >& samples
        )
        {
            running_stats<double> rs;
            for (unsigned long i = 0; i < samples.size(); ++i)
            {
                for (unsigned long j = 0; j < samples[i].near_vects.size(); ++j)
                {
                    const double d1 = length_squared(samples[i].anchor_vect - samples[i].near_vects[j]);
                    for (unsigned long k = 0; k < samples[i].far_vects.size(); ++k)
                    {
                        const double d2 = length_squared(samples[i].anchor_vect - samples[i].far_vects[k]);
                        rs.add(d2-d1);
                    }
                }
            }

            dlog << LINFO << "dist gap max:    "<< rs.max();
            dlog << LINFO << "dist gap min:    "<< rs.min();
            dlog << LINFO << "dist gap mean:   "<< rs.mean();
            dlog << LINFO << "dist gap stddev: "<< rs.stddev();
            DLIB_TEST(rs.min() >= 0.99);
            DLIB_TEST(rs.mean() >= 0.9999);
        }

        void test_vector_normalizer_frobmetric(dlib::rand& rnd)
        {
            print_spinner();
            typedef matrix<double,0,1> sample_type;
            vector_normalizer_frobmetric<sample_type> normalizer;

            std::vector<frobmetric_training_sample<sample_type> > samples;
            frobmetric_training_sample<sample_type> samp;

            const long key = 1;
            const long dims = 5;
            // Lets make some two class training data.  Each sample will have dims dimensions but
            // only the one with index equal to key will be meaningful.  In particular, if the key
            // dimension is > 0 then the sample is class +1 and -1 otherwise.

            long k = 0;
            for (int i = 0; i < 50; ++i)
            {
                samp.clear();
                samp.anchor_vect = gaussian_randm(dims,1,k++);
                if (samp.anchor_vect(key) > 0)
                    samp.anchor_vect(key) = rnd.get_random_double() + 5;
                else
                    samp.anchor_vect(key) = -(rnd.get_random_double() + 5);

                matrix<double,0,1> temp;

                for (int j = 0; j < 5; ++j)
                {
                    // Don't always put an equal number of near_vects and far_vects vectors into the
                    // training samples.
                    const int numa = rnd.get_random_32bit_number()%2 + 1;
                    const int numb = rnd.get_random_32bit_number()%2 + 1;

                    for (int num = 0; num < numa; ++num)
                    {
                        temp = gaussian_randm(dims,1,k++); temp(key) = 0.1;
                        //temp = gaussian_randm(dims,1,k++); temp(key) = std::abs(temp(key));
                        if (samp.anchor_vect(key) > 0) samp.near_vects.push_back(temp);
                        else                    samp.far_vects.push_back(temp);
                    }

                    for (int num = 0; num < numb; ++num)
                    {
                        temp = gaussian_randm(dims,1,k++); temp(key) = -0.1;
                        //temp = gaussian_randm(dims,1,k++); temp(key) = -std::abs(temp(key));
                        if (samp.anchor_vect(key) < 0) samp.near_vects.push_back(temp);
                        else                    samp.far_vects.push_back(temp);
                    }
                }
                samples.push_back(samp);
            }

            normalizer.set_epsilon(0.0001);
            normalizer.set_c(100);
            normalizer.set_max_iterations(6000);
            normalizer.train(samples);

            dlog << LINFO << "learned transform: \n" << normalizer.transform();

            matrix<double,0,1> total;

            for (unsigned long i = 0; i < samples.size(); ++i)
            {
                samples[i].anchor_vect = normalizer(samples[i].anchor_vect);
                total += samples[i].anchor_vect;
                for (unsigned long j = 0; j < samples[i].near_vects.size(); ++j)
                    samples[i].near_vects[j] = normalizer(samples[i].near_vects[j]);
                for (unsigned long j = 0; j < samples[i].far_vects.size(); ++j)
                    samples[i].far_vects[j] = normalizer(samples[i].far_vects[j]);
            }
            total /= samples.size();
            dlog << LINFO << "sample transformed means: "<< trans(total);
            DLIB_TEST(length(total) < 1e-9);
            check_distance_metrics(samples);

            // make sure serialization works
            stringstream os;
            serialize(normalizer, os);
            vector_normalizer_frobmetric<sample_type> normalizer2;
            deserialize(normalizer2, os);
            DLIB_TEST(equal(normalizer.transform(), normalizer2.transform()));
            DLIB_TEST(equal(normalizer.transformed_means(), normalizer2.transformed_means()));
            DLIB_TEST(normalizer.in_vector_size() == normalizer2.in_vector_size());
            DLIB_TEST(normalizer.out_vector_size() == normalizer2.out_vector_size());
            DLIB_TEST(normalizer.get_max_iterations() == normalizer2.get_max_iterations());
            DLIB_TEST(std::abs(normalizer.get_c() - normalizer2.get_c()) < 1e-14);
            DLIB_TEST(std::abs(normalizer.get_epsilon() - normalizer2.get_epsilon()) < 1e-14);

        }

        void prior_frobnorm_test()
        {
            frobmetric_training_sample<matrix<double,0,1> > sample;
            std::vector<frobmetric_training_sample<matrix<double,0,1> > > samples;

            matrix<double,3,1> x, near_, far_;
            x    = 0,0,0;
            near_ = 1,0,0;
            far_  = 0,1,0;

            sample.anchor_vect = x;
            sample.near_vects.push_back(near_);
            sample.far_vects.push_back(far_);

            samples.push_back(sample);

            vector_normalizer_frobmetric<matrix<double,0,1> > trainer;
            trainer.set_c(100);
            print_spinner();
            trainer.train(samples);

            matrix<double,3,3> correct;
            correct = 0, 0, 0,
                      0, 1, 0,
                      0, 0, 0;

            dlog << LDEBUG << trainer.transform();
            DLIB_TEST(max(abs(trainer.transform()-correct)) < 1e-8);

            trainer.set_uses_identity_matrix_prior(true);
            print_spinner();
            trainer.train(samples);
            correct = 1, 0, 0,
                      0, 2, 0,
                      0, 0, 1;

            dlog << LDEBUG << trainer.transform();
            DLIB_TEST(max(abs(trainer.transform()-correct)) < 1e-8);

        }

        void test_lda ()
        {
            // This test makes sure we pick the right direction in a simple 2D -> 1D LDA
            typedef matrix<double,2,1> sample_type;

            std::vector<unsigned long> labels;
            std::vector<sample_type> samples;
            for (int i=0; i<4; i++)
            {
                sample_type s;
                s(0) = i;
                s(1) = i+1;
                samples.push_back(s);
                labels.push_back(1);

                sample_type s1;
                s1(0) = i+1;
                s1(1) = i;
                samples.push_back(s1);
                labels.push_back(2);
            }

            matrix<double> X;
            X.set_size(8,2);
            for (int i=0; i<8; i++){
                X(i,0) = samples[i](0);
                X(i,1) = samples[i](1);
            }

            matrix<double,0,1> mean;

            dlib::compute_lda_transform(X,mean,labels,1);

            std::vector<double> vals1, vals2;
            for (unsigned long i = 0; i < samples.size(); ++i)
            {
                double val = X*samples[i]-mean;
                if (i%2 == 0)
                    vals1.push_back(val);
                else
                    vals2.push_back(val);
                dlog << LINFO << "1D LDA output: " << val;
            }

            if (vals1[0] > vals2[0])
                swap(vals1, vals2);

            const double err = equal_error_rate(vals1, vals2).first;
            dlog << LINFO << "LDA ERR: " << err;
            DLIB_TEST(err == 0);
            DLIB_TEST(equal_error_rate(vals2, vals1).first == 1);
        }

        void perform_test (
        )
        {
            prior_frobnorm_test();
            dlib::rand rnd;
            for (int i = 0; i < 5; ++i)
                test_vector_normalizer_frobmetric(rnd);

            test_random_subset_selector();
            test_random_subset_selector2();
            test_running_covariance();
            test_running_cross_covariance();
            test_running_cross_covariance_sparse();
            test_running_stats();
            test_skewness_and_kurtosis_1();
            test_skewness_and_kurtosis_2();
            test_randomize_samples();
            test_randomize_samples2();
            another_test();
            test_average_precision();
            test_lda();
        }
    } a;

}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>

int main(int argc, char **argv)
{
  // exploit namespaces to shorten code
  using namespace boost::numeric::ublas;
  using std::cout;
  using std::endl;
  int xmin = atoi(argv[1]);
  int xmax = atoi(argv[2]);
  int xlen = atoi(argv[3]);
  int ymin = atoi(argv[4]);
  int ymax = atoi(argv[5]);
  int ylen = atoi(argv[6]);
  int dx = (xmax-xmin)/(xlen-1);
  int dy = (ymax-ymin)/(ylen-1);

  // declare three 3x3 matrices of complex<long double> elements
  matrix<std::complex<long double> > m(xlen, ylen);

  // iterate over 3x3 matrix entries
  // r : row index
  // c : column index
  for (unsigned r = 0; r < m.size1(); r++) {
    for (unsigned c = 0; c < m.size1(); c++) {
      // enumerated matrix entries

      m(r,c) = (xmin + r*dx) + ((ymin + c * dy) * std::complex<long double>(0,1));
    }
  }

  // print to screen as demonstration

  for (unsigned int i=0; i<xlen; i++) {
        for (unsigned int j=0; j<ylen; j++) {
            std::cout << m(i,j) << "\t";
        }
        std::cout << std::endl;
    }
}


/*
 * Copyright 2009-2015 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

namespace votca { namespace tools {

using namespace std;

void linalg_invert( ub::matrix<double> &A, ub::matrix<double> &V){
        // matrix inversion using gsl

        gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();
	// signum s (for LU decomposition)
	int s;
        //make copy of A as A is destroyed by GSL
        ub::matrix<double> work=A;
        V.resize(N, N, false);

	// Define all the used matrices
        gsl_matrix_view A_view = gsl_matrix_view_array(&work(0,0), N, N);
        gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_permutation * perm = gsl_permutation_alloc (N);

	// Make LU decomposition of matrix A_view
	gsl_linalg_LU_decomp (&A_view.matrix, perm, &s);

	// Invert the matrix A_view
	(void)gsl_linalg_LU_invert (&A_view.matrix, perm, &V_view.matrix);

        gsl_set_error_handler(handler);

	// return (status != 0);
}

}}


#include <iostream>
#include <utility>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace std;
using namespace boost;
typedef adjacency_list<listS, vecS, directedS, no_property, property<edge_weight_t, int>> Graph;
typedef Graph::vertex_descriptor Vertex;
typedef pair<int, int> Edge;

int main(int argc, char* argv[])
{
  enum { A, B, C, D, E, N };
  const int num_vertices = N;
  Edge edge_array[] = { Edge(A,B), Edge(A,D), Edge(C,A), Edge(D,C), Edge(C,E), Edge(B,D), Edge(D,E) };
  const int num_edges = sizeof(edge_array)/sizeof(Edge);
  int weights[] = { 1, 2, 1, 2, 7, 3, 1};
  Graph g(edge_array, edge_array + num_vertices, weights, num_vertices);
  cout << "edges(g) = ";
  Graph::edge_iterator EdgeI, EdgeLast;
  for (tie(EdgeI, EdgeLast) = edges(g); EdgeI != EdgeLast; ++EdgeI)
    cout << "(" << source(*EdgeI, g) << "," << target(*EdgeI, g) << ") ";
  cout << endl;
  vector<int> distances(num_vertices);
  Vertex start = *(vertices(g).first);
  dijkstra_shortest_paths(g, start, distance_map(&distances[0]));
  cout << "distances from start vertex:" << endl;
  Graph::vertex_iterator VertexI, VertexLast;

  for(tie(VertexI, VertexLast) = vertices(g); VertexI != VertexLast; ++VertexI)
    cout << "distance(" << *VertexI << ") = " << distances[*VertexI] << endl;
  cout << endl;
  return 0;
}


#include <igl/boundary_facets.h>
#include <igl/colon.h>
#include <igl/cotmatrix.h>
#include <igl/jet.h>
#include <igl/min_quad_with_fixed.h>
#include <igl/readOFF.h>
#include <igl/setdiff.h>
#include <igl/slice.h>
#include <igl/slice_into.h>
#include <igl/unique.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Sparse>
#include <iostream>
#include "tutorial_shared_path.h"

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  MatrixXd V;
  MatrixXi F;
  igl::readOFF(TUTORIAL_SHARED_PATH "/camelhead.off",V,F);
  // Find boundary edges
  MatrixXi E;
  igl::boundary_facets(F,E);
  // Find boundary vertices
  VectorXi b,IA,IC;
  igl::unique(E,b,IA,IC);
  // List of all vertex indices
  VectorXi all,in;
  igl::colon<int>(0,V.rows()-1,all);
  // List of interior indices
  igl::setdiff(all,b,in,IA);

  // Construct and slice up Laplacian
  SparseMatrix<double> L,L_in_in,L_in_b;
  igl::cotmatrix(V,F,L);
  igl::slice(L,in,in,L_in_in);
  igl::slice(L,in,b,L_in_b);

  // Dirichlet boundary conditions from z-coordinate
  VectorXd bc;
  VectorXd Z = V.col(2);
  igl::slice(Z,b,bc);

  // Solve PDE
  SimplicialLLT<SparseMatrix<double > > solver(-L_in_in);
  VectorXd Z_in = solver.solve(L_in_b*bc);
  // slice into solution
  igl::slice_into(Z_in,in,Z);

  // Alternative, short hand
  igl::min_quad_with_fixed_data<double> mqwf;
  // Linear term is 0
  VectorXd B = VectorXd::Zero(V.rows(),1);
  // Empty constraints
  VectorXd Beq;
  SparseMatrix<double> Aeq;
  // Our cotmatrix is _negative_ definite, so flip sign
  igl::min_quad_with_fixed_precompute((-L).eval(),b,Aeq,true,mqwf);
  igl::min_quad_with_fixed_solve(mqwf,B,bc,Beq,Z);

  // Pseudo-color based on solution
  MatrixXd C;
  igl::jet(Z,true,C);

  // Plot the mesh with pseudocolors
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.data().show_lines = false;
  viewer.data().set_colors(C);
  viewer.launch();
}


#include <stan/math/prim/scal.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <math/prim/scal/prob/util.hpp>
#include <vector>

TEST(ProbDistributionsBernoulliLogit, error_check) {
  boost::random::mt19937 rng;

  EXPECT_NO_THROW(stan::math::bernoulli_logit_rng(-3.5, rng));
  EXPECT_THROW(
      stan::math::bernoulli_logit_rng(stan::math::positive_infinity(), rng),
      std::domain_error);
}

TEST(ProbDistributionsBernoulliLogit, logitChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  // number of samples
  int N = 10000;

  // logit-transformed probability
  double parameter = -0.5;
  // actual probability
  double prob = stan::math::inv_logit(-0.5);

  std::vector<double> expected;
  expected.push_back(N * (1 - prob));
  expected.push_back(N * prob);

  std::vector<int> counts(2);
  for (int i = 0; i < N; ++i) {
    ++counts[stan::math::bernoulli_logit_rng(parameter, rng)];
  }

  assert_chi_squared(counts, expected, 1e-6);
}


#ifndef COORDINATE_CALCULATION
#define COORDINATE_CALCULATION

#include "util/coordinate.hpp"

#include <boost/optional.hpp>

#include <utility>

namespace osrm
{
namespace util
{
namespace coordinate_calculation
{

namespace detail
{
const constexpr long double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
const constexpr long double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD;
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const constexpr long double EARTH_RADIUS = 6372797.560856;
}

//! Takes the squared euclidean distance of the input coordinates. Does not return meters!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs);

double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
                                                                const FloatCoordinate &target,
                                                                const FloatCoordinate &coordinate)
{
    const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat};
    const FloatCoordinate rel_coordinate{coordinate.lon - source.lon, coordinate.lat - source.lat};
    // dot product of two un-normed vectors
    const auto unnormed_ratio = static_cast<double>(slope_vector.lon * rel_coordinate.lon) +
                                static_cast<double>(slope_vector.lat * rel_coordinate.lat);
    // squared length of the slope vector
    const auto squared_length = static_cast<double>(slope_vector.lon * slope_vector.lon) +
                                static_cast<double>(slope_vector.lat * slope_vector.lat);

    if (squared_length < std::numeric_limits<double>::epsilon())
    {
        return {0, source};
    }

    const double normed_ratio = unnormed_ratio / squared_length;
    double clamped_ratio = normed_ratio;
    if (clamped_ratio > 1.)
    {
        clamped_ratio = 1.;
    }
    else if (clamped_ratio < 0.)
    {
        clamped_ratio = 0.;
    }

    return {clamped_ratio,
            {
                FloatLongitude(1.0 - clamped_ratio) * source.lon +
                    target.lon * FloatLongitude(clamped_ratio),
                FloatLatitude(1.0 - clamped_ratio) * source.lat +
                    target.lat * FloatLatitude(clamped_ratio),
            }};
}

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location);

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio);

Coordinate centroid(const Coordinate lhs, const Coordinate rhs);

double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate);

// Get angle of line segment (A,C)->(C,B)
double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third);

// find the center of a circle through three coordinates
boost::optional<Coordinate> circleCenter(const Coordinate first_coordinate,
                                         const Coordinate second_coordinate,
                                         const Coordinate third_coordinate);

// find the radius of a circle through three coordinates
double circleRadius(const Coordinate first_coordinate,
                    const Coordinate second_coordinate,
                    const Coordinate third_coordinate);

// factor in [0,1]. Returns point along the straight line between from and to. 0 returns from, 1
// returns to
Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to);

} // ns coordinate_calculation
} // ns util
} // ns osrm

#endif // COORDINATE_CALCULATION


//
// Created by Amir Masoud Abdol on 2020-04-11
//

#include "TestStrategy.h"

#include <boost/math/distributions/students_t.hpp>

using namespace sam;
using boost::math::students_t;

void TTest::run(Experiment *experiment) {

  static ResultType res{};

  // The first group is always the control group
  for (int i{experiment->setup.nd()}, d{0}; i < experiment->setup.ng();
       ++i, ++d %= experiment->setup.nd()) {

    if (params.var_equal) {
      res = two_samples_t_test_equal_sd((*experiment)[d].mean_, (*experiment)[d].stddev_,
                                        (*experiment)[d].nobs_, (*experiment)[i].mean_,
                                        (*experiment)[i].stddev_, (*experiment)[i].nobs_,
                                        params.alpha, params.alternative);
    } else {
      res = two_samples_t_test_unequal_sd((*experiment)[d].mean_, (*experiment)[d].stddev_,
                                          (*experiment)[d].nobs_, (*experiment)[i].mean_,
                                          (*experiment)[i].stddev_, (*experiment)[i].nobs_,
                                          params.alpha,
                                          params.alternative);
    }

    (*experiment)[i].stats_ = res.tstat;
    (*experiment)[i].pvalue_ = res.pvalue;
    (*experiment)[i].sig_ = res.sig;
  }
}

TTest::ResultType TTest::t_test(const arma::Row<float> &dt1,
                                const arma::Row<float> &dt2, float alpha,
                                TestStrategy::TestAlternative alternative) {
  return t_test(arma::mean(dt1), arma::stddev(dt1), dt1.size(), arma::mean(dt2),
                arma::stddev(dt2), dt2.size(), alpha, alternative, true);
}

TTest::ResultType TTest::t_test(float Sm1, float Sd1, float Sn1, float Sm2,
                                float Sd2, float Sn2, float alpha,
                                TestStrategy::TestAlternative alternative,
                                bool equal_var = false) {

  if (Sm1 == 0.) {
    return single_sample_t_test(Sm1, Sm2, Sd2, Sn2, alpha, alternative);
  }

  if (equal_var) {
    return two_samples_t_test_equal_sd(Sm1, Sd1, Sn1, Sm2, Sd2, Sn2, alpha,
                                       alternative);
  } else {
    return two_samples_t_test_unequal_sd(Sm1, Sd1, Sn1, Sm2, Sd2, Sn2, alpha,
                                         alternative);
  }
}

std::pair<float, bool>
TTest::compute_pvalue(float t_stat, float df, float alpha, TestStrategy::TestAlternative alternative) {

  using boost::math::students_t;
  students_t dist(df);

  double p{0};
  bool sig{false};

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Mean != M
    p = 2. * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) { // Alternative "NOT REJECTED"
      sig = true;
    } else { // Alternative "REJECTED"
      sig = false;
    }
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Mean  > M
    p = cdf(dist, t_stat);
    if (p > alpha) { // Alternative "NOT REJECTED"
      sig = true;
    } else { // Alternative "REJECTED"
      sig = false;
    }
  }

  if (alternative == TestStrategy::TestAlternative::Less) {
    // Mean  < M
    p = cdf(complement(dist, t_stat));
    if (p > alpha) { // Alternative "NOT REJECTED"
      sig = true;
    } else { // Alternative "REJECTED"
      sig = false;
    }
  }

  return std::make_pair(p, sig);
}

///
/// A Students t test applied to a single set of data. We are testing the null
/// hypothesis that the true mean of the sample is M, and that any variation is
/// down to chance.  We can also test the alternative hypothesis that any
/// difference is not down to chance
///
/// @note       Obtained from [Boost Library
///             Example](https://www.boost.org/doc/libs/1_69_0/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/paired_st.html).
///
/// @param      M      True Mean.
/// @param      Sm     Sample Mean.
/// @param      Sd     Sample Standard Deviation.
/// @param      Sn     Sample Size.
/// @param      alpha  Significance Level.
/// @return     TTest::ResultType
///
TTest::ResultType
TTest::single_sample_t_test(float M, float Sm, float Sd, unsigned Sn,
                            float alpha,
                            TestStrategy::TestAlternative alternative) {

  bool sig = false;

  // Difference in means:
  float diff = Sm - M;

  // Degrees of freedom:
  float df = Sn - 1;

  // t-statistic:
  float t_stat = diff * sqrt(float(Sn)) / Sd;

  //
  // Finally define our distribution, and get the probability:
  //
  students_t dist(df);
  float p = 0;

  //
  // Finally print out results of alternative hypothesis:
  //

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Mean != M
    p = 2 * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Mean  > M
    p = cdf(dist, t_stat);
    if (p > alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Less) {
    // Mean  < M
    p = cdf(complement(dist, t_stat));
    if (p > alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  return {.tstat = t_stat, .df = df, .pvalue = p, .sig = sig};
}

///
/// A Students t test applied to two sets of data. We are testing the null
/// hypothesis that the two samples have the same mean and that any difference
/// if due to chance.
///
/// @note       Obtained from [Boost Library
///             Example](https://www.boost.org/doc/libs/1_69_0/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/paired_st.html).
///
/// @param      Sm1    Sample Mean 1.
/// @param      Sd1    Sample Standard Deviation 1.
/// @param      Sn1    Sample Size 1.
/// @param      Sm2    Sample Mean 2.
/// @param      Sd2    Sample Standard Deviation 2.
/// @param      Sn2    Sample Size 2.
/// @param      alpha  Significance Level.
/// @return     TTest::ResultType
///
TTest::ResultType TTest::two_samples_t_test_equal_sd(
    float Sm1, float Sd1, unsigned Sn1, float Sm2, float Sd2, unsigned Sn2,
    float alpha, TestStrategy::TestAlternative alternative) {

  bool sig = false;

  // Degrees of freedom:
  float df = Sn1 + Sn2 - 2;

  // Pooled variance and hence standard deviation:
  float sp = sqrt(((Sn1 - 1) * Sd1 * Sd1 + (Sn2 - 1) * Sd2 * Sd2) / df);

  // NOTE: I had to do this, I don't want my simulations fail.
  // While this is not perfect, it allows me to handle an edge case
  // SAM should not throw and should continue working.
  if (!(isgreater(sp, 0) or isless(sp, 0))) {
    // Samples are almost equal and elements are constant
    sp += std::numeric_limits<float>::epsilon();
  }

  // t-statistic:
  float t_stat = (Sm1 - Sm2) / (sp * sqrt(1.0 / Sn1 + 1.0 / Sn2));

  //
  // Define our distribution, and get the probability:
  //
  students_t dist(df);
  float p = 0;

  //
  // Finally print out results of alternative hypothesis:
  //

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Sample 1 Mean != Sample 2 Mean
    p = 2 * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Sample 1 Mean <  Sample 2 Mean
    p = cdf(dist, t_stat);
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Less) {

    // Sample 1 Mean >  Sample 2 Mean
    p = cdf(complement(dist, t_stat));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else
      sig = false; // Alternative "REJECTED"
  }

  return {.tstat = t_stat, .df = df, .pvalue = p, .sig = sig};
}

///
/// A Students t test applied to two sets of data with _unequal_ variance. We
/// are testing the null hypothesis that the two samples have the same mean and
/// that any difference is due to chance.
///
/// @note       Obtained from [Boost Library
///             Example](https://www.boost.org/doc/libs/1_69_0/libs/math/doc/html/math_toolkit/stat_tut/weg/st_eg/paired_st.html).
///
/// @param      Sm1    Sample Mean 1.
/// @param      Sd1    Sample Standard Deviation 1.
/// @param      Sn1    Sample Size 1.
/// @param      Sm2    Sample Mean 2.
/// @param      Sd2    Sample Standard Deviation 2.
/// @param      Sn2    Sample Size 2.
/// @param      alpha  Significance Level.
/// @return     TTest::ResultType
///
TTest::ResultType TTest::two_samples_t_test_unequal_sd(
    float Sm1, float Sd1, unsigned Sn1, float Sm2, float Sd2, unsigned Sn2,
    float alpha, TestStrategy::TestAlternative alternative) {

  bool sig = false;

  // Degrees of freedom:
  float df = Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2;
  df *= df;
  float t1 = Sd1 * Sd1 / Sn1;
  t1 *= t1;
  t1 /= (Sn1 - 1);
  float t2 = Sd2 * Sd2 / Sn2;
  t2 *= t2;
  t2 /= (Sn2 - 1);
  df /= (t1 + t2);

  // t-statistic:
  float t_stat = (Sm1 - Sm2) / sqrt(Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2);

  //
  // Define our distribution, and get the probability:
  //
  students_t dist(df);
  float p = 0;

  //
  // Finally print out results of alternative hypothesis:
  //

  if (alternative == TestStrategy::TestAlternative::TwoSided) {
    // Sample 1 Mean != Sample 2 Mean
    p = 2 * cdf(complement(dist, fabs(t_stat)));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Greater) {
    // Sample 1 Mean <  Sample 2 Mean
    p = cdf(dist, t_stat);
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else // Alternative "REJECTED"
      sig = false;
  }

  if (alternative == TestStrategy::TestAlternative::Less) {
    // Sample 1 Mean >  Sample 2 Mean
    p = cdf(complement(dist, t_stat));
    if (p < alpha) // Alternative "NOT REJECTED"
      sig = true;
    else
      sig = false; // Alternative "REJECTED"
  }

  return {.tstat = t_stat, .df = df, .pvalue = p, .sig = sig};
}


#include "functions/division.hh"
#include <boost/test/unit_test.hpp>
#include "functions/full_function_defs.hh"
#include "functions/operators.hh"
#include "functions/std_functions.hh"
#include "pointwise_equal.hh"

BOOST_AUTO_TEST_CASE(reciprocal_test) {
  using namespace manifolds;

  Division<Sin, Cos> t;
  BOOST_CHECK_EQUAL(t(2), std::sin(2) / std::cos(2));
  auto d1 = Sin()(x) / Log()(x);
  auto d2 = Pow()(x, y) / Tan()(x);
  auto m12 = MultiplyRaw(d1, d2);
  static_assert(Simplifies<decltype(m12)>::value, "");
  auto m12s = Simplify(m12);
  PointwiseEqual(m12, m12s, pointwise_default_num_points, 1E-11, 0.1, 20);
  auto d12 = DivideRaw(d1, d2);
  static_assert(Simplifies<decltype(d12)>::value, "");
  auto d12s = Simplify(d12);
  PointwiseEqual(d12s, d12, pointwise_default_num_points, 1E-11, 0.001, 20);
}


// ========================================================================== //
// Copyright (c) 2014-2019 The University of Texas at Austin.                 //
// All rights reserved.                                                       //
//                                                                            //
// Licensed under the Apache License, Version 2.0 (the "License");            //
// you may not use this file except in compliance with the License.           //
// A copy of the License is included with this software in the file LICENSE.  //
// If your copy does not contain the License, you may obtain a copy of the    //
// License at:                                                                //
//                                                                            //
//     https://www.apache.org/licenses/LICENSE-2.0                            //
//                                                                            //
// Unless required by applicable law or agreed to in writing, software        //
// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT  //
// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.           //
// See the License for the specific language governing permissions and        //
// limitations under the License.                                             //
//                                                                            //
// ========================================================================== //

#pragma once

#include <iostream>
#include "dtypes.h"

#include <boost/math/quaternion.hpp>
#include <cmath>

typedef boost::math::quaternion<double> quaternion;

namespace gxy
{

class Q
{
public:
	Q() { zero(); }
	Q(quaternion q) : q(q) {}
	Q(vec3f axis) { q = quaternion(0.0, axis.x, axis.y, axis.z); }
	Q(vec3f u, float r) { q =  quaternion(cos(r/2), sin(r/2)*u.x, sin(r/2)*u.y, sin(r/2)*u.z); }

	~Q() {}

	Q operator*(const Q& q) const
	{
		return Q(this->q * q.q);
	}

	void print(std::ostream& o)
	{
	}

	vec3f rotate(vec3f v)
	{
		quaternion qr = this->q * quaternion(0.0, v.x, v.y, v.z) * conj(this->q);
		return vec3f( qr.R_component_2(), qr.R_component_3(),qr.R_component_4());
	}

	void zero() { q = quaternion(1.0, 0.0, 0.0, 0.0); }

	quaternion get() { return q; }

private:
	quaternion q;
};

class Trackball
{
public:
	Trackball(float s = 1) : size(s) {}
	~Trackball() {}

	void spin(float p1x, float p1y, float p2x, float p2y)
	{
    vec3f axis;      /* Axis of rotation */
    float phi;       /* how much to rotate about axis */
    vec3f p1, p2, d;
    float t;

    if (p1x == p2x && p1y == p2y)
			return;

    /*
     * First, figure out z-coordinates for projection of P1 and P2 to
     * deformed sphere
     */
    p1 = vec3f(p1x, p1y, project_to_sphere(size,p1x,p1y));
    p2 = vec3f(p2x, p2y, project_to_sphere(size,p2x,p2y));

    /*
     *  Now, we want the cross product of P1 and P2
     */
    cross(p2,p1,axis);
    normalize(axis);

    /*
     *  Figure out how much to rotate around that axis.
     */
    sub(p1,p2,d);
    t = len(d) / (2.0*size);

    /*
     * Avoid problems with out-of-control values...
     */
    if (t > 1.0) t = 1.0;
    if (t < -1.0) t = -1.0;
    phi = 2.0 * asin(t);

		current = current * Q(axis, phi);

		return;
	}

	vec3f rotate_vector(vec3f v)
	{
		quaternion qr = current.get() * quaternion(0.0, v.x, v.y, v.z) * conj(current.get());
		return vec3f( qr.R_component_2(), qr.R_component_3(),qr.R_component_4());
	}

	Q get() { return current; }

	void reset() { current.zero(); }

private:
	Q current;
	float size;

	float project_to_sphere(float r, float x, float y)
	{
    float d, t, z;

    d = sqrt(x*x + y*y);
    if (d < r * 0.70710678118654752440)
        z = sqrt(r*r - d*d);
    else
		{
        t = r / 1.41421356237309504880;
        z = t*t / d;
    }
    return z;
	}
};

}


#include <cmath>
#include <boost/test/unit_test.hpp>
#include "Werk/Math/ContinuousEma.hpp"

BOOST_AUTO_TEST_SUITE(ContinuousEmaTest)

BOOST_AUTO_TEST_CASE(testEmpty) {

	Werk::ContinuousEma ema(10.0);
	BOOST_CHECK_EQUAL(ema.halfLife(), 10.0);
	BOOST_CHECK(ema.factor() < 1.0/10.0);
	BOOST_CHECK(std::isnan(ema.value()));
}

BOOST_AUTO_TEST_CASE(testConstant) {

	Werk::ContinuousEma ema(10.0);
	BOOST_CHECK(std::isnan(ema.value()));
	ema.sample(0.0, 10.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
	ema.sample(4.0, 10.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
	ema.sample(17.0, 10.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
}

BOOST_AUTO_TEST_CASE(testDecreasing)
{
	Werk::ContinuousEma ema(60.0);

	BOOST_CHECK(std::isnan(ema.value()));
	ema.sample(0.0, 10.0);

	BOOST_CHECK_EQUAL(ema.value(), 10.0);
	ema.sample(13.0, 0.0);
	BOOST_CHECK(ema.value() < 10.0);

	double value = ema.value();
	ema.sample(19.0, 0.0);
	BOOST_CHECK(ema.value() < value);
}

BOOST_AUTO_TEST_CASE(testIncreasing)
{
	Werk::ContinuousEma ema(60.0);

	BOOST_CHECK(std::isnan(ema.value()));
	ema.sample(0.0, 10.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
	ema.sample(7.0, 13.0);
	BOOST_CHECK(ema.value() > 10);

	double value = ema.value();
	ema.sample(19.0, 20.0);
	BOOST_CHECK(ema.value() > value);
}

BOOST_AUTO_TEST_CASE(testOscillating)
{
	Werk::ContinuousEma ema(10.0);

	BOOST_CHECK(std::isnan(ema.value()));
	ema.sample(0.0, 10.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
	ema.sample(10.0, 0.0);
	BOOST_CHECK_CLOSE(ema.value(), 5.0, 0.5);
	ema.sample(20.0, 10.0);
	BOOST_CHECK_CLOSE(ema.value(), 7.5, 0.5);
	ema.sample(30.0, 0.0);
	BOOST_CHECK_CLOSE(ema.value(), 3.75, 0.5);
}

BOOST_AUTO_TEST_CASE(testNan)
{
	Werk::ContinuousEma ema(60.0);

	BOOST_CHECK(std::isnan(ema.value()));
	ema.sample(0.0, 10.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
	ema.sample(3.0, std::numeric_limits<double>::quiet_NaN());
	BOOST_CHECK(std::isnan(ema.value()));
	ema.sample(5.0, 10.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
}

BOOST_AUTO_TEST_CASE(testInfiniteHalfLife)
{
	Werk::ContinuousEma ema(std::numeric_limits<double>::infinity());

	BOOST_CHECK(std::isnan(ema.value()));
	ema.sample(0.0, 10.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
	ema.sample(3.0, 15.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
	ema.sample(15.0, 2310.0);
	BOOST_CHECK_EQUAL(ema.value(), 10.0);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef STAN_MATH_PRIM_SCAL_FUN_HYPOT_HPP
#define STAN_MATH_PRIM_SCAL_FUN_HYPOT_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <boost/math/tools/promotion.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Return the length of the hypoteneuse of a right triangle with
 * opposite and adjacent side lengths given by the specified
 * arguments (C++11).  In symbols, if the arguments are
 * x and y, the result is sqrt(x *
 * x + y * y).
 *
 * @param x First argument.
 * @param y Second argument.
 * @return Length of hypoteneuse of right triangle with opposite
 * and adjacent side lengths x and y.
 */
template <typename T1, typename T2>
inline return_type_t<T1, T2> hypot(const T1& x, const T2& y) {
  using std::sqrt;
  return sqrt(square(x) + square(y));
}

}  // namespace math
}  // namespace stan
#endif


/**
 * \Description:
 * this node is used to estimate the absoulte location of the robot using the optimization library  "RobOptim".
 * It subscribe to:
 * - /beacon_distances
 * It publishes to:
 * - /robot_pose
 *
 * SERRANO&ALI_ECN_M1_2017
 */

// Cpp
#include <stdexcept>
#include <sstream>
#include <stdio.h>
#include <vector>
#include <iostream>
#include <stdlib.h>
#include <string>
#include <math.h>

// Boost
#include <boost/shared_ptr.hpp>

// RobOptim
#include <roboptim/core/linear-function.hh>
#include <roboptim/core/differentiable-function.hh>
#include <roboptim/core/twice-differentiable-function.hh>
#include <roboptim/core/io.hh>
#include <roboptim/core/solver.hh>
#include <roboptim/core/solver-factory.hh>

//ROS
#include "ros/ros.h"
#include <geometry_msgs/Pose2D.h>
#include <std_msgs/Float64MultiArray.h>

// blue beacon coordinates
#define X1 0
#define Y1 0
// red beacon coordinates
#define X2 0
#define Y2 190
// green beacon coordinates
#define X3 350
#define Y3 0
// upper and lower bounds of map
#define LB 0
#define UB 400

// global variables
float blue_distance, red_distance, green_distance;
bool are_dist_available = false;
ros::Publisher pose_pub;

using namespace roboptim;
using namespace std;

// function to minimize
struct F : public TwiceDifferentiableFunction
{
    F () : TwiceDifferentiableFunction (2, 1, "(x[0]-X1)^2 + (x[1]-Y1)^2 + (x[0]-X2)^2 + (x[1]-Y2)^2 + (x[0]-X3)^2 + (x[1]-Y3)^2")
    {
    }

    void impl_compute (result_ref result, const_argument_ref x) const
    {
        result[0] = pow(x[0]-X1, 2) + pow(x[1]-Y1, 2) + pow(x[0]-X2, 2) + pow(x[1]-Y2, 2) + pow(x[0]-X3, 2) + pow(x[1]-Y3, 2);
    }

    void impl_gradient (gradient_ref grad, const_argument_ref x, size_type) const
    {
        grad << 2*(x[0]-X1) + 2*(x[0]-X2) + 2*(x[0]-X3),
                2*(x[1]-Y1) + 2*(x[1]-Y2) + 2*(x[1]-Y3);
    }

    void impl_hessian (hessian_ref h, const_argument_ref x, size_type) const
    {
        h << 6.0, 0.0,
                0.0, 6.0;
    }
};

// constraint 1
struct G0 : public TwiceDifferentiableFunction
{
    G0 () : TwiceDifferentiableFunction (2, 1, "(x[0]-X1)^2 + (x[1]-Y1)^2")
    {
    }

    void impl_compute (result_ref result, const_argument_ref x) const
    {
        result[0] = pow(x[0]-X1, 2) + pow(x[1]-Y1, 2);
    }

    void impl_gradient (gradient_ref grad, const_argument_ref x, size_type) const
    {
        grad << 2*(x[0]-X1),
                2*(x[1]-Y1);
    }

    void impl_hessian (hessian_ref h, const_argument_ref x, size_type) const
    {
        h << 2.0, 0.0,
                0.0, 2.0;
    }
};

// constraint 2
struct G1 : public TwiceDifferentiableFunction
{
    G1 () : TwiceDifferentiableFunction (2, 1, "(x[0]-X2)^2 + (x[1]-Y2)^2")
    {
    }

    void impl_compute (result_ref result, const_argument_ref x) const
    {
        result[0] = pow(x[0]-X2, 2) + pow(x[1]-Y2, 2);
    }

    void impl_gradient (gradient_ref grad, const_argument_ref x, size_type) const
    {
        grad << 2*(x[0]-X2),
                2*(x[1]-Y2);
    }

    void impl_hessian (hessian_ref h, const_argument_ref x, size_type) const
    {
        h << 2.0, 0.0,
                0.0, 2.0;
    }
};

// constraint 3
struct G2 : public TwiceDifferentiableFunction
{
    G2 () : TwiceDifferentiableFunction (2, 1, "(x[0]-X3)^2 + (x[1]-Y3)^2")
    {
    }

    void impl_compute (result_ref result, const_argument_ref x) const
    {
        result[0] = pow(x[0]-X3, 2) + pow(x[1]-Y3, 2);
    }

    void impl_gradient (gradient_ref grad, const_argument_ref x, size_type) const
    {
        grad << 2*(x[0]-X3),
                2*(x[1]-Y3);
    }

    void impl_hessian (hessian_ref h, const_argument_ref x, size_type) const
    {
        h << 2.0, 0.0,
                0.0, 2.0;
    }
};

// Callback functions
void getDistanceCallback(std_msgs::Float64MultiArray distances)
{
    are_dist_available = true;
    blue_distance = distances.data[0];
    red_distance = distances.data[1];
    green_distance = distances.data[2];
}

int main (int argc, char** argv)
{

    //ROS Initialization
    ros::init(argc, argv, "triangulation_optimization");

    // Define your node handles
    ros::NodeHandle nh, nh_loc("~");

    // Declare your subscribers
    ros::Subscriber dist_sub = nh.subscribe<std_msgs::Float64MultiArray>("beacon_distances", 1, getDistanceCallback);

    // Declare your publishers
    pose_pub = nh.advertise<geometry_msgs::Pose2D>("robot_pose", 1);

    // node initilization
    // ...

    // rate
    ros::Rate rate(30); // Hz
    while (ros::ok()){
        ros::spinOnce();

        // node code
        if (!are_dist_available) {
            ROS_INFO("Waiting for distances");
            continue;
        }

        // ------------ RobOptim ---------------
        typedef Solver<EigenMatrixDense> solver_t;

        // Create cost function.
        boost::shared_ptr<F> f (new F ());

        // Create problem.
        solver_t::problem_t pb (f);

        // Set bounds for all optimization parameters.
        // 1. < x_i < 5. (x_i in [1.;5.])
        for (Function::size_type i = 0; i < pb.function ().inputSize (); ++i)
            pb.argumentBounds ()[i] = Function::makeInterval (LB, UB);

        // Set the starting point.
        Function::vector_t start (pb.function ().inputSize ());
        start << 150.,100.;
        pb.startingPoint() = start;

        // Create constraints.
        boost::shared_ptr<G0> g0 (new G0 ());
        boost::shared_ptr<G1> g1 (new G1 ());
        boost::shared_ptr<G2> g2 (new G2 ());

        F::intervals_t bounds;
        solver_t::problem_t::scaling_t scaling;

        // Add constraints
        bounds.push_back(Function::makeLowerInterval( pow(blue_distance, 2) ));  // blue beacon distance
        scaling.push_back (1.);
        pb.addConstraint (g0, bounds, scaling);

        bounds.clear ();
        scaling.clear ();

        bounds.push_back(Function::makeLowerInterval( pow(red_distance, 2) ));   // red beacon distance
        scaling.push_back (1.);
        pb.addConstraint (g1, bounds, scaling);

        bounds.clear ();
        scaling.clear ();

        bounds.push_back(Function::makeLowerInterval( pow(green_distance, 2) )); // green beacon distance
        scaling.push_back (1.);
        pb.addConstraint (g2, bounds, scaling);

        bounds.clear ();
        scaling.clear ();

        // Initialize solver.

        // Here we are relying on the CFSQP solver.
        // You may change this string to load the solver you wish to use:
        //  - Ipopt: "ipopt", "ipopt-sparse", "ipopt-td"
        //  - Eigen: "eigen-levenberg-marquardt"
        //  etc.
        // The plugin is built for a given solver type, so choose it adequately.
        SolverFactory<solver_t> factory ("ipopt", pb);
        solver_t& solver = factory ();

        // Compute the minimum and retrieve the result.
        solver_t::result_t res = solver.minimum ();

        // Display solver information.
        // std::cout << solver << std::endl;

        // Check if the minimization has succeeded.

        // Process the result
        switch (res.which ())
        {
        case solver_t::SOLVER_VALUE:
        {
            // Get the result.
            Result& result = boost::get<Result> (res);

            // Display the result.
            std::cout << "A solution has been found: " << std::endl << result << std::endl;
            geometry_msgs::Pose2D pose_msg;
            pose_msg.x = result.x[0];
            pose_msg.y = result.x[1];
            pose_msg.theta = 0;
            pose_pub.publish(pose_msg);
            break;
        }

        case solver_t::SOLVER_VALUE_WARNINGS:
        {
            // Get the result.
            ResultWithWarnings& result = boost::get<ResultWithWarnings> (res);

            // Display the result.
            std::cout << "A solution w/warning has been found: " << std::endl
                      << result << std::endl;

            break;
        }

        case solver_t::SOLVER_NO_SOLUTION:
        case solver_t::SOLVER_ERROR:
        {
            std::cout << "A solution should have been found. Failing..."
                      << std::endl
                      << boost::get<SolverError> (res).what ()
                      << std::endl;

            break;
        }
        }

        are_dist_available = false;

        rate.sleep();
    }
}


/*
 * DIPlib 3.0
 * This file contains the thin plate spline functionality
 *
 * (c)2019, Cris Luengo.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "diplib/library/numeric.h"

#if defined(__GNUG__) || defined(__clang__)
// For this file, turn off -Wsign-conversion, Eigen is really bad at this!
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic ignored "-Wconversion"
#if __GNUC__ >= 7 || __clang_major__ >= 12
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#endif
#if __GNUC__ >= 9
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#endif

#include <Eigen/QR>
#include <Eigen/Cholesky>

namespace dip {

namespace {

dfloat RadialBasis( dfloat r ) {
   return r > 0 ? r * r * std::log( r ) : 0;
}

} // namespace

ThinPlateSpline::ThinPlateSpline(
      FloatCoordinateArray coordinate,    // use std::move if you can!
      FloatCoordinateArray const& value,  // correspondence points
      dfloat lambda
) {
   // NOTE: `source` and `destination` are already checked for sizes
   c_ = std::move( coordinate );
   dip::uint nPoints = c_.size();
   dip::uint nDims = c_[ 0 ].size();

   // Create matrices L and b
   DIP_ASSERT( value.size() == nPoints );
   dip::uint N = nPoints + nDims + 1;
   Eigen::MatrixXd L( N, N );
   L.fill( 0 );
   Eigen::MatrixXd b( N, nDims );
   b.fill( 0 );
   dfloat alpha = 0;
   for( dip::uint ii = 0; ii < nPoints; ++ii ) {
      for( dip::uint jj = 0; jj < ii; ++jj ) {
         L( ii, jj ) = L( jj, ii ); // Previously computed, L is symmetric
      }
      for( dip::uint jj = ii + 1; jj < nPoints; ++jj ) {
         dfloat d = Distance( c_[ ii ], c_[ jj ] );
         L( ii, jj ) = RadialBasis( d );
         alpha += d;
      }
      L( ii, nPoints ) = 1;
      for( dip::uint jj = 0; jj < nDims; ++jj ) {
         L( ii, nPoints + 1 + jj ) = c_[ ii ][ jj ];
         b( ii, jj ) = value[ ii ][ jj ] - c_[ ii ][ jj ];
      }
   }
   if( lambda > 0.0 ) {
      alpha /= static_cast< dfloat >( nPoints * ( nPoints - 1 ) / 2 );
      alpha *= alpha * lambda;
      for( dip::uint ii = 0; ii < nPoints; ++ii ) {
         L( ii, ii ) = alpha;
      }
   }
   for( dip::uint jj = 0; jj < nDims + 1; ++jj ) {
      L.row( nPoints + jj ) = L.col( nPoints + jj );
   }

   // Solve equation Lx=b for x
   // Using Eigen::Ref to get in-place decomposition, it re-uses L to store the decomposition.
   Eigen::HouseholderQR <Eigen::Ref< Eigen::MatrixXd >> decomposition( L );
   //Eigen::ColPivHouseholderQR <Eigen::Ref< Eigen::MatrixXd >> decomposition( L );
   // TODO: Eigen::HouseholderQR is faster but less accurate than Eigen::ColPivHouseholderQR. Which one to pick?
   x_.resize( N * nDims );
   Eigen::Map< Eigen::MatrixXd >( x_.data(), N, nDims ) = decomposition.solve( b );
}

// Evaluates the thin plate spline function at point `pt`.
FloatArray ThinPlateSpline::Evaluate( FloatArray const& pt ) {
   dip::uint nPoints = c_.size();
   dip::uint nDims = c_[ 0 ].size();
   dip::uint N = nPoints + nDims + 1;
   Eigen::Map <Eigen::MatrixXd> x( x_.data(), N, nDims );
   // Note: w( ii, jj ) = x( ii, jj ), and a( ii, jj ) = x( nPoints + ii, jj )
   FloatArray res = pt;
   for( dip::uint ii = 0; ii < nPoints; ++ii ) {
      dfloat scale = RadialBasis( Distance( pt, c_[ ii ] ));
      for( dip::uint jj = 0; jj < nDims; ++jj ) {
         res[ jj ] += x( ii, jj ) * scale;
      }
   }
   for( dip::uint jj = 0; jj < nDims; ++jj ) {
      res[ jj ] += x( nPoints, jj );
   }
   for( dip::uint ii = 0; ii < nDims; ++ii ) {
      for( dip::uint jj = 0; jj < nDims; ++jj ) {
         res[ jj ] += x( nPoints + 1 + ii, jj ) * pt[ ii ];
      }
   }
   return res;
}

} // namespace dip

#if defined(__GNUG__) || defined(__clang__)
#pragma GCC diagnostic pop
#endif


#pragma once

#include <Eigen/Dense>
#include <geometry.hpp>

// reference: https://www.kalmanfilter.net/multiSummary.html fuck http://ros-developer.com/2019/04/10/kalman-filter-explained-with-python-code-from-scratch/
// Everything is fitted for 2d tracking

class Kalman{
public:
    static const int    nx = 4,
                        nu = 2,
                        nz = 2, NUM_VARS = 1;

    //  Actual state
    Eigen::Matrix<double, nx, 1> Xn;
    // Output state
    Eigen::Matrix<double, nz, 1> Zn;
    // State transition matrix
    Eigen::Matrix<double, nx, nx> F;
    // Control matrix
    Eigen::Matrix<double, nx, nu> G;
    // Covariance matrix
    Eigen::Matrix<double, nx, nx> P;
    // Process covariance matrix
    Eigen::Matrix<double, nx, nx> Q;
    // Uncertainty matrix
    Eigen::Matrix<double, nz, nz> R;
    // Observation matrix
    Eigen::Matrix<double, nz, nx> H;
    // Kalman gain matrix
    Eigen::Matrix<double, nx, nz> K;
    // Identity matrix
    Eigen::Matrix<double, nx, nx> I;

    float dt;

    // Variable indexes
    static const int iX  = 0; // X position
    static const int iY  = 1; // Y position
    static const int idX = 2; // X velocity
    static const int idY = 3; // Y velocity

    Kalman(){
        // Prediction matrices
        F.setIdentity();
        G.setZero();
        Q.setIdentity();
        P.setIdentity();

        H <<    1, 0, 0, 0,
                0, 1, 0, 0;

        R <<    1, 0,
                0, 1;
    }

    Kalman(double iniX, double inidX, double iniY, double inidY){
        Xn(iX)  = iniX;
        Xn(idX) = inidX;
        Xn(iY)  = iniY;
        Xn(idY) = inidY;

        // Prediction matrices
        F = F.setIdentity();
        G = G.setZero();
        Q = Q.setIdentity();

        R = R.setIdentity();
        R *= 5;

        P = P.setIdentity();

        H = H.setZero();
        H(iX, iX) = 1;
        H(iY, iY) = 1;

        I = I.Identity();
    }

    void init(double iniX, double iniY, double dt){
        Xn(iX)  = iniX;
        Xn(idX) = 0;
        Xn(iY)  = iniY;
        Xn(idY) = 0;
        this->dt = dt;

        F(iX, idX) = dt;
        F(iY, idY) = dt;
        G(iX, iX) = 0.5 * dt * dt;
        G(iY, iY) = 0.5 * dt * dt;
        G(idX, iX) = dt;
        G(idY, iY) = dt;

        Q = G * 1 * G.transpose();
    }

    // // Prediction method for object tracking
    // void predict(){
    //     // Predict:   Xn+1 = F * Xn + G * Un
    //     Xn = F * Xn;

    // }

    // Prediction method for object tracking
    void predict(){
        // Predict:   Xn+1 = F * Xn + G * Un
        Xn = F * Xn;

        P  = F * P * F.transpose() + Q;
    }

    void update(int x, int y){
        // Zn
        Zn <<   x,
                y;

        // K
        K = P * H.transpose() * (H * P * H.transpose() + R).inverse();

        // Xn
        Xn = Xn + K * (Zn - H * Xn);

        // P
        Eigen::Matrix<double, nx, nx> I;
        I = I.Identity();
        // P = P * (I.Identity() - K * H);
        // P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose();
        P = P * (I - K * H);
    }

    std::pair<double, double> getPosition(){
        return {Xn(iX), Xn(iY)};
    }
};


#ifndef enhancer_hpp
#define enhancer_hpp

#include <cmath>
#include <Eigen/Core>

namespace enhancer
{
#if defined(ENHANCER_WITH_LIFT_GAMMA_GAIN)
    constexpr int NUM_PARAMETERS = 12;
#else
    constexpr int NUM_PARAMETERS = 5;
#endif

    ///////////////////////////////////////////////////////////
    // Interface
    ///////////////////////////////////////////////////////////

    inline Eigen::Vector3d enhance(const Eigen::Vector3d& input_rgb, const Eigen::VectorXd& parameters);

    ///////////////////////////////////////////////////////////
    // Implementation
    ///////////////////////////////////////////////////////////

    namespace internal
    {
        inline Eigen::Vector3d convertRgbToLinearRgb(const Eigen::Vector3d& rgb)
        {
            return rgb.array().pow(2.2).matrix();
        }

        inline Eigen::Vector3d convertLinearRgbToRgb(const Eigen::Vector3d& linear_rgb)
        {
            return linear_rgb.array().pow(1.0 / 2.2).matrix();
        }

        // Y'UV (BT.709) to linear RGB
        // Values are from https://en.wikipedia.org/wiki/YUV
        inline Eigen::Vector3d yuv2rgb(const Eigen::Vector3d& yuv)
        {
            constexpr double m[9] = { +1.00000, +1.00000, +1.00000,   // 1st column
                                      +0.00000, -0.21482, +2.12798,   // 2nd column
                                      +1.28033, -0.38059, +0.00000 }; // 3rd column
            return Eigen::Map<const Eigen::Matrix3d>(m) * yuv;
        }

        // Linear RGB to Y'UV (BT.709)
        // Values are from https://en.wikipedia.org/wiki/YUV
        inline Eigen::Vector3d rgb2yuv(const Eigen::Vector3d& rgb)
        {
            constexpr double m[9] = { +0.21260, -0.09991, +0.61500,   // 1st column
                                      +0.71520, -0.33609, -0.55861,   // 2nd column
                                      +0.07220, +0.43600, -0.05639 }; // 3rd column
            return Eigen::Map<const Eigen::Matrix3d>(m) * rgb;
        }

        inline double rgb2h(const Eigen::Vector3d& rgb)
        {
            const double r = rgb(0);
            const double g = rgb(1);
            const double b = rgb(2);
            const double M = std::max({r, g, b});
            const double m = std::min({r, g, b});

            double h;
            if (M == m)      h = 0.0;
            else if (m == b) h = 60.0 * (g - r) / (M - m) + 60.0;
            else if (m == r) h = 60.0 * (b - g) / (M - m) + 180.0;
            else if (m == g) h = 60.0 * (r - b) / (M - m) + 300.0;
            else             abort();
            h /= 360.0;
            if (h < 0.0) {
                ++ h;
            } else if (h > 1.0) {
                -- h;
            }
            return h;
        }

        inline double rgb2s4hsv(const Eigen::Vector3d& rgb)
        {
            const double r = rgb(0);
            const double g = rgb(1);
            const double b = rgb(2);
            const double M = std::max({r, g, b});
            const double m = std::min({r, g, b});

            if (M < 1e-14) return 0.0;
            return (M - m) / M;
        }

        inline double rgb2s4hsl(const Eigen::Vector3d& rgb)
        {
            const double r = rgb(0);
            const double g = rgb(1);
            const double b = rgb(2);
            const double M = std::max({r, g, b});
            const double m = std::min({r, g, b});

            if (M - m < 1e-14) return 0.0;
            return (M - m) / (1.0 - std::abs(M + m - 1.0));
        }

        inline Eigen::Vector3d hsl2rgb(const Eigen::Vector3d& hsl)
        {
            auto hue2rgb = [](const double f1, const double f2, double hue)
            {
                if (hue < 0.0) hue += 1.0;
                if (hue > 1.0) hue -= 1.0;

                double res;
                if ((6.0 * hue) < 1.0)
                    res = f1 + (f2 - f1) * 6.0 * hue;
                else if ((2.0 * hue) < 1.0)
                    res = f2;
                else if ((3.0 * hue) < 2.0)
                    res = f1 + (f2 - f1) * ((2.0 / 3.0) - hue) * 6.0;
                else
                    res = f1;
                return res;
            };

            if (hsl.y() == 0.0)
            {
                return Eigen::Vector3d(hsl.z(), hsl.z(), hsl.z());
            }

            const double f2 = (hsl.z() < 0.5) ? hsl.z() * (1.0 + hsl.y()) : (hsl.z() + hsl.y()) - (hsl.y() * hsl.z());
            const double f1 = 2.0 * hsl.z() - f2;

            Eigen::Vector3d rgb;
            rgb(0) = hue2rgb(f1, f2, hsl.x() + (1.0 / 3.0));
            rgb(1) = hue2rgb(f1, f2, hsl.x());
            rgb(2) = hue2rgb(f1, f2, hsl.x() - (1.0 / 3.0));

            return rgb;
        }

        inline Eigen::Vector3d rgb2hsv(const Eigen::Vector3d& rgb)
        {
            const double& r = rgb(0);
            const double& g = rgb(1);
            const double& b = rgb(2);

            const double M = std::max({r, g, b});

            const double h = rgb2h(rgb);
            const double s = rgb2s4hsv(rgb);
            const double v = M;

            return Eigen::Vector3d(h, s, v);
        }

        inline double rgb2l(const Eigen::Vector3d& rgb)
        {
            const double r = rgb(0);
            const double g = rgb(1);
            const double b = rgb(2);
            const double M = std::max({r, g, b});
            const double m = std::min({r, g, b});

            return 0.5 * (M + m);
        }

        inline Eigen::Vector3d hsv2rgb(const Eigen::Vector3d& hsv)
        {
            const double& h = hsv(0);
            const double& s = hsv(1);
            const double& v = hsv(2);

            if (s < 1e-14)
            {
                return Eigen::Vector3d(v, v, v);
            }

            const double h6 = h * 6.0;
            const int    i  = static_cast<int>(floor(h6)) % 6;
            const double f  = h6 - static_cast<double>(i);
            const double p  = v * (1 - s);
            const double q  = v * (1 - (s * f));
            const double t  = v * (1 - (s * (1 - f)));
            double r, g, b;
            switch(i)
            {
                case 0: r = v; g = t; b = p; break;
                case 1: r = q; g = v; b = p; break;
                case 2: r = p; g = v; b = t; break;
                case 3: r = p; g = q; b = v; break;
                case 4: r = t; g = p; b = v; break;
                case 5: r = v; g = p; b = q; break;
            }

            return Eigen::Vector3d(r, g, b);
        }

        inline Eigen::Vector3d rgb2hsl(const Eigen::Vector3d& rgb)
        {
            const double h = rgb2h(rgb);
            const double s = rgb2s4hsl(rgb);
            const double l = rgb2l(rgb);

            return Eigen::Vector3d(h, s, l);
        }

        inline float clamp(const float value) { return std::max(0.0, std::min(static_cast<double>(value), 1.0)); }
        inline Eigen::Vector3d clamp(const Eigen::Vector3d& v) { return Eigen::Vector3d(clamp(v.x()), clamp(v.y()), clamp(v.z())); }

        inline Eigen::Vector3d changeColorBalance(const Eigen::Vector3d& inputRgb, const Eigen::Vector3d& shift)
        {
            constexpr double a     = 0.250;
            constexpr double b     = 0.333;
            constexpr double scale = 0.700;

            const double          lightness = rgb2l(inputRgb);
            const Eigen::Vector3d midtones  = (clamp((lightness - b) / a + 0.5) * clamp((lightness + b - 1.0) / (- a) + 0.5) * scale) * shift;
            const Eigen::Vector3d newColor  = clamp(inputRgb + midtones);
            const Eigen::Vector3d newHsl    = rgb2hsl(newColor);

            return hsl2rgb(Eigen::Vector3d(newHsl(0), newHsl(1), lightness));
        }

        inline Eigen::Vector3d applyLiftGammaGainEffect(const Eigen::Vector3d& linear_rgb,
                                                        const Eigen::Vector3d& lift,
                                                        const Eigen::Vector3d& gamma,
                                                        const Eigen::Vector3d& gain)
        {
            const Eigen::Array3d lift_applied_linear_rgb  = ((linear_rgb.array() - Eigen::Array3d::Ones()) * (Eigen::Array3d::Constant(2.0) - lift.array()) + Eigen::Array3d::Ones()).max(0.0);
            const Eigen::Array3d gain_applied_linear_rgb  = lift_applied_linear_rgb * gain.array();
            const Eigen::Array3d gamma_applied_linear_rgb = gain_applied_linear_rgb.pow(gamma.array().inverse());

            return gamma_applied_linear_rgb.matrix();
        }

        inline Eigen::Vector3d applyTemperatureTintEffect(const Eigen::Vector3d& linear_rgb, const double temperature, const double tint)
        {
            constexpr double scale = 0.10;

            return clamp(yuv2rgb(rgb2yuv(linear_rgb) + temperature * scale * Eigen::Vector3d(0.0, -1.0, 1.0) + tint * scale * Eigen::Vector3d(0.0, 1.0, 1.0)));
        }

        inline Eigen::Vector3d applyBrightnessEffect(const Eigen::Vector3d& linear_rgb, const double brightness)
        {
            constexpr double scale = 1.5;

            return linear_rgb.array().pow(1.0 / (1.0 + scale * brightness)).matrix();
        }

        inline Eigen::Vector3d applySaturationEffect(const Eigen::Vector3d& linear_rgb, const double saturation)
        {
            const Eigen::Vector3d hsv = rgb2hsv(clamp(linear_rgb));
            const double s = clamp(hsv(1) * (saturation + 1.0));

            return hsv2rgb(Eigen::Vector3d(hsv(0), s, hsv(2)));
        }

        inline Eigen::Vector3d applyContrastEffect(const Eigen::Vector3d& linear_rgb, const double contrast)
        {
            constexpr double pi_4 = 3.14159265358979 * 0.25;

            const double contrast_coef = std::tan((contrast + 1.0) * pi_4);

            return convertRgbToLinearRgb((contrast_coef * (convertLinearRgbToRgb(linear_rgb) - Eigen::Vector3d::Constant(0.5)) + Eigen::Vector3d::Constant(0.5)).array().max(0.0));
        }

        inline Eigen::Vector3d enhance(const Eigen::Vector3d& input_rgb, const Eigen::VectorXd& parameters)
        {
            assert(parameters.size() == NUM_PARAMETERS);

            const double brightness  = clamp(parameters[0]) - 0.5;
            const double contrast    = clamp(parameters[1]) - 0.5;
            const double saturation  = clamp(parameters[2]) - 0.5;

#if defined(ENHANCER_WITH_LIFT_GAMMA_GAIN)
            const Eigen::Vector3d lift  = Eigen::Vector3d::Constant(0.5) + clamp(parameters.segment<3>(3)); // [0.5, 1.5]^3
            const Eigen::Vector3d gamma = Eigen::Vector3d::Constant(0.5) + clamp(parameters.segment<3>(6)); // [0.5, 1.5]^3
            const Eigen::Vector3d gain  = Eigen::Vector3d::Constant(0.5) + clamp(parameters.segment<3>(9)); // [0.5, 1.5]^3
#else
            const double temperature = clamp(parameters[3]) - 0.5;
            const double tint        = clamp(parameters[4]) - 0.5;
#endif

            Eigen::Vector3d linear_rgb = convertRgbToLinearRgb(input_rgb);

#if defined(ENHANCER_WITH_LIFT_GAMMA_GAIN)
            // Lift/Gamma/Gain
            linear_rgb = applyLiftGammaGainEffect(linear_rgb, lift, gamma, gain);
#else
            // Approximate temperature/tint effect
            linear_rgb = applyTemperatureTintEffect(linear_rgb, temperature, tint);
#endif

            // Brightness
            linear_rgb = applyBrightnessEffect(linear_rgb, brightness);

            // Contrast
            linear_rgb = applyContrastEffect(linear_rgb, contrast);

            // Saturation
            linear_rgb = applySaturationEffect(linear_rgb, saturation);

            return clamp(convertLinearRgbToRgb(linear_rgb));
        }

        inline Eigen::Vector3d enhance_v1(const Eigen::Vector3d& input_rgb, const Eigen::VectorXd& parameters)
        {
            assert(parameters.size() == NUM_PARAMETERS);

            const double          brightness  = parameters[0] - 0.5;
            const double          contrast    = parameters[1] - 0.5;
            const double          saturation  = parameters[2] - 0.5;
            const Eigen::Vector3d balance     = parameters.segment<3>(3) - Eigen::Vector3d::Constant(0.5);

            // color balance
            Eigen::Vector3d rgb = changeColorBalance(input_rgb, balance);

            // brightness
            rgb *= 1.0 + brightness;

            // contrast
            constexpr double pi_4 = 3.14159265358979 * 0.25;
            const double contrast_coef = std::tan((contrast + 1.0) * pi_4);
            rgb = contrast_coef * (rgb - Eigen::Vector3d::Constant(0.5)) + Eigen::Vector3d::Constant(0.5);

            // clamp
            rgb = clamp(rgb);

            // saturation
            Eigen::Vector3d hsv = rgb2hsv(rgb);
            double s = hsv.y();
            s *= saturation + 1.0;
            hsv(1) = clamp(s);
            const Eigen::Vector3d output_rgb = hsv2rgb(hsv);

            return output_rgb;
        }
    }

    inline Eigen::Vector3d enhance(const Eigen::Vector3d& input_rgb, const Eigen::VectorXd& parameters)
    {
        return internal::enhance(input_rgb, parameters);
    }
}

#endif /* enhancer_hpp */


/**@file get_initial_calib.hpp
 * @brief Header file containing required headers and methods
 *        for computing initial intrinsic calibration matrix.
 *
 * Detailed description follows here.
 * @author     : Abhinav Modi, Kartik Madhira
 *
 * Copyright 2019 MIT License
 */

#pragma once

#include <iostream>
#include <vector>
#include "Eigen/Dense"
#include "Eigen/Core"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/filesystem.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace Eigen; //NOLINT
using std::vector;
using std::cout;

 /**
  * @brief get_corners outputs corners of the input checkerboard image.
  * @param board_image  - input checkerboard image.
  * @param pattern_size - tuple(num_rows, num_columns) in the checkerboard.
  * @param show_corners - flag to enable display of input image with corners.
  * @return vector of corners points in the input checkerboard image.
  */
vector<cv::Point2f> get_corners(cv::Mat board_image, cv::Size pattern_size,
                                bool show_corners);

 /**
  * @brief get_V_matrix outputs V matrix.
  * @param corner_vector - vector of corner points of the checkerboard image.
  * @param square_size   - size of the square pattern in the checkerboard.
  * @param pattern_size  - tuple(num_rows, num_columns) in the checkerboard.
  * @return 2D matrix of shape(12,2) containing V values
  */
MatrixXf get_V_matrix(vector<cv::Point2f> corner_vector, float square_size,
                        cv::Size pattern_size);

/**
 * @brief get_homography computes homography matrix using image and world coords
 * @param world_corners - (N, 2) matrix of world coordinates
 * @param square_size   - (N, 2) matrix of image coordinates
 * @return (3, 3) homography matrix
 */
cv::Mat get_homography(cv::Mat world_corners, cv::Mat image_corners);

/**
 * @brief create_V_matrix computes V matrix using homography
 * @param world_corners - (N, 2) matrix of world coordinates
 * @param square_size   - (N, 2) matrix of image coordinates
 * @return (3, 3) homography matrix
 */
MatrixXf create_V_matrix(const cv::Mat& H);

/**
 * @brief get_vij_matrix is a helper function to compute vij matrix
 * @param vij - reference to the vij matrix for computing V matrix
 * @param H   - const reference to the Homography matrix
 * @param i   - index i for vij
 * @param j   - index j for vij
 */
void get_vij_matrix(MatrixXf& vij, const cv::Mat& H, int i, int j); //NOLINT

MatrixXf get_initial_K(const MatrixXf& V);

MatrixXf get_B_matrix(const MatrixXf& rt_eigen_matrix);

Matrix3f compute_K(const MatrixXf& B);


#include "dsl_compiler.h"
#include "placeholders.h"
#include "test_ctx_fixture.h"

#include <boost/range/algorithm.hpp>

#include <gtest/gtest.h>

namespace{
        using namespace dsl_compiler;
        using namespace dsl_compiler::placeholders;

        using FibTest = BasicCtxFixture<struct tag_test_fib>;

        TEST_F( FibTest, First20 ){

                auto max_fib = 20;

                struct tag_lag1{};
                struct tag_lag2{};
                struct tag_n{};

                auto lag1 = placeholder<tag_lag1>();
                auto lag2 = placeholder<tag_lag2>();
                auto n = placeholder<tag_n>();

                auto stmt = (
                        lag1 = 1
                      , lag2 = 1
                      , push(lag1)
                      , push(lag2)
                      , for_( n = 2, n < max_fib , n += 1 )
                        (
                                _1 = lag1 + lag2
                              , lag1 = lag2
                              , lag2 = _1
                              , push(lag2)
                        )
                );

                auto prog = compile(stmt);

                prog.execute(ctx);

                std::vector<int> known = {1,1,2,3,5,8,13,21,34,55,89,144,233,377,610,987,1597,2584,4181,6765};
                std::vector<int> result;
                boost::for_each( ctx.get_stack(), [&result](auto _){ result.emplace_back(static_cast<int>(_));});
                ASSERT_EQ( boost::equal( known , result ), true );
        }

}


//----------------------------------*-C++-*-------------------=---------------//
/**
 *  @file  test_SphericalHarmonics.cc
 *  @brief Test of SphericalHarmonics class.
 *  @note  Copyright (C) Jeremy Roberts 2012-2013
 */
//----------------------------------------------------------------------------//

// LIST OF TEST FUNCTIONS
#define TEST_LIST                                 \
        FUNC(test_SphericalHarmonics)             \
        FUNC(test_SphericalHarmonics_integration)

#include "utilities/TestDriver.hh"
#include "angle/SphericalHarmonics.hh"
#include "angle/QuadratureFactory.hh"
#ifdef DETRAN_ENABLE_BOOST
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <boost/math/special_functions/factorials.hpp>
using boost::math::spherical_harmonic_r;
using boost::math::spherical_harmonic_i;
using boost::math::factorial;
#endif
#include "utilities/Constants.hh"
#include <cstdio>
// Setup
/* ... */

using namespace detran_angle;
using namespace detran_utilities;
using namespace detran_test;
using std::cout;
using std::endl;
using std::printf;

int main(int argc, char *argv[])
{
  RUN(argc, argv);
}

//----------------------------------------------------------------------------//
// TEST DEFINITIONS
//----------------------------------------------------------------------------//

int test_SphericalHarmonics(int argc, char *argv[])
{
  double mu  = 0.350021174581540677777041;
  double eta = 0.350021174581540677777041;
  double xi  = 0.868890300722201205229788;
  TEST(soft_equiv(SphericalHarmonics::Y_lm(0, 0, mu, eta, xi), 1.0));
  TEST(soft_equiv(SphericalHarmonics::Y_lm(1,-1, mu, eta, xi), eta));
  TEST(soft_equiv(SphericalHarmonics::Y_lm(1, 0, mu, eta, xi), xi));
  TEST(soft_equiv(SphericalHarmonics::Y_lm(1, 1, mu, eta, xi), mu));
  return 0;
}

/*
 *  This test compares the accuracy of 3-D quadratures.  In
 *  particular, we test a quadrature on the following
 *  moments:
 *
 *    (2/pi)int( cos(phi)^m * sin(phi)^(l+1) * sin(theta)^l, phi=0..pi/2, theta=0..pi/2)
 *
 *    l, m   even
 *
 *  = 1/(m+1)                                 for l = 0
 *    (l/2-1/2)!(m/2-1/2)! / (l/2+m/2+1/2)!   for l >= 0
 *
 *  The level symmetric quadrature integrates these exactly for
 *  N >= k + l, where N is the order (= twice the polar/octant)
 *
 *  This could be made a public function at some point.
 */
int test_SphericalHarmonics_integration(int argc, char *argv[])
{
  InputDB::SP_input inp(new InputDB());
  inp->put<int>("quad_number_polar_octant",   4);
  inp->put<int>("quad_number_azimuth_octant", 4);
  inp->put<std::string>("quad_type", "asqr-asdr");
  QuadratureFactory::SP_quadrature Q = QuadratureFactory::build(inp, 3);

  int L = 6;

  vec2_dbl err(L + 1, vec_dbl(L + 1, 0.0));

  for (int ll = 0; ll <= L; ++ll)
  {
    int l = 2*ll;
    for (int mm = 0; mm <= L; ++mm)
    {
      int m = 2*mm;

      // Compute the reference
      double ref;
      if (l == 0)
      {
        ref = 1.0 / (m + 1.0);
      }
      else
      {
        ref = 1.0 / (l + m + 1.0);
        for (int i = 1; i < l; i+=2)
        {
          double den = m + i;
          ref *= (double)i / den;
        }
      }
      ref *= four_pi;

      double val = 0.0;
      for (int o = 0; o < 8; ++o)
      {
        for (int a = 0; a < Q->number_angles_octant(); ++a)
        {
          double mu  = Q->mu(o, a);
          //double eta = Q->eta(o, a);
          double xi  = Q->xi(o, a);
          val += Q->weight(a) * std::pow(mu, m) * std::pow(xi, l);
        }
      }
      err[ll][mm] = 100.0*(val-ref)/ref;
    }
  }

  // Print a table of errors
  cout << " NUMBER OF ANGLES = " << Q->number_angles() << endl;
  printf("  l \\ m ");
  for (int mm = 0; mm <= L; ++mm)
    printf("  %3i         ", 2*mm);
  printf("\n ------");
  for (int mm = 0; mm <= L; ++mm)
    printf("--------------");
  printf("\n");
  for (int ll = 0; ll <= L; ++ll)
  {
    int l = 2*ll;
    printf(" %3i |", l);
    for (int mm = 0; mm <= L; ++mm)
    {
      int m = 2*mm;
      printf(" %12.6f ", err[ll][mm]);
    }
    printf("\n");
  }

  return 0;
}

//----------------------------------------------------------------------------//
//              end of test_SphericalHarmonics.cc
//----------------------------------------------------------------------------//