/** \file oaunittest.cpp

C++ program: oaunittest

oaunittest: run some tests on the code

Author: Pieter Eendebak <pieter.eendebak@gmail.com>
Copyright: See LICENSE.txt file that comes with this distribution
*/

#include <algorithm>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>

#include "lmc.h"
#include "Deff.h"
#include "anyoption.h"
#include "arrayproperties.h"
#include "arraytools.h"
#include "conference.h"
#include "extend.h"
#include "tools.h"

#include "graphtools.h"
#include "unittests.h"

#include "Eigen/Dense"

#ifdef HAVE_BOOST
#include <boost/filesystem.hpp>
#include <string>
#endif

// functions with no public header
array_link createJdtable(const array_link &al);

enum { UNITTEST_SUCCESS, UNITTEST_FAIL };

/** unittest for oapackage
 *
 * Returns UNITTEST_SUCCESS if all tests are ok.
 *
 */
int oaunittest (int verbose, int writetests = 0, int randval = 0) {
        double t0 = get_time_ms ();
        const char *bstr = "OA unittest";
        cprintf (verbose, "%s: start\n", bstr);

        srand (randval);

        int allgood = UNITTEST_SUCCESS;

        Combinations::initialize_number_combinations (20);

        /* constructors */
        {
                cprintf (verbose, "%s: interaction matrices\n", bstr);

                array_link al = exampleArray (2);
                Eigen::MatrixXd m1 = array2xfeigen (al);
                Eigen::MatrixXd m2 = arraylink2eigen (array2xf (al));

                Eigen::MatrixXd dm = m1 - m2;
                int sum = dm.sum ();

                myassert (sum == 0, "unittest error: construction of interaction matrices\n");
        }

		cprintf(verbose, "%s: reduceConferenceTransformation\n", bstr);
		myassert(unittest_reduceConferenceTransformation()==0, "unittest unittest_reduceConferenceTransformation failed");

        /* constructors */
        {
                cprintf (verbose, "%s: array manipulation operations\n", bstr);

                test_array_manipulation (verbose);
        }

        /* double conference matrices */
        {
                cprintf (verbose, "%s: double conference matrices\n", bstr);

                array_link al = exampleArray (36, verbose);
                myassert (al.is_conference (2), "check on double conference design type");

                myassert (testLMC0checkDC (al, verbose >= 2), "testLMC0checkDC");

        }

        /* conference matrices */
        {
                cprintf (verbose, "%s: conference matrices\n", bstr);

                int N = 4;
                conference_t ctype (N, N, 0);

                arraylist_t kk;
                array_link al = ctype.create_root ();
                kk.push_back (al);

                for (int extcol = 2; extcol < N; extcol++) {
                        kk = extend_conference (kk, ctype, 0);
                }
                myassert (kk.size () == 1, "unittest error: conference matrices for N=4\n");
        }

        {
                cprintf (verbose, "%s: generators for conference matrix extensions\n", bstr);
                test_conference_candidate_generators (verbose);
        }

        {
                cprintf(verbose, "%s: conference matrix Fvalues\n", bstr);
                std::vector< int > jj5 = Jcharacteristics_conference(exampleArray(18, 0), 5);
                myassert(jj5 == std::vector<int>{ -1, -1, -1, -5, 11, -3, 3, 3, -1, -1, 1, -1, 3, -1, 1, 7, 3, 5, -1, 1, 1}, "Jcharacteristics_conference result incorrect");

                array_link al = exampleArray (22, 0);
                if (verbose >= 2)
                        al.show ();
                std::vector< int > jj3 = Jcharacteristics_conference(al, 3);
                myassert(jj3 == std::vector<int> { 0, 0, 0, 0 }, "Jcharacteristics_conference result incorrect");

                const int N = al.n_rows;
                jstructconference_t js (N, 4);
                std::vector< int > f4 = al.FvaluesConference (4);
                std::vector< int > j4 = js.Jvalues ();

                if (verbose >= 2) {
                        printf ("j4: ");
                        display_vector (j4);
                        printf ("\n");
                        printf ("F4: ");
                        display_vector (f4);
                        printf ("\n");
                }

                myassert (j4[0] == 28, "unittest error: conference matricex F values: j4[0]\n");
                myassert (f4[0] == 0, "unittest error: conference matricex F values: f4[0] \n");
                myassert (f4[1] == 0, "unittest error: conference matricex F values: j4[1]\n");
        }

        {
                cprintf (verbose, "%s: LMC0 check for arrays in C(4, 3)\n", bstr);

                array_link al = exampleArray (28, 1);
                if (verbose >= 2)
                        al.showarray ();
                lmc_t r = LMC0check (al, verbose);
                if (verbose >= 2)
                        printf ("LMC0check: result %d\n", r);
                myassert (r >= LMC_EQUAL, "LMC0 check\n");

                al = exampleArray (29, 1);
                if (verbose >= 2)
                        al.showarray ();
                r = LMC0check (al, verbose);
                if (verbose >= 2)
                        printf ("LMC0check: result %d (LMC_LESS %d)\n", r, LMC_LESS);
                myassert (r == LMC_LESS, "LMC0 check of example array 29\n");
        }

        {
                cprintf (verbose, "%s: LMC0 check\n", bstr);

                array_link al = exampleArray (31, 1);
                if (verbose >= 2)
                        al.showarray ();
                conference_transformation_t T (al);

                for (int i = 0; i < 80; i++) {
                        T.randomize ();
                        array_link alx = T.apply (al);

                        lmc_t r = LMC0check (alx, verbose);

                        if (verbose >= 2) {
                                printfd ("%d: transformed array: r %d\n", i, r);
                                alx.showarray ();
                        }
                        if (alx == al)
                                myassert (r >= LMC_EQUAL, "result should be LMC_MORE\n");
                        else {
                                myassert (r == LMC_LESS, "result should be LMC_LESS\n");
                        }
                }
        }

        {
                cprintf (verbose, "%s: random transformation for conference matrices\n", bstr);

                array_link al = exampleArray (19, 1);
                conference_transformation_t T (al);
                // T.randomizerowflips();
                T.randomize ();

                conference_transformation_t Ti = T.inverse ();
                array_link alx = Ti.apply (T.apply (al));

                myassert (alx == al, "transformation of conference matrix\n");
        }

        /* constructors */
        {
                cprintf (verbose, "%s: constructors\n", bstr);

                array_transformation_t t;
                conference_transformation_t ct;
        }

        /* J-characteristics */
        {
                cprintf (verbose, "%s: J-characteristics\n", bstr);

                array_link al = exampleArray (8, 1);

                const int mm[] = {-1, -1, 0, 0, 8, 16, 0, -1};

                for (int jj = 2; jj < 7; jj++) {
                        std::vector< int > jx = al.Jcharacteristics (jj);
                        int j5max = vectormax (jx, 0);
                        if (verbose >= 2) {
                                printf ("oaunittest: jj %d: j5max %d\n", jj, j5max);
                        }

                        if (j5max != mm[jj]) {
                                printfd ("j5max %d (should be %d)\n", j5max, mm[jj]);
                                allgood = UNITTEST_FAIL;
                                return allgood;
                        }
                }
        }
        {
                cprintf (verbose, "%s: array transformations\n", bstr);

                const int N = 9;
                const int t = 3;
                arraydata_t adataX (3, N, t, 4);

                array_link al (adataX.N, adataX.ncols, -1);
                al.create_root (adataX);

                if (checkTransformationInverse (al))
                        allgood = UNITTEST_FAIL;

                if (checkTransformationComposition (al, verbose >= 2))
                        allgood = UNITTEST_FAIL;

                al = exampleArray (5, 1);
                if (checkTransformationInverse (al))
                        allgood = UNITTEST_FAIL;

                if (checkTransformationComposition (al))
                        allgood = UNITTEST_FAIL;

                for (int i = 0; i < 15; i++) {
                        al = exampleArray (18, 0);
                        if (checkConferenceComposition (al))
                                allgood = UNITTEST_FAIL;
                        if (checkConferenceInverse (al))
                                allgood = UNITTEST_FAIL;
                        al = exampleArray (19, 0);
                        if (checkConferenceComposition (al))
                                allgood = UNITTEST_FAIL;
                        if (checkConferenceInverse (al))
                                allgood = UNITTEST_FAIL;
                }
        }

        {
                cprintf (verbose, "%s: rank \n", bstr);

                const int idx[10] = {0, 1, 2, 3, 4, 6, 7, 8, 9};
                const int rr[10] = {4, 11, 13, 18, 16, 4, 4, 29, 29};
                for (int ii = 0; ii < 9; ii++) {
                        array_link al = exampleArray (idx[ii], 0);
                        myassert (al.is2level (), "unittest error: input array is not 2-level\n");

                        int r = arrayrankColPivQR (array2xf (al));

                        int r3 = (array2xf (al)).rank ();
                        myassert (r == r3, "unittest error: rank of array");

                        if (verbose >= 2) {
                                al.showarray ();
                                printf ("unittest: rank of array %d: %d\n", idx[ii], r);
                        }

                        myassert (rr[ii] == r, "unittest error: rank of example matrix\n");
                }
        }

        {
                cprintf (verbose, "%s: Doptimize \n", bstr);
                const int N = 40;
                const int t = 0;
                arraydata_t arrayclass (2, N, t, 6);
                std::vector< double > alpha (3);
                alpha[0] = 1;
                alpha[1] = 1;
                alpha[2] = 0;
                int niter = 5000;
                double t00 = get_time_ms ();
                DoptimReturn rr = Doptimize (arrayclass, 10, alpha, 0, DOPTIM_AUTOMATIC, niter);

                array_t ss[7] = {3, 3, 2, 2, 2, 2, 2};
                arraydata_t arrayclassmixed (ss, 36, t, 5);
                rr = Doptimize (arrayclassmixed, 10, alpha, 0, DOPTIM_AUTOMATIC, niter);

                cprintf (verbose, "%s: Doptimize time %.3f [s] \n", bstr, get_time_ms () - t00);
        }

        {
                cprintf (verbose, "%s: J-characteristics for conference matrix\n", bstr);

                array_link al = exampleArray (19, 0);
                std::vector< int > j2 = Jcharacteristics_conference (al, 2);
                std::vector< int > j3 = Jcharacteristics_conference (al, 3);

                myassert (j2[0] == 0, "j2 value incorrect");
                myassert (j2[1] == 0, "j2 value incorrect");
                myassert (std::abs (j3[0]) == 1, "j3 value incorrect");

                if (verbose >= 2) {
                        al.showarray ();
                        printf ("j2: ");
                        display_vector (j2);
                        printf ("\n");
                        printf ("j3: ");
                        display_vector (j3);
                        printf ("\n");
                }
        }

        {
                // test PEC sequence
                cprintf (verbose, "%s: PEC sequence\n", bstr);
                for (int ii = 0; ii < 5; ii++) {
                        array_link al = exampleArray (ii, 0);
                        std::vector< double > pec = PECsequence (al);
                        printf ("oaunittest: PEC for array %d: ", ii);
                        display_vector (pec);
                        printf (" \n");
                }
        }

        {
                cprintf (verbose, "%s: D-efficiency test\n", bstr);
                //  D-efficiency near-zero test
                {
                        array_link al = exampleArray (14);
                        double D = al.Defficiency ();
                        std::vector< double > dd = al.Defficiencies ();
                        printf ("D %f, D (method 2) %f\n", D, dd[0]);
                        assert (fabs (D - dd[0]) < 1e-4);
                }
                {
                        array_link al = exampleArray (15);
                        double D = al.Defficiency ();
                        std::vector< double > dd = al.Defficiencies ();
                        printf ("D %f, D (method 2) %f\n", D, dd[0]);
                        assert (fabs (D - dd[0]) < 1e-4);
                        assert (fabs (D - 0.335063) < 1e-3);
                }
        }

        arraydata_t adata (2, 20, 2, 6);
        OAextend oaextendx;
        oaextendx.setAlgorithm ((algorithm_t)MODE_ORIGINAL, &adata);

        std::vector< arraylist_t > aa (adata.ncols + 1);
        printf ("OA unittest: create root array\n");
        create_root (&adata, aa[adata.strength]);

        /** Test extend of arrays **/
        {
                cprintf (verbose, "%s: extend arrays\n", bstr);

                setloglevel (SYSTEM);

                for (int kk = adata.strength; kk < adata.ncols; kk++) {
                        aa[kk + 1] = extend_arraylist (aa[kk], adata, oaextendx);
                        printf ("  extend: column %d->%d: %ld->%ld arrays\n", kk, kk + 1, aa[kk].size (),
                                aa[kk + 1].size ());
                }

                if (aa[adata.ncols].size () != 75) {
                        printf ("extended ?? to %d arrays\n", (int)aa[adata.ncols].size ());
                }
                myassert (aa[adata.ncols].size () == 75, "number of arrays is incorrect");

                aa[adata.ncols].size ();
                setloglevel (QUIET);
        }

        {
                cprintf (verbose, "%s: test LMC check\n", bstr);

                array_link al = exampleArray (1, 1);

                lmc_t r = LMCcheckOriginal (al);

                myassert (r != LMC_LESS, "LMC check of array in normal form");

                for (int i = 0; i < 20; i++) {
                        array_link alx = al.randomperm ();
                        if (alx == al)
                                continue;
                        lmc_t r = LMCcheckOriginal (alx);

                        myassert (r == LMC_LESS, "randomized array cannot be in minimal form");
                }
        }

        {
                /** Test dof **/
                cprintf (verbose, "%s: test delete-one-factor reduction\n", bstr);

                array_link al = exampleArray (4);
                cprintf (verbose >= 2, "LMC: \n");
                al.reduceLMC ();
                cprintf (verbose >= 2, "DOP: \n");
                al.reduceDOP ();
        }

        arraylist_t lst;

        {
                /** Test different methods **/
                cprintf (verbose, "%s: test 2 different methods\n", bstr);

                const int s = 2;
                arraydata_t adata (s, 32, 3, 10);
                arraydata_t adata2 (s, 32, 3, 10);
                OAextend oaextendx;
                oaextendx.setAlgorithm ((algorithm_t)MODE_ORIGINAL, &adata);
                OAextend oaextendx2;
                oaextendx2.setAlgorithm ((algorithm_t)MODE_LMC_2LEVEL, &adata2);

                printf ("OA unittest: test 2-level algorithm on %s\n", adata.showstr ().c_str ());
                std::vector< arraylist_t > aa (adata.ncols + 1);
                create_root (&adata, aa[adata.strength]);
                std::vector< arraylist_t > aa2 (adata.ncols + 1);
                create_root (&adata, aa2[adata.strength]);

                setloglevel (SYSTEM);

                for (int kk = adata.strength; kk < adata.ncols; kk++) {
                        aa[kk + 1] = extend_arraylist (aa[kk], adata, oaextendx);
                        aa2[kk + 1] = extend_arraylist (aa2[kk], adata2, oaextendx2);
                        printf ("  extend: column %d->%d: %ld->%ld arrays, 2-level method %ld->%ld arrays\n", kk,
                                kk + 1, (long) aa[kk].size (), (long)aa[kk + 1].size (), aa2[kk].size (), aa2[kk + 1].size ());

                        if (aa[kk + 1] != aa2[kk + 1]) {
                                printf ("oaunittest: error: 2-level algorithm unequal to original algorithm\n");
                                exit (1);
                        }
                }
                setloglevel (QUIET);

                lst = aa[8];
        }

        {
                cprintf (verbose, "%s: rank calculation using rankStructure\n", bstr);

                for (int i = 0; i < 27; i++) {
                        array_link al = exampleArray (i, 0);
                        if (al.n_columns < 5)
                                continue;
                        al = exampleArray (i, 1);

                        rankStructure rs;
                        rs.verbose = 0;
                        int r = array2xf (al).rank ();
                        int rc = rs.rankxf (al);
                        if (verbose >= 2) {
                                printf ("rank of example array %d: %d %d\n", i, r, rc);
                                if (verbose >= 3) {
                                        al.showproperties ();
                                }
                        }
                        myassert (r == rc, "rank calculations");
                }
        }
        {
                cprintf (verbose, "%s: test dtable creation\n", bstr);

                for (int i = 0; i < 4; i++) {
                        array_link al = exampleArray (5);
                        array_link dtable = createJdtable (al);
                }
        }

        {
                cprintf (verbose, "%s: test Pareto calculation\n", bstr);
                double t0x = get_time_ms ();

                int nn = lst.size ();
                for (int k = 0; k < 5; k++) {
                        for (int i = 0; i < nn; i++) {
                                lst.push_back (lst[i]);
                        }
                }
                Pareto< mvalue_t< long >, long > r = parsePareto (lst, 1);
                cprintf (verbose, "%s: test Pareto %d/%d: %.3f [s]\n", bstr, r.number (), r.numberindices (),
                         (get_time_ms () - t0x));
        }

        {
                cprintf (verbose, "%s: check reduction transformation\n", bstr);
                array_link al = exampleArray (6).reduceLMC ();

                arraydata_t adata = arraylink2arraydata (al);
                LMCreduction_t reduction (&adata);
                reduction.mode = OA_REDUCE;

                reduction.init_state = COPY;
                OAextend oaextend;
                oaextend.setAlgorithm (MODE_ORIGINAL, &adata);
                array_link alr = al.randomperm ();

                array_link al2 = reduction.transformation->apply (al);

                lmc_t tmp = LMCcheck (alr, adata, oaextend, reduction);

                array_link alx = reduction.transformation->apply (alr);

                bool c = alx == al;
                if (!c) {
                        printf ("oaunittest: error: reduction of randomized array failed!\n");
                        printf ("-- al \n");
                        al.showarraycompact ();
                        printf ("-- alr \n");
                        alr.showarraycompact ();
                        printf ("-- alx \n");
                        alx.showarraycompact ();
                        allgood = UNITTEST_FAIL;
                }
        }

        {
                cprintf (verbose, "%s: reduce randomized array\n", bstr);
                array_link al = exampleArray (3);

                arraydata_t adata = arraylink2arraydata (al);
                LMCreduction_t reduction (&adata);

                for (int ii = 0; ii < 50; ii++) {
                        reduction.transformation->randomize ();
                        array_link al2 = reduction.transformation->apply (al);

                        array_link alr = al2.reduceLMC ();

                        bool c = (al == alr);
                        if (!c) {
                                printf ("oaunittest: error: reduction of randomized array failed!\n");
                                allgood = UNITTEST_FAIL;
                        }
                }
        }

        /* Calculate symmetry group */
        {
                cprintf (verbose, "%s: calculate symmetry group\n", bstr);

                array_link al = exampleArray (2);
                symmetry_group sg = al.row_symmetry_group ();
                assert (sg.permsize () == sg.permsize_large ().toLong ());

                // symmetry_group
                std::vector< int > vv;
                vv.push_back (0);
                vv.push_back (0);
                vv.push_back (1);
                symmetry_group sg2 (vv);
                assert (sg2.permsize () == 2);
                if (verbose >= 2)
                        printf ("sg2: %ld\n", sg2.permsize ());
                assert (sg2.ngroups == 2);
        }

        /* Test efficiencies */
        {
                cprintf (verbose, "%s: efficiencies\n", bstr);

                std::vector< double > d;
                int vb = 1;

                array_link al;
                if (1) {
                        al = exampleArray (9, vb);
                        al.showproperties ();
                        d = al.Defficiencies (0, 1);
                        if (verbose >= 2)
                                printf ("  efficiencies: D %f Ds %f D1 %f Ds0 %f\n", d[0], d[1], d[2], d[3]);
                        if (fabs (d[0] - al.Defficiency ()) > 1e-10) {
                                printf ("oaunittest: error: Defficiency not good!\n");
                                allgood = UNITTEST_FAIL;
                        }
                }
                al = exampleArray (8, vb);
                al.showproperties ();
                d = al.Defficiencies ();
                if (verbose >= 2)
                        printf ("  efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
                if (fabs (d[0] - al.Defficiency ()) > 1e-10) {
                        printf ("oaunittest: error: Defficiency of examlple array 8 not good!\n");
                }

                al = exampleArray (13, vb);
                if (verbose >= 3) {
                        al.showarray ();
                        al.showproperties ();
                }
                d = al.Defficiencies (0, 1);
                if (verbose >= 2)
                        printf ("  efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);

                if ((fabs (d[0] - 0.939014) > 1e-4) || (fabs (d[3] - 0.896812) > 1e-4) || (fabs (d[2] - 1) > 1e-4)) {
                        printf ("ERROR: D-efficiencies of example array 13 incorrect! \n");
                        d = al.Defficiencies (2, 1);
                        printf ("  efficiencies: D %f Ds %f D1 %f Ds0 %f\n", d[0], d[1], d[2], d[3]);

                        allgood = UNITTEST_FAIL;
                        exit (1);
                }

                for (int ii = 11; ii < 12; ii++) {
                        printf ("ii %d: ", ii);
                        al = exampleArray (ii, vb);
                        al.showarray ();
                        al.showproperties ();

                        d = al.Defficiencies ();
                        if (verbose >= 2)
                                printf ("  efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
                }
        }
        {
                cprintf (verbose, "%s: test robustness\n", bstr);

                array_link A (0, 8, 0);
                printf ("should return an error\n  ");
                A.Defficiencies ();

                A = array_link (1, 8, 0);
                printf ("should return an error\n  ");
                A.at (0, 0) = -2;
                A.Defficiencies ();
        }

        {
                cprintf (verbose, "%s: test nauty\n", bstr);

                array_link alr = exampleArray (7, 0);
                if (unittest_nautynormalform (alr, 1) == 0) {
                        printf ("oaunittest: error: unittest_nautynormalform returns an error!\n");
                }
        }

#ifdef HAVE_BOOST
        if (writetests) {
                cprintf (verbose, "OA unittest: reading and writing of files\n");

                boost::filesystem::path tmpdir = boost::filesystem::temp_directory_path ();
                boost::filesystem::path temp = boost::filesystem::unique_path ("test-%%%%%%%.oa");

                const std::string tempstr = (tmpdir / temp).native ();

                if (verbose >= 2)
                        printf ("generate text OA file: %s\n", tempstr.c_str ());

                int nrows = 16;
                int ncols = 8;
                int narrays = 10;
                arrayfile_t afile (tempstr.c_str (), nrows, ncols, narrays, ATEXT);
                for (int i = 0; i < narrays; i++) {
                        array_link al (nrows, ncols, array_link::INDEX_DEFAULT);
                        afile.append_array (al);
                }
                afile.closefile ();

                arrayfile_t af (tempstr.c_str (), 0);
                std::cout << "  " << af.showstr () << std::endl;
                af.closefile ();

                // check read/write of binary file

                arraylist_t ll0;
                ll0.push_back (exampleArray (7));
                ll0.push_back (exampleArray (7).randomcolperm ());
                writearrayfile (tempstr.c_str (), ll0, ABINARY);
                arraylist_t ll = readarrayfile (tempstr.c_str ());
                myassert (ll0.size () == ll.size (), "read and write of arrays: size of list");
                for (size_t i = 0; i < ll0.size (); i++) {
                        myassert (ll0[i] == ll[i], "read and write of arrays: array unequal");
                }

                ll0.resize (0);
                ll0.push_back (exampleArray (24));
                writearrayfile (tempstr.c_str (), ll0, ABINARY_DIFFZERO);
                ll = readarrayfile (tempstr.c_str ());
                myassert (ll0.size () == ll.size (), "read and write of arrays: size of list");
                for (size_t i = 0; i < ll0.size (); i++) {
                        myassert (ll0[i] == ll[i], "read and write of arrays: array unequal");
                }
        }

#endif

        {
                cprintf (verbose, "OA unittest: test nauty\n");
                array_link al = exampleArray (5, 2);
                arraydata_t arrayclass = arraylink2arraydata (al);

                for (int i = 0; i < 20; i++) {
                        array_link alx = al;
                        alx.randomperm ();
                        array_transformation_t t1 = reduceOAnauty (al);
                        array_link alr1 = t1.apply (al);

                        array_transformation_t t2 = reduceOAnauty (alx);
                        array_link alr2 = t2.apply (alx);

                        if (alr1 != alr2)
                                printf ("oaunittest: error: Nauty reductions unequal!\n");
                        allgood = UNITTEST_FAIL;
                }
        }

        cprintf (verbose, "OA unittest: complete %.3f [s]!\n", (get_time_ms () - t0));
        cprintf (verbose, "OA unittest: also run ptest.py to perform checks!\n");

        if (allgood) {
                printf ("OA unittest: all tests ok\n");
                return UNITTEST_SUCCESS;
        } else {
                printf ("OA unittest: ERROR!\n");
                return UNITTEST_FAIL;
        }
}

/**
* @brief Read in files with arrays and join them into a single file
* @param argc
* @param argv[]
* @return
*/
int main (int argc, char *argv[]) {

        AnyOption opt;
        opt.setFlag ("help", 'h'); /* a flag (takes no argument), supporting long and short form */
        opt.setOption ("verbose", 'v');
        opt.setOption ("random", 'r');

        opt.addUsage ("OA: unittest: Perform some checks on the code");
        opt.addUsage ("Usage: unittest [OPTIONS]");
        opt.addUsage ("");
        opt.addUsage (" -v  --verbose  			Print documentation");
        opt.addUsage (" -r  --random  			Seed for random number generator");

        opt.processCommandArgs (argc, argv);
        int verbose = opt.getIntValue ('v', 1);
        int random = opt.getIntValue ('r', 0);

        if (opt.getFlag ("help") || opt.getFlag ('h')) {
                opt.printUsage ();
                exit (0);
        }

        if (verbose) {
                print_copyright ();
        }
        if (verbose >= 2) {
                print_options (std::cout);
        }

        oaunittest (verbose, 1, random);

        return 0;
}
// kate: indent-mode cstyle; indent-width 5; replace-tabs on;


/*
 * EllipseIterator.hpp
 *
 *  Created on: Dec 2, 2015
 *      Author: Péter Fankhauser
 *   Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "grid_map_core/iterators/EllipseIterator.hpp"
#include "grid_map_core/GridMapMath.hpp"

#include <math.h>
#include <Eigen/Geometry>

using namespace std;

namespace grid_map {

EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation)
    : center_(center)
{
  semiAxisSquare_ = (0.5 * length).square();
  double sinRotation = sin(rotation);
  double cosRotation = cos(rotation);
  transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
  mapLength_ = gridMap.getLength();
  mapPosition_ = gridMap.getPosition();
  resolution_ = gridMap.getResolution();
  bufferSize_ = gridMap.getSize();
  bufferStartIndex_ = gridMap.getStartIndex();
  Index submapStartIndex;
  Index submapBufferSize;
  findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
  internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
  if(!isInside()) ++(*this);
}

EllipseIterator& EllipseIterator::operator =(const EllipseIterator& other)
{
  center_ = other.center_;
  semiAxisSquare_ = other.semiAxisSquare_;
  transformMatrix_ = other.transformMatrix_;
  internalIterator_ = other.internalIterator_;
  mapLength_ = other.mapLength_;
  mapPosition_ = other.mapPosition_;
  resolution_ = other.resolution_;
  bufferSize_ = other.bufferSize_;
  bufferStartIndex_ = other.bufferStartIndex_;
  return *this;
}

bool EllipseIterator::operator !=(const EllipseIterator& other) const
{
  return (internalIterator_ != other.internalIterator_);
}

const Eigen::Array2i& EllipseIterator::operator *() const
{
  return *(*internalIterator_);
}

EllipseIterator& EllipseIterator::operator ++()
{
  ++(*internalIterator_);
  if (internalIterator_->isPastEnd()) return *this;

  for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
    if (isInside()) break;
  }

  return *this;
}

bool EllipseIterator::isPastEnd() const
{
  return internalIterator_->isPastEnd();
}

bool EllipseIterator::isInside()
{
  Position position;
  getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
  double value = ((transformMatrix_ * (position - center_)).array().square() / semiAxisSquare_).sum();
  return (value <= 1);
}

void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation,
                                           Index& startIndex, Size& bufferSize) const
{
  const Eigen::Rotation2Dd rotationMatrix(rotation);
  Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
  Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
  const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
  Position topLeft = center.array() + boundingBoxHalfLength;
  Position bottomRight = center.array() - boundingBoxHalfLength;
  limitPositionToRange(topLeft, mapLength_, mapPosition_);
  limitPositionToRange(bottomRight, mapLength_, mapPosition_);
  getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
  Index endIndex;
  getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
  bufferSize = endIndex - startIndex + Eigen::Array2i::Ones();
}

} /* namespace grid_map */


#include <iostream>
#include <fstream>
#include <string>
#include <math.h>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/program_options.hpp>

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_data_structure_3.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Delaunay_triangulation_3<K>                   Triangulation;
typedef Triangulation::Point                                Point;

// using a global for this is not good. I really should be passing it
// some other way.
double scale_z=1.0;

// these two functions are for my readvtk function.
Triangulation::Vertex_handle
add_vertex(Triangulation &T, std::string& vname, double x, double y, double z)
{
  // the exp(z) scaling shouldn't be done here.
  // max value of z is 1000. in that case, z/1000 =1.
  // which leaves us with exp(log(1000) which == 1000
  double newz = scale_z*exp(log(1000.0)*z/1000.0);;
  return T.insert(Point(x,y,newz));
}

void add_edge(Triangulation &T,
               Triangulation::Vertex_handle s,
               Triangulation::Vertex_handle t)
{
  // no edges to add. CGAL does that.
}

#include <readvtk.hxx>

struct VertexData {
  std::string name;
  double x,y,z;
};

struct EdgeData {
  double distance;
};

typedef boost::adjacency_list<boost::vecS, boost::vecS,
                              boost::undirectedS,
                              VertexData,
                              boost::property<boost::edge_weight_t, double, EdgeData>
                              > MyGraphType;

// these are needed by readvtk
typedef typename boost::graph_traits<MyGraphType>::vertex_descriptor vertex_descriptor;
typedef typename boost::graph_traits<MyGraphType>::edge_descriptor   edge_descriptor;
typename boost::graph_traits<MyGraphType>::vertex_descriptor
add_vertex(MyGraphType &G, std::string& vname, double x, double y, double z)
{
  typedef typename boost::graph_traits<MyGraphType>::vertex_descriptor vertex_descriptor;
  vertex_descriptor v = add_vertex(G);
  G[v].x = x;
  G[v].y = y;
  G[v].z = z;
  return v;
}

inline
double distance(MyGraphType &G,
                typename boost::graph_traits<MyGraphType>::vertex_descriptor v1,
                typename boost::graph_traits<MyGraphType>::vertex_descriptor v2)
{
  return sqrt((G[v1].x - G[v2].x)*(G[v1].x - G[v2].x) +
              (G[v1].y - G[v2].y)*(G[v1].y - G[v2].y) +
              (G[v1].z - G[v2].z)*(G[v1].z - G[v2].z));
}

typename boost::graph_traits<MyGraphType>::edge_descriptor
add_edge(MyGraphType &G,
         typename boost::graph_traits<MyGraphType>::vertex_descriptor v1,
         typename boost::graph_traits<MyGraphType>::vertex_descriptor v2)
{
  typedef typename boost::graph_traits<MyGraphType>::edge_descriptor edge_descriptor;
  edge_descriptor e = add_edge(v1, v2, G).first;
  boost::property_map<MyGraphType, boost::edge_weight_t>::type weightmap = get(boost::edge_weight, G);
  weightmap[e] = distance(G, v1, v2);
  return e;
}

#include <readvtk.hxx>

enum MSTAlgorithm { PRIM, KRUSKAL };
std::istream& operator>>(std::istream& in, MSTAlgorithm &format)
{
    std::string token;
    in >> token;
    if (token == "prim")
        format = PRIM;
    else if (token == "kruskal")
        format = KRUSKAL;
    else
        in.setstate(std::ios_base::failbit);
    return in;
}

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

  namespace po = boost::program_options;
  po::options_description desc("Usage");

  std::string filename;
  desc.add_options()
    ("help", "produce help message")
    ("filename", po::value<std::string>(&filename)->default_value(""),
     "filename containing input points");

  MSTAlgorithm mst_algorithm;
    desc.add_options()
        ("algorithm", po::value<MSTAlgorithm>(&mst_algorithm)->default_value(KRUSKAL),
         "which output format");

    desc.add_options()
        ("scale", po::value<double>(&scale_z)->default_value(1.0),
         "scale factor");

  po::variables_map opts;
  po::store(po::parse_command_line(argc, argv, desc), opts);

  try {
    po::notify(opts);
  } catch (std::exception& e) {
    std::cerr << "Error: " << e.what() << "\n";
    return 1;
  }

  if (filename == "") {
    std::cerr << "please provide a vtk file with the --filename <file> option" << std::endl;
    exit(-1);
  }
  std::ifstream input(filename.c_str());

  std::cerr << "using scale factor " << scale_z << std::endl;

  Triangulation T;
  readvtk<Triangulation,Triangulation::Vertex_handle>(input, T);
  assert( T.is_valid() ); // checking validity of T

  // at this point, the points have been loaded and the triangulation is done.

  MyGraphType G;

  std::map<Point, MyGraphType::vertex_descriptor> vertex_map;

  Triangulation::Finite_vertices_iterator viter;
  for (viter =  T.finite_vertices_begin();
       viter != T.finite_vertices_end();
       viter++) {
    Triangulation::Triangulation_data_structure::Vertex v = *viter;
    Point p = v.point();
    double x = CGAL::to_double(p.x());
    double y = CGAL::to_double(p.y());
    double z = CGAL::to_double(p.z());
    std::string d("");
    auto boost_vertex = vertex_map[v.point()] = add_vertex(G, d, x,y,z);

  }

  Triangulation::Finite_edges_iterator iter;
  for(iter =  T.finite_edges_begin();
      iter != T.finite_edges_end();
      iter++) {
    // edges are not represented as edges in CGAL triangulation graphs.
    // Instead, they are stored in faces/cells.

    Triangulation::Triangulation_data_structure::Edge e = *iter;
    Triangulation::Triangulation_data_structure::Cell_handle c = e.first;
    int i = e.second;
    int j = e.third;
    auto boost_edge = add_edge(G, vertex_map[c->vertex(i)->point()], vertex_map[c->vertex(j)->point()]);
  }

  std::cerr << "running prim" << std::endl;
  std::vector<vertex_descriptor> mst_prim(num_vertices(G));

  std::cout << "# vtk DataFile Version 1.0\n";
  std::cout << "3D triangulation data\n";
  std::cout << "ASCII\n";
  std::cout << std::endl;
  std::cout << "DATASET POLYDATA\n";

  std::cout << "POINTS " << num_vertices(G) << " float\n";
  for(int i=0; i<num_vertices(G); i++) {
    std::cout << G[i].x  << " " << G[i].y << " " << G[i].z << std::endl;
  }

  std::cout << "LINES " << (num_vertices(G)-1) << " " << (num_vertices(G)-1)*3 << std::endl;
  if (mst_algorithm == PRIM) {

    // the not particularly helpful doc for iterator_property_map:
    // http://www.boost.org/doc/libs/1_64_0/libs/property_map/doc/iterator_property_map.html
    // iterator_property_map<RandomAccessIterator, OffsetMap, T, R>
    //
    typedef boost::property_map<MyGraphType, boost::vertex_index_t>::type IdMap;
    boost::iterator_property_map<std::vector<vertex_descriptor>::iterator,
                                 IdMap,
                                 vertex_descriptor,
                                 vertex_descriptor&>
      predmap(mst_prim.begin(), get(boost::vertex_index, G));

    boost::prim_minimum_spanning_tree(G, predmap);

    for(int i=0; i<num_vertices(G); i++) {
      if (i == mst_prim[i]) {
        std::cerr << "skipping " << i << std::endl;
        continue;
      }
      std::cout << "2 " << i << " " << mst_prim[i] << std::endl;
    }
  }

  if (mst_algorithm == KRUSKAL) {
    std::cerr << "running kruskal" << std::endl;
    std::list<boost::graph_traits<MyGraphType>::edge_descriptor> mst_kruskal;
    boost::kruskal_minimum_spanning_tree(G, std::back_inserter(mst_kruskal));

    for(auto iter=mst_kruskal.begin();
        iter != mst_kruskal.end();
        iter++) {
      std::cout << "2 " << source(*iter, G) << " " << target(*iter, G) <<std::endl;
    }
  }

}


/**************************************************************************
** This file is a part of our work (Siggraph'16 paper, binary, code and dataset):
**
** Roto++: Accelerating Professional Rotoscoping using Shape Manifolds
** Wenbin Li, Fabio Viola, Jonathan Starck, Gabriel J. Brostow and Neill D.F. Campbell
**
** w.li AT cs.ucl.ac.uk
** http://visual.cs.ucl.ac.uk/pubs/rotopp
**
** Copyright (c) 2016, Wenbin Li
** All rights reserved.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions are met:
**
** -- Redistributions of source code and data must retain the above
**    copyright notice, this list of conditions and the following disclaimer.
** -- Redistributions in binary form must reproduce the above copyright
**    notice, this list of conditions and the following disclaimer in the
**    documentation and/or other materials provided with the distribution.
**
** THIS WORK AND THE RELATED SOFTWARE, SOURCE CODE AND DATA IS PROVIDED BY
** THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
** WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
** MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN
** NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
** INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
** BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
** USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
** NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
** EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************/

#ifndef TRANSFORMATIONS_H
#define TRANSFORMATIONS_H

#include "ceres/ceres.h"
#include "glog/logging.h"
#include "ceres/solver.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/Eigenvalues>

typedef Eigen::MatrixXd Matrix;
typedef Eigen::VectorXd Vector;

#include "baseDefs.hpp"

template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrixFromAngle(const T& alpha)
{
    Eigen::Matrix<T, 2, 2> Q;
    Q << cos(alpha), -sin(alpha), sin(alpha), cos(alpha);
    return Q;
}

template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrixDerivativeFromAngle(const T& alpha)
{
    Eigen::Matrix<T, 2, 2> Q;
    Q << -sin(alpha), -cos(alpha), cos(alpha), -sin(alpha);
    return Q;
}

struct RigidMotionEstimator
{
private:
    Eigen::MatrixXd _Y;
    Eigen::MatrixXd _translations;
    Eigen::VectorXd _rotations;
    Eigen::MatrixXd _referencePoints;
    int _numPoints;

public:
    RigidMotionEstimator(const Eigen::MatrixXd& Y);

    Eigen::MatrixXd CalcNormalisedY() const;

    void SaveToFile(const std::string matlabFilename) const;

    const Eigen::MatrixXd& translations() const
    {
        return _translations;
    }

    const Eigen::VectorXd& rotations() const
    {
        return _rotations;
    }

    const Eigen::MatrixXd& referencePoints() const
    {
        return _referencePoints;
    }

    int numPoints() //const
    {
        return _numPoints;
    }

    Eigen::Matrix2d GetRotationMatrix(int index) const
    {
        nassert ((index >=0) && (index < _Y.rows()));

        return Eigen::Matrix2d(RotationMatrixFromAngle(_rotations[index]));
    }

private:
    void Initialise();

    bool Solve();

    void testRotation() const
    {
        const double alpha = 0.5;
        Eigen::Matrix2d Q(RotationMatrixFromAngle(alpha));
        vdbg(Q);
    }

};

#endif // TRANSFORMATIONS_H


#include <iostream>
#include <stdio.h>
#include <sstream>
#include <deque>
#include <boost/multiprecision/gmp.hpp>

void require(bool cond, const char* msg)
{
    if (!cond) {
        std::cerr << "ERROR: " << msg << "!" << std::endl;
        exit(1);
    }
}

enum Op { LEQ, LT, GT, GEQ, EQ, ADD, SUB, MUL, DIV, AND, OR, NOT };

struct StackItem
{
    typedef boost::multiprecision::mpq_rational val_t;

    enum Kind { OP, VAL, BV } kind;
    Op op;
    val_t val;
    bool bv;

    StackItem(Op o) : kind(OP), op(o) {}
    StackItem(val_t v) : kind(VAL), val(v) {}
    StackItem(bool b) : kind(BV), bv(b) {}

    friend std::ostream& operator<< (std::ostream& stream, const StackItem& si);
};

std::ostream& operator<< (std::ostream& stream, const StackItem& si)
{
    switch (si.kind) {
        case StackItem::OP:
            stream << "op: " << si.op;
            break;
        case StackItem::VAL:
            stream << si.val;
            break;
        case StackItem::BV:
            stream << (si.bv ? "true" : "false");
            break;
    }
    return stream;
}

#define CHECK_BIN(type)\
            require(operands.size() == 2, "Invalid number of arguments for binary operator");\
            require(operands[0].kind == StackItem::type, "Invalid first argument type");\
            require(operands[1].kind == StackItem::type, "Invalid second argument type");

#define CHECK_UN(type)\
            require(operands.size() == 1, "Invalid number of arguments for unary operator");\
            require(operands[0].kind == StackItem::type, "Invalid argument type");

StackItem eval(Op o, const std::deque<StackItem>& operands)
{
    switch  (o) {
        case LEQ: CHECK_BIN(VAL);
            return StackItem(operands[0].val <= operands[1].val);
        case GEQ: CHECK_BIN(VAL);
            return StackItem(operands[0].val >= operands[1].val);
        case LT: CHECK_BIN(VAL);
            return StackItem(operands[0].val < operands[1].val);
        case GT: CHECK_BIN(VAL);
            return StackItem(operands[0].val > operands[1].val);
        case EQ: CHECK_BIN(VAL);
            return StackItem(operands[0].val == operands[1].val);
        case ADD: CHECK_BIN(VAL);
            return StackItem(operands[0].val + operands[1].val);
        case SUB:
            if (operands.size() == 1) {
                CHECK_UN(VAL);
                return StackItem(- operands[0].val);
            } else if (operands.size() == 2) {
                CHECK_BIN(VAL);
                return StackItem(operands[0].val - operands[1].val);
            } else {
                require(false, "Invalid number of arguments for operator '-'");
            }
        case MUL: CHECK_BIN(VAL);
            return StackItem(operands[0].val * operands[1].val);
        case DIV: CHECK_BIN(VAL);
            require(operands[1].val != 0, "Division by zero");
            return StackItem(operands[0].val / operands[1].val);
        case AND: CHECK_BIN(BV);
            return StackItem(operands[0].bv && operands[1].bv);
        case OR: CHECK_BIN(BV);
            return StackItem(operands[0].bv || operands[1].bv);
        case NOT: CHECK_UN(BV);
            return StackItem(!operands[0].bv);
        default:
            require(false, "Invalid operation!");
    }
}

#define EXPECT(x) require(std::getchar() == x, "Expected " #x);

int main(int argc, char *argv[])
{
    std::deque<StackItem> stack;
    char chr = std::getchar();
    while (chr != EOF) {
        switch (chr) {
            case '<':
                chr = std::getchar();
                if (chr == '=') {
                    stack.emplace_back(Op::LEQ);
                } else {
                    stack.emplace_back(Op::LT);
                    continue;
                }
                break;
            case '>':
                chr = std::getchar();
                if (chr == '=') {
                    stack.emplace_back(Op::GEQ);
                } else {
                    stack.emplace_back(Op::GT);
                    continue;
                }
                break;
            case '=':
                stack.emplace_back(Op::EQ);
                break;
            case 'a': EXPECT('n'); EXPECT('d');
                stack.emplace_back(Op::AND);
                break;
            case 'o': EXPECT('r');
                stack.emplace_back(Op::OR);
                break;
            case 'n': EXPECT('o'); EXPECT('t');
                stack.emplace_back(Op::NOT);
                break;
            case '+':
                stack.emplace_back(Op::ADD);
                break;
            case '-':
                stack.emplace_back(Op::SUB);
                break;
            case '*':
                stack.emplace_back(Op::MUL);
                break;
            case '/':
                stack.emplace_back(Op::DIV);
                break;
            case '0': case '1': case '2': case '3': case '4':
            case '5': case '6': case '7': case '8': case '9': {
                    std::stringstream sb;
                    sb << chr;
                    chr = getchar();
                    while (chr >= '0' && chr <= '9') {
                        sb << chr;
                        chr = getchar();
                    }
                    stack.emplace_back(StackItem::val_t(sb.str()));
                    continue;
                }
                break;
            case ')': {
                    std::deque<StackItem> v;
                    while (stack.back().kind != StackItem::OP) {
                        v.push_front(stack.back());
                        stack.pop_back();
                    }
                    auto o = stack.back().op;
                    stack.pop_back();
                    stack.push_back(eval(o, v));
                }
                break;
            case ' ': case '\n': case '\t': case '(':
                break;
            default:
                require(false, "Invalid input");
        }
        chr = getchar();
    }

    for (const auto& si : stack) {
        std::cout << "> " << si << std::endl;
    }

    return 0;
}


/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

#include <jni.h>

#include <iostream>

#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>

#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "edu_wpi_first_wpiutil_math_DrakeJNI.h"
#include "wpi/jni_util.h"

using namespace wpi::java;

extern "C" {

/*
 * Class:     edu_wpi_first_wpiutil_math_DrakeJNI
 * Method:    discreteAlgebraicRiccatiEquation
 * Signature: ([D[D[D[DII[D)V
 */
JNIEXPORT void JNICALL
Java_edu_wpi_first_wpiutil_math_DrakeJNI_discreteAlgebraicRiccatiEquation
  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
  jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
  jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);

  Eigen::Map<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      Amat{nativeA, states, states};
  Eigen::Map<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      Bmat{nativeB, states, inputs};
  Eigen::Map<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      Qmat{nativeQ, states, states};
  Eigen::Map<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
      Rmat{nativeR, inputs, inputs};

  Eigen::MatrixXd result =
      drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);

  env->ReleaseDoubleArrayElements(A, nativeA, 0);
  env->ReleaseDoubleArrayElements(B, nativeB, 0);
  env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
  env->ReleaseDoubleArrayElements(R, nativeR, 0);

  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}

}  // extern "C"


#include "globals.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/connected_components.hpp>
// #include <iostream>
#include <vector>

// #include <boost/graph/graphviz.hpp>

using namespace boost;

typedef adjacency_list_traits<vecS, vecS, directedS> Traits;

// typedef adjacency_list<vecS, vecS, directedS,
//                        property<vertex_name_t, std::string>,
//                        property<edge_capacity_t, int,
//                                 property<edge_residual_capacity_t, int,
//                                          property<edge_reverse_t,
//                                          Traits::edge_descriptor>>>>
//     Graph;

typedef adjacency_list<
    vecS, vecS, directedS,
    property<vertex_name_t, std::string,
             property<vertex_index_t, long,
                      property<vertex_color_t, boost::default_color_type,
                               property<vertex_distance_t, long,
                                        property<vertex_predecessor_t,
                                                 Traits::edge_descriptor>>>>>,
    property<edge_capacity_t, double,
             property<edge_residual_capacity_t, double,
                      property<edge_reverse_t, Traits::edge_descriptor>>>>
    Graph;

// Prepare to do multiple min cuts (cache graphs etc.)
void prepareForMinCuts() {
  totalClasses = 0;
  for (int i = 0; i < classPoints.size(); i++) {
    if (classPoints[i].cls + 1 > totalClasses) {
      totalClasses = classPoints[i].cls + 1;
    }
    if (verboseMode) {
      printf("clspt %d: (%d, %d)\n", classPoints[i].cls, classPoints[i].ri,
             classPoints[i].ci);
    }
  }
  clsMasks = vector<vector<int>>(numClusters, vector<int>(totalClasses, 0));
}

// Perform a min cut against a cls
void minCutCls(int targetCls) {
  Traits::vertex_descriptor s, t;

  Graph g;

  property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, g);
  property_map<Graph, edge_reverse_t>::type rev = get(edge_reverse, g);
  property_map<Graph, edge_residual_capacity_t>::type residual_capacity =
      get(edge_residual_capacity, g);

  double flow;

  std::vector<Traits::vertex_descriptor> verts;

  verts.reserve(numClusters + 2);
  for (int i = 0; i < numClusters + 2; i++) {
    verts.push_back(add_vertex(g));
  }

  for (auto &sp1 : spAdjMap) {
    const int vi1 = sp1.first.first, vi2 = sp1.first.second;
    Traits::edge_descriptor e1, e2;
    bool in1, in2;
    boost::tie(e1, in1) = add_edge(verts[vi1], verts[vi2], g);
    boost::tie(e2, in2) = add_edge(verts[vi2], verts[vi1], g);
    if (!in1 || !in2) {
      // std::cerr << "unable to add edge" << std::endl;
      continue;
    }
    capacity[e1] = sp1.second;
    capacity[e2] = sp1.second;
    rev[e1] = e2;
    rev[e2] = e1;
    // for (auto &sp2 : sp1.second) {
    //   const int vi1 = sp1.first, vi2 = sp2.first;
    //   if (vi1 >= vi2)
    //     continue;
    //   Traits::edge_descriptor e1, e2;
    //   bool in1, in2;
    //   boost::tie(e1, in1) = add_edge(verts[vi1], verts[vi2], g);
    //   boost::tie(e2, in2) = add_edge(verts[vi2], verts[vi1], g);
    //   if (!in1 || !in2) {
    //     // std::cerr << "unable to add edge" << std::endl;
    //     continue;
    //   }
    //   capacity[e1] = sp2.second;
    //   capacity[e2] = sp2.second;
    //   rev[e1] = e2;
    //   rev[e2] = e1;
    // }
  }

  s = verts[numClusters];
  t = verts[numClusters + 1];

  std::unordered_set<int> visitedSP;
  // Add source/target edges
  for (int cpi = 0; cpi < classPoints.size(); cpi++) {
    const ClassPoint cp = classPoints[cpi];
    const int cci = clusters[cp.ri][cp.ci];
    if (visitedSP.find(cci) != visitedSP.end())
      continue;
    visitedSP.insert(cci);
    const auto v = verts[cci];

    Traits::vertex_descriptor *sourceorsink;
    if (cp.cls == targetCls) {
      sourceorsink = &s;
    } else {
      sourceorsink = &t;
    }

    Traits::edge_descriptor e1, e2;
    bool in1, in2;
    boost::tie(e1, in1) = add_edge(v, *sourceorsink, g);
    boost::tie(e2, in2) = add_edge(*sourceorsink, v, g);
    if (!in1 || !in2) {
      continue;
    }

    rev[e1] = e2;
    rev[e2] = e1;

    capacity[e1] = 1000;
    capacity[e2] = 1000;
  }

  // write_graphviz(std::cout, g, default_writer(),
  // make_label_writer(capacity));

  flow = boykov_kolmogorov_max_flow(g, s, t);

  if (verboseMode) {
    printf("flow: %f\n", flow);
  }

  // remove edges with zero residual capacity

  // auto epair = edges(g);
  // for (auto ei = epair.first; ei != epair.second; ei++)
  // {
  //     std::cout << "edge: " << source(*ei, g) << "-" << target(*ei, g) << "
  //     " << residual_capacity[*ei] << std::endl;
  // }

  // write_graphviz(std::cout, g, default_writer());

  auto eiter = edges(g);
  std::vector<Traits::edge_descriptor> edgesToRemove;
  for (auto ei = eiter.first; ei != eiter.second; ei++) {
    if (residual_capacity[*ei] == 0) {
      edgesToRemove.push_back(*ei);
      edgesToRemove.push_back(rev[*ei]);
    }
  }

  for (auto edge : edgesToRemove) {
    remove_edge(edge, g);
  }

  // write_graphviz(std::cout, g, default_writer(),
  // make_label_writer(residual_capacity));

  // std::cout << "edges remaining..." << std::endl;

  // auto epair = edges(g);
  // for (auto ei = epair.first; ei != epair.second; ei++)
  // {
  //     std::cout << "edge: " << source(*ei, g) << "-" << target(*ei, g) <<
  //     std::endl;
  //     // remove_edge(*ei, g);
  // }

  // write_graphviz(std::cout, g, default_writer());

  std::vector<int> components(num_vertices(g));
  int totalConnectedComponents = connected_components(g, &components[0]);

  // for (int i = 0; i < components.size(); i++)
  // {
  //     std::cout << "Vertex " << i << " is in component " << components[i] <<
  //     std::endl;
  // }

  int clsComponentIndex = components[numClusters];
  int othComponentIndex = components[numClusters + 1];

  for (int cci = 0; cci < numClusters; cci++) {
    clsMasks[cci][targetCls] = std::numeric_limits<int>::max();
  }

  for (int cci = 0; cci < numClusters; cci++) {
    if (components[cci] == clsComponentIndex) {
      // If connected to source, take distance
      clsMasks[cci][targetCls] = get(vertex_distance, g)[verts[cci]];
    } else {
      // If connected to sink, take inverse distance
      clsMasks[cci][targetCls] =
          maxClusters - get(vertex_distance, g)[verts[cci]];
    }
  }

  // std::cout << clsMasks << std::endl;
}

void resolveMasks() {
  vector<int> resolvedMasks(numClusters);
  for (int cci = 0; cci < numClusters; cci++) {
    int smallestCls = -1;
    int smallestDist = std::numeric_limits<int>::max();
    for (int cls = 0; cls < totalClasses; cls++) {
      if (clsMasks[cci][cls] < smallestDist) {
        smallestDist = clsMasks[cci][cls];
        smallestCls = cls;
      }
    }
    resolvedMasks[cci] = smallestCls;
  }
  if (verboseMode) {
    for (int cci = 0; cci < numClusters; cci++) {
      printf("cci %d: %d\n", cci, resolvedMasks[cci]);
    }
  }
  coloredMask = std::vector<uint32_t>(width * height);
  for (int ri = 0; ri < height; ri++) {
    for (int ci = 0; ci < width; ci++) {
      const int imgi = ri * width + ci;
      const int cci = clusters[ri][ci];
      const int cls = resolvedMasks[cci];

      if (cls == -1) {
        coloredMask[imgi] = 0xffffff88;
      } else {
        coloredMask[imgi] = classToColor[cls];
      }
    }
  }
}


#pragma once

#include <skynet/utility/tag.hpp>
#include <skynet/ublas.hpp>

#include <random>
#include <boost/numeric/ublas/symmetric.hpp>

namespace skynet{namespace nn{

	template <typename T>
	class boltzmann_machine;

	template <>
	class boltzmann_machine<full_connected> {
	public:
		boltzmann_machine(size_t visible_num, size_t hidden_num)
			: _visible_num(visible_num), _hidden_num(visible_num), _T(10), _c(0.99), _cycles_num(10),
			_eta(1.0), _k(10){

		}

		template <typename T>
		void learn(const ublas::matrix_expression<T> &patterns){
			init(patterns);

			std::uniform_real_distribution<double> rand_energe(0, 1.0);
			std::mt19937						   mt1(std::time(nullptr));
			std::uniform_int_distribution<size_t> rand_row(0, _clamped_signals.size1()-1);
			std::mt19937						   mt2(std::time(nullptr));

			for (size_t k = 0; k < k(); ++k){
				while (_T > 1.0){
					auto Xf = ublas::prod(_W, _free_signals);
					for (size_t col = 0; col < pf.size2(); ++col){
						for (size_t i = 0; i < _cycles_num; ++i){
							auto row = rand_row(mt2);
							auto probability = 1.0 / (1.0 + exp(-Xf(row, col)/_T));
							if (rand_energe(mt1) < probability)
								_free_signals(row, col) = 1;
							else
								_free_signals(row, col) = -1;
						}
					}

					auto Xc = ublas::prod(_W, _clamped_signals);
					for (size_t col = 0; col < pf.size2(); ++col){
						for (size_t i = 0; i < _cycles_num; ++i){
							auto row = rand_row(mt2);
							//if the state is clamped, do none
							if (row < _visible_num)	continue;

							auto probability = 1.0 / (1.0 + exp(-Xc(row, col)/_T));
							if (rand_energe(mt1) < probability)
								_clamped_signals(row, col) = 1;
							else
								_clamped_signals(row, col) = -1;
						}
					}

					_T *= _c;
				}

				auto rep_num = _clamped_signals.size2();
				//update the weights
				ublas::matrix<double> Expc =
					(1.0/rep_num) * ublas::prod(ublas::trans(_clamped_signals), _clamped_signals);
				ublas::matrix<double> Expf =
					(1.0/rep_num) * ublas::prod(ublas::trans(_free_signals), _free_signals);
				auto deltaW = Expc - Expf;
				_W = _W + (_eta/_T) * deltaW;
			}
		}

		size_t cycles_num() const						{ return _cycles_num; }
		void cycles_num(size_t v)						{ _cycles_num = v; }

		double T() const								{ return _T; }
		void T(double v)								{ _T = v; }

		double c() const								{ return _c; }
		void c(double v)								{ _c = v; }

		double eta() const								{ return _eta; }
		void eta(double v)								{ _eta = v; }

		double k() const								{ return _k; }
		void k(size_t v)								{ _k = v; }

	private:
		template <typename T>
		void init(const ublas::matrix_expression<T> &patterns){
			ASSERT(patterns.size1() == _visible_num; "The size is not matched!");
			//initialize weights
			_W.resize(_visible_num+_hidden_num+1);
			for (size_t i = 0; i < _W.size1(); ++i){
				for (size_t j = 0; j < _W.size2(); ++j){
					_W(i, j) = 0;
				}
			}

			//random initialize the state
			_free_signals.resize(_visible_num + _hidden_num + 1, patterns().size2());
			_clamped_signals.resize(_visible_num + _hidden_num + 1, patterns().size2());
			std::uniform_int_distribution<int> rand(0, 1);
			std::mt19937 mt(std::time(nullptr));
			for (int col = 0; col < _free_signals.size2(); ++col){
				for (int row = 0; row < _free_signals.size1(); ++row){
					_free_signals(row, col) = rand(mt) == 0 ? -1 : 1;
					_clamped_signals(row, col) = rand(mt) == 0 ? -1 : 1;
				}
			}
			for (int col = 0; col < _clamped_signals.size2(); ++col){
				_clamped_signals(_clamped_signals.size1()-1, col) = -1;
			}

			//set the  visible state
			for (int col = 0; col _clamped_signals.size2(); ++col){
				for (int row = 0; row < _visible_num; ++row){
					_clamped_signals(row, col) = patterns()(row, col);
				}
			}
		}

	private:
		ublas::matrix<double> _free_signals;
		ublas::matrix<double> _clamped_signals;

		ublas::symmetric_matrix<double> _W;

		size_t _visible_num;
		size_t _hidden_num;

		double _T;
		double _c;
		double _eta;
		size_t _cycles_num;
		size_t _k;
	};

}}


#define BOOST_TEST_MODULE robust_ea_solver_unit_tests

#include <boost/test/included/unit_test.hpp>
#include <armadillo>
#include "../src/numerical/evolutionary_algorithm/robust_ea_solver.h"
#include "test_utils.cpp"
#include "../src/logging/easylogging++.h"

INITIALIZE_EASYLOGGINGPP

using namespace arma;

RobustEASolver solver = RobustEASolver(SIGNALS, 500, 1000, 0.0001, 100, 100);

BOOST_AUTO_TEST_CASE(test_common_cases)
{
    auto tester = SolverTester<RobustEASolver>(solver);
    tester.test_common(0.1);
}

BOOST_AUTO_TEST_CASE(fit_random_signals_with_outliers)
{
    mat weights = {-10, 1, 500};
    mat signals = arma::randu(3, 100);
    RobustEASolver solver = RobustEASolver(signals, 500, 1000, 0.0001, 100, 100);
    mat signal = sum_signal(weights, signals);
    signal[10] -= 1000;
    signal[20] += 1000;
    mat result = solver.solve(signal);
    BOOST_CHECK(is_equal(weights, result, 0.1));
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#ifdef _MSC_VER
# pragma warning (disable : 4305) // 'initializing' : truncation from 'long double' to 'const eval_type'
# pragma warning (disable : 4244) // 'conversion' : truncation from 'long double' to 'const eval_type'
#endif

//[policy_ref_snip4

#include <boost/math/distributions/normal.hpp>
using boost::math::normal_distribution;

using namespace boost::math::policies;

// Define a policy:
typedef policy<
      promote_float<false>
      > my_policy;

// Define the new normal distribution using my_policy:
typedef normal_distribution<float, my_policy> my_norm;

// Get a quantile:
float q = quantile(my_norm(), 0.05f);

//] [policy_ref_snip4]

#include <iostream>
using std::cout; using std::endl;

int main()
{
   cout << " quantile(my_norm(), 0.05f) = " << q << endl; //   -1.64485
}


///////////////////////////////////////////////////////////////////////////////
// statistics::model::algorithm::log_posteriors2.hpp                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_MODEL_ALGORITHM_LOG_POSTERIORS2_HPP_ER_2009
#define BOOST_MODEL_ALGORITHM_LOG_POSTERIORS2_HPP_ER_2009
#include <algorithm>
#include <boost/iterator/iterator_traits.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/statistics/model/wrap/aggregate/prior_model_dataset.hpp>
#include <boost/statistics/model/functional/log_prior_evaluator.hpp>
#include <boost/statistics/model/functional/log_likelihood_evaluator.hpp>

namespace boost{
namespace statistics{
namespace model{

// Evaluates the log-postetior for each parameter in [b_p,e_p)
// Also see log_posteriors
template<
    typename T,typename D,typename M,typename Rx,typename Ry,typename ItP,
    typename ItLw
>
ItLw
log_posteriors2(
    boost::statistics::model::prior_model_dataset_<D,M,Rx,Ry> pmd,
    ItP b_p,
    ItP e_p,
    ItLw o_lw
){
    typedef log_prior_evaluator<T,D>            eval0_;
    typedef log_likelihood_evaluator<T,M,Rx,Ry> eval_;
    return std::transform(
        b_p,
        e_p,
        o_lw,
        boost::lambda::bind(
            eval0_(pmd),
            boost::lambda::_1
        ) + boost::lambda::bind(
            eval_(pmd),
            boost::lambda::_1
        )
    );
}

// log-pdf_Q --> log_pdf P + log_pdf L - log_pdf_Q
template<
    typename T,
    typename D,
    typename M,
    typename Rx,
    typename Ry,
    typename ItP,
    typename ItLw,
    typename ItLw2
>
ItLw2
log_posteriors2(
    prior_model_dataset_<D,M,Rx,Ry> pmd,
    ItP b_p,
    ItP e_p,
    ItLw b_lw,
    ItLw2 o_lw   // lw <- (log_posterior - lw)
){
    typedef log_prior_evaluator<T,D> eval0_;
    typedef log_likelihood_evaluator<T,M,Rx,Ry> eval_;
    eval0_ e0(pmd);
    eval_ e(pmd);

    return std::transform(
        b_p,    //1
        e_p,
        b_lw,   //2
        o_lw,
        boost::lambda::bind(
            e,
            boost::lambda::_1
        ) +
        (
            boost::lambda::bind(
                e0,
                boost::lambda::_1
            )
            - boost::lambda::_2
        )
    );
}

}// model
}// statistics
}// boost

#endif


//stdlib
#include <iostream>

//libraries
#include <Eigen/Eigen>
#include <boost/python.hpp>
#include <boost/shared_ptr.hpp>

//local
#include <lsf_config.h>
#include "math/typedefs.hpp"
#include "python_export/eigen_numpy.hpp"
#include "python_export/math.hpp"
#include "python_export/slavcheva_optimizer.hpp"
#include "python_export/hierarchical_optimizer.hpp"
#include "python_export/tsdf.hpp"
#include "python_export/conversion_tests.hpp"
#include "python_export/telemetry.hpp"
#include "python_export/sdf_2_sdf_optimizer.hpp"

namespace bp = boost::python;
namespace pe = python_export;

BOOST_PYTHON_MODULE ( MODULE_NAME )
{
	setup_Eigen_matrix_converters();
	setup_Eigen_tensor_converters();
	setup_Eigen_list_converters();

	pe::export_conversion_tests();

	pe::export_math_types();
	pe::export_math_functions();

	pe::tsdf::export_algorithms();

	pe::export_telemetry_utilities<math::Vector2i, eig::MatrixXf, math::MatrixXv2f>("2d");
	pe::export_telemetry_utilities<math::Vector3i, math::Tensor3f, math::Tensor3v3f>("3d");

	pe::slavcheva::export_auxiliary_functions();
	pe::slavcheva::export_setting_singletons();
	pe::slavcheva::export_algorithms();

	pe::hierarchical_optimizer::export_algorithms<eig::MatrixXf, math::MatrixXv2f>("2d");
	pe::hierarchical_optimizer::export_algorithms<math::Tensor3f, math::Tensor3v3f>("3d");

	pe::sdf_2_sdf_optimizer::export_algorithms();
}


#ifndef AHTLP_HPP_
#define AHTLP_HPP_

#include <NTL/ZZ.h>
#include <assert.h>
#include <openssl/sha.h>
#include <vector>
#include <sstream>

#include "HTLP.hpp"
#include "Puzzle.hpp"

#ifndef RSA_
#define RSA_
typedef struct RSA
{
    NTL::ZZ p;
    NTL::ZZ q;
} RSA;
#endif

class AHTLP : public HTLP
{

public:
    AHTLP(const long modulus_len, const long T, const long kappa);
    AHTLP(const NTL::ZZ &n, const NTL::ZZ &g, const NTL::ZZ &h, const long T, const long kappa);
    AHTLP(const long modulus_len, const long T, const long kappa, bool cheeting_mode);

    APuzzle GeneratePuzzle(const NTL::ZZ &s);
    APuzzle GeneratePuzzle(const NTL::ZZ &s, const NTL::ZZ &r);
    NTL::ZZ SolvePuzzle(const APuzzle &Z);
    NTL::ZZ QuickSolvePuzzle(const APuzzle &Z);

    std::tuple<NTL::ZZ, NTL::ZZ, NTL::ZZ, NTL::ZZ> GenerateAValidProof(const APuzzle &Z, const NTL::ZZ &s, const NTL::ZZ &r);
    bool VerifyAValidProof(const APuzzle &Z, const std::tuple<NTL::ZZ, NTL::ZZ, NTL::ZZ, NTL::ZZ> &proof);

    std::tuple<NTL::ZZ, NTL::ZZ, NTL::ZZ> SolvePuzzleWithProof(const long k, const long gamma, const APuzzle &Z);
    std::tuple<NTL::ZZ, NTL::ZZ, NTL::ZZ> QuickSolvePuzzleWithProof(const APuzzle &Z);

    int VerifyProofOfSol(const APuzzle Z, const std::tuple<NTL::ZZ, NTL::ZZ, NTL::ZZ> &proof);
};

#endif


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/random.hpp>
#include <boost/make_shared.hpp>

#include "Tudat/Mathematics/Statistics/boostProbabilityDistributions.h"

namespace tudat
{

namespace statistics
{

//! Function to create a random variable class of BoostContinuousProbabilityDistribution type
std::shared_ptr< InvertibleContinuousProbabilityDistribution< double > > createBoostRandomVariable(
        const ContinuousBoostStatisticalDistributions boostDistribution, const std::vector< double >& parameters )
{
    using namespace boost::math;

    std::shared_ptr< InvertibleContinuousProbabilityDistribution< double > > continuousRandomVariable;

    // Check which distribution type is requested
    switch( boostDistribution )
    {
    case uniform_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Uniform distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: lower bound, 1: upper bound
            uniform_distribution< > uniformDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< uniform_distribution< > > >(
                        uniformDistribution );
        }
        break;
    }
    case normal_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Normal distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: mean, 1: standard deviation
            normal_distribution< > normalDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< normal_distribution< > > >(
                        normalDistribution );
        }
        break;
    }
    case exponential_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 1 )
        {
            throw std::runtime_error( "Exponential distribution requires one parameter." );
        }
        else
        {
            // Create uniform distribution. parameters 0: lambda parameter
            exponential_distribution< > exponentialDistribution( parameters.at( 0 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< exponential_distribution< > > >(
                        exponentialDistribution );
        }
        break;
    }
    case gamma_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Gamma distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: shape parameter, parameter 1: scale parameter
            gamma_distribution< > gammaDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< gamma_distribution< > > >(
                        gammaDistribution );
        }
        break;
    }
    case lognormal_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Log-normal distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: location parameter, parameter 1: scale parameter
            lognormal_distribution< > logNormalDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< lognormal_distribution< > > >(
                        logNormalDistribution );
        }
        break;
    }
    case beta_boost_distribution:
    {
        // Check number of provided parameters
        if ( parameters.size( ) != 2 )
        {
            throw std::runtime_error( "Beta distribution requires two parameters." );
        }
        else
        {
            // Create uniform distribution. parameters 0: alpha parameter, parameter 1: beta parameter
            beta_distribution< > betaDistribution( parameters.at( 0 ), parameters.at( 1 ) );
            continuousRandomVariable =
                    std::make_shared< BoostContinuousProbabilityDistribution< beta_distribution< > > >(
                        betaDistribution );
        }
        break;
    }
    default:
        throw std::runtime_error( "Boost distribution not recognized." );
    }
    return continuousRandomVariable;
}

}

}


#include <CGAL/trace.h>
#include <CGAL/Timer.h>
#include <iostream>
#include <string>
#include <fstream>

#include <Eigen/Eigen>

// #define USE_SCALAR_IMPLEMENTATION
#define USE_SSE_IMPLEMENTATION
#define COMPUTE_V_AS_MATRIX
#define COMPUTE_U_AS_MATRIX

#include <CGAL/internal/Surface_mesh_deformation/auxiliary/Singular_Value_Decomposition_Preamble.hpp>

int main() {

  std::ifstream file;
  file.open("SVD_benchmark");
  if (!file)
  {
    CGAL_TRACE_STREAM << "Error loading file!\n";
    return 0;
  }

  int ite = 200000;
  Eigen::JacobiSVD<Eigen::Matrix3d> svd;
  Eigen::Matrix3d u, v, m, r;
  Eigen::Vector3d w;

  int matrix_idx = rand()%200;
  for (int i = 0; i < matrix_idx; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      for (int k = 0; k < 3; k++)
      {
        file >> m(j, k);
      }
    }
  }

  CGAL::Timer task_timer;

  std::cout << "Start SVD decomposition...";
  task_timer.start();
  for (int i = 0; i < ite; i++)
  {

    #include <CGAL/internal/Surface_mesh_deformation/auxiliary/Singular_Value_Decomposition_Kernel_Declarations.hpp>

    ENABLE_SCALAR_IMPLEMENTATION(Sa11.f=m(0,0);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa21.f=m(1,0);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa31.f=m(2,0);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa12.f=m(0,1);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa22.f=m(1,1);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa32.f=m(2,1);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa13.f=m(0,2);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa23.f=m(1,2);)
    ENABLE_SCALAR_IMPLEMENTATION(Sa33.f=m(2,2);)

    ENABLE_SSE_IMPLEMENTATION(Va11=_mm_set1_ps(m(0,0));)
    ENABLE_SSE_IMPLEMENTATION(Va21=_mm_set1_ps(m(1,0));)
    ENABLE_SSE_IMPLEMENTATION(Va31=_mm_set1_ps(m(2,0));)
    ENABLE_SSE_IMPLEMENTATION(Va12=_mm_set1_ps(m(0,1));)
    ENABLE_SSE_IMPLEMENTATION(Va22=_mm_set1_ps(m(1,1));)
    ENABLE_SSE_IMPLEMENTATION(Va32=_mm_set1_ps(m(2,1));)
    ENABLE_SSE_IMPLEMENTATION(Va13=_mm_set1_ps(m(0,2));)
    ENABLE_SSE_IMPLEMENTATION(Va23=_mm_set1_ps(m(1,2));)
    ENABLE_SSE_IMPLEMENTATION(Va33=_mm_set1_ps(m(2,2));)

    #include <CGAL/internal/Surface_mesh_deformation/auxiliary/Singular_Value_Decomposition_Main_Kernel_Body.hpp>

    std::pair<Eigen::Matrix3d, Eigen::Matrix3d> solver;

#ifdef USE_SCALAR_IMPLEMENTATION
    solver.first(0,0) = Su11.f;
    solver.first(1,0) = Su21.f;
    solver.first(2,0) = Su31.f;
    solver.first(0,1) = Su12.f;
    solver.first(1,1) = Su22.f;
    solver.first(2,1) = Su32.f;
    solver.first(0,2) = Su13.f;
    solver.first(1,2) = Su23.f;
    solver.first(2,2) = Su33.f;

    solver.second(0,0) = Sv11.f;
    solver.second(1,0) = Sv21.f;
    solver.second(2,0) = Sv31.f;
    solver.second(0,1) = Sv12.f;
    solver.second(1,1) = Sv22.f;
    solver.second(2,1) = Sv32.f;
    solver.second(0,2) = Sv13.f;
    solver.second(1,2) = Sv23.f;
    solver.second(2,2) = Sv33.f;
#endif

#ifdef USE_SSE_IMPLEMENTATION
    float buf[4];
    _mm_storeu_ps(buf,Vu11);solver.first(0,0)=buf[0];
    _mm_storeu_ps(buf,Vu21);solver.first(1,0)=buf[0];
    _mm_storeu_ps(buf,Vu31);solver.first(2,0)=buf[0];
    _mm_storeu_ps(buf,Vu12);solver.first(0,1)=buf[0];
    _mm_storeu_ps(buf,Vu22);solver.first(1,1)=buf[0];
    _mm_storeu_ps(buf,Vu32);solver.first(2,1)=buf[0];
    _mm_storeu_ps(buf,Vu13);solver.first(0,2)=buf[0];
    _mm_storeu_ps(buf,Vu23);solver.first(1,2)=buf[0];
    _mm_storeu_ps(buf,Vu33);solver.first(2,2)=buf[0];

    _mm_storeu_ps(buf,Vv11);solver.second(0,0)=buf[0];
    _mm_storeu_ps(buf,Vv21);solver.second(1,0)=buf[0];
    _mm_storeu_ps(buf,Vv31);solver.second(2,0)=buf[0];
    _mm_storeu_ps(buf,Vv12);solver.second(0,1)=buf[0];
    _mm_storeu_ps(buf,Vv22);solver.second(1,1)=buf[0];
    _mm_storeu_ps(buf,Vv32);solver.second(2,1)=buf[0];
    _mm_storeu_ps(buf,Vv13);solver.second(0,2)=buf[0];
    _mm_storeu_ps(buf,Vv23);solver.second(1,2)=buf[0];
    _mm_storeu_ps(buf,Vv33);solver.second(2,2)=buf[0];
#endif

    r = solver.first * solver.second.transpose();
  }
  task_timer.stop();
  file.close();

  std::cout << "done: " << task_timer.time() << "s\n";
  return 0;
}


#include "multiindex.hpp"
#include "potgen.hpp"
#include "fft.hpp"
#include "dynamic_grid.hpp"

#include <boost/test/unit_test.hpp>

complex_grid discretizeFunctionForFFT(std::vector<std::size_t> gridsize, std::vector<double>, correlation_fn F);

static const std::vector<double> vec_1{1.0};
static const std::vector<double> vec_2{1.0, 1.0};
static const std::vector<double> vec_3{1.0, 1.0, 1.0};

BOOST_AUTO_TEST_SUITE(potgen_internals_test)

// test discretize function
BOOST_AUTO_TEST_CASE( discretizeFunctionForFFT_1d )
{
	std::function<double(const gen_vect&)> f = [](const gen_vect& x) { return std::exp(-x[0]*x[0]*10); };

	auto dgrid = discretizeFunctionForFFT(std::vector<size_t>{64}, vec_1, f);

	gen_vect s(1);
	for(int i=0; i < 64; ++i)
	{
		// compare with shifted: this is required to make FT real
		s[0] = ((i+32)%64 - 32) / 64.0;
		BOOST_CHECK_EQUAL(dgrid.getContainer().at<complex_t>(i), f(s));
	}

	// check symmetry
	/// \todo this is not symmetric. why is this correct?
	/*for(int i = 0; i < 32; ++i)
	{
		BOOST_CHECK_EQUAL(dgrid.getContainer().at<double>(i), dgrid.getContainer().at<double>(63-i));
	}*/
}

BOOST_AUTO_TEST_CASE( discretize_1d_identity )
{
	constexpr std::size_t size = 64;
	std::function<double(const gen_vect&)> f = [=](const gen_vect& x) { return x[0] * size; };

	auto dgrid = discretizeFunctionForFFT(std::vector<size_t>{size}, vec_1, f);

	gen_vect s(1);
	/// \todo is there any need for the complicated index calculations here?
	for(unsigned i=0; i < size; ++i)
	{
		BOOST_REQUIRE_EQUAL((int)((dgrid.getContainer().at<complex_t>(i)).real()), (i+size/2)%size - size/2);
	}
}

BOOST_AUTO_TEST_CASE( discretizeFunctionForFFT_2d )
{
	// first check target range
	std::function<double(const gen_vect&)> f = [](const gen_vect&) { return 1; };
	auto dgrid = discretizeFunctionForFFT(std::vector<size_t>{8,8}, vec_2, f);

	for(int i=0; i < 64; ++i)
	{
		BOOST_REQUIRE_EQUAL(dgrid.getContainer().at<complex_t>(i), 1.0);
	}

	std::function<double(const gen_vect&)> f2 = [](const gen_vect& g) { return std::exp((-g[0]*g[0]-g[1]*g[1])*100); };
	auto dgrid2 = discretizeFunctionForFFT(std::vector<size_t>{32, 32}, vec_2, f2);

	// check that fft phases make it completely real
	std::vector<complex_t> ct;
	ct.assign(dgrid2.begin(), dgrid2.end());

	fft( ct, std::vector<int>({32, 32}) );

	for(unsigned i = 0; i < ct.size(); ++i)
	{
		BOOST_REQUIRE_SMALL(std::imag(ct[i]), 1e-10);
		BOOST_REQUIRE_GE(std::real(ct[i]), -1e-10);
	}

	/// \todo check general positions for general function
}

BOOST_AUTO_TEST_CASE( discretizeFunctionForFFT_error )
{
	std::function<double(const gen_vect&)> f = [](const gen_vect&) { return 1; };
	// odd_sized grids are unsupported
	BOOST_CHECK_THROW( (discretizeFunctionForFFT(std::vector<size_t>{3}, vec_1, f)), std::invalid_argument);
	// incompatible dimensions between grid size and support
	BOOST_CHECK_THROW( (discretizeFunctionForFFT(std::vector<size_t>{2, 2}, vec_1, f)), std::invalid_argument);
}

BOOST_AUTO_TEST_SUITE_END()


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <boost/numeric/odeint.hpp>
#include <smooth/bundle.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/feedback/asif.hpp>
#include <smooth/feedback/mpc.hpp>
#include <smooth/se2.hpp>
#include <smooth/se3.hpp>

#include <chrono>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

#ifdef ENABLE_ROS
#include <gazebo_msgs/srv/set_entity_state.hpp>
#include <geometry_msgs/msg/accel.hpp>
#include <rclcpp/rclcpp.hpp>
#include <smooth/compat/ros.hpp>
#endif

using namespace std::chrono_literals;
using namespace boost::numeric::odeint;

using Time = std::chrono::duration<double>;

template<typename S>
using X = smooth::Bundle<smooth::SE2<S>, Eigen::Matrix<S, 3, 1>>;
template<typename S>
using U = Eigen::Matrix<S, 2, 1>;

using Xd = X<double>;
using Ud = U<double>;

using Tangentd = smooth::Tangent<Xd>;

int main()
{
  // dynamics
  auto f = []<typename S>(const X<S> & x, const U<S> & u) -> smooth::Tangent<X<S>> {
    return {
      x.template part<1>().x(),
      x.template part<1>().y(),
      x.template part<1>().z(),
      -S(0.2) * x.template part<1>().x() + u.x(),
      S(0),
      -S(0.4) * x.template part<1>().z() + u.y(),
    };
  };

  auto cr = []<typename S>(const X<S> &, const U<S> & u) -> Eigen::Vector<S, 2> { return u; };
  Eigen::Vector2d crl{-0.5, -0.5};
  Eigen::Vector2d cru{0.5, 0.5};

  // simulation time step
  const auto dt = 25ms;

  ////////////////////
  //// SET UP MPC ////
  ////////////////////

  smooth::feedback::MPC<Time, Xd, Ud, decltype(f), decltype(cr)> mpc{
    f,
    cr,
    crl,
    cru,
    {.K = 30, .tf = 5},
  };

  // define desired trajectory
  auto xdes = []<typename S>(S t) -> X<S> {
    const Eigen::Vector3d vdes{1, 0, 0.4};
    return X<S>{
      smooth::SE2<S>(smooth::SO2<S>(M_PI_2), Eigen::Vector2<S>(2.5, 0)) + (t * vdes),
      vdes,
    };
  };

  mpc.set_weights({
    .Q   = Eigen::Matrix<double, 6, 6>::Identity(),
    .Qtf = 0.1 * Eigen::Matrix<double, 6, 6>::Identity(),
    .R   = Eigen::Matrix2d::Identity(),
  });

  // set desired trajectory in MPC
  mpc.set_xdes_rel(xdes);
  mpc.set_udes_rel([]<typename S>(S) -> U<S> { return U<S>::Zero(); });

  /////////////////////
  //// SET UP ASIF ////
  /////////////////////

  // safe set
  auto h = []<typename S>(S, const X<S> & x) -> Eigen::Matrix<S, 1, 1> {
    const Eigen::Vector2<S> dir = x.template part<0>().r2() - Eigen::Vector2<S>{0, -2.3};
    const Eigen::Vector2d e_dir = dir.template cast<double>().normalized();
    return Eigen::Matrix<S, 1, 1>(dir.dot(e_dir) - 0.7);
  };

  // backup controller
  auto bu = []<typename S>(S, const X<S> & x) -> U<S> {
    return {0.2 * x.template part<1>().x(), -0.5};
  };

  const smooth::feedback::ManifoldBounds<Ud> ulim{
    .A = Eigen::Matrix2d{{1, 0}, {0, 1}},
    .c = Ud::Zero(),
    .l = Eigen::Vector2d(-0.2, -0.5),
    .u = Eigen::Vector2d(0.5, 0.5),
  };

  // parameters
  smooth::feedback::ASIFilterParams<Ud> asif_prm{
    .T        = 2.5,
    .nh       = 1,
    .u_weight = Eigen::Vector2d{20, 1},
    .ulim     = ulim,
    .asif =
      {
        .K          = 200,
        .alpha      = 5,
        .dt         = 0.01,
        .relax_cost = 100,
      },
    .qp =
      {
        .polish = false,
      },
  };

  smooth::feedback::ASIFilter<Xd, Ud, decltype(f)> asif(f, asif_prm);

  /////////////////////////
  //// CREATE ROS NODE ////
  /////////////////////////

#ifdef ENABLE_ROS
  rclcpp::init(0, nullptr);
  auto node       = std::make_shared<rclcpp::Node>("se2_example");
  auto ses_client = node->create_client<gazebo_msgs::srv::SetEntityState>("/set_entity_state");
  auto u_mpc_pub =
    node->create_publisher<geometry_msgs::msg::Accel>("u_mpc", rclcpp::SystemDefaultsQoS{});
  auto u_asif_pub =
    node->create_publisher<geometry_msgs::msg::Accel>("u_asif", rclcpp::SystemDefaultsQoS{});

  rclcpp::Rate rate(25ms);
#endif

  /////////////////////////////////////
  //// SIMULATE CLOSED-LOOP SYSTEM ////
  /////////////////////////////////////

  // system variables
  Xd x = Xd::Identity();
  Ud u;

  // prepare for integrating the closed-loop system
  runge_kutta4<Xd, double, Tangentd, double, vector_space_algebra> stepper{};
  const auto ode = [&f, &u](const Xd & x, Tangentd & d, double) { d = f(x, u); };
  std::vector<double> tvec, xvec, yvec, u1vec, u2vec, u1mpcvec, u2mpcvec;

  // integrate closed-loop system
  for (std::chrono::milliseconds t = 0s; t < 30s; t += dt) {
    // compute MPC input
    const auto [u_mpc, mpc_code] = mpc(t, x);
    if (mpc_code != smooth::feedback::QPSolutionStatus::Optimal) {
      std::cerr << "MPC failed with mpc_code " << static_cast<int>(mpc_code) << std::endl;
    }

    // filter input with ASIF
    const auto [u_asif, asif_code] = asif(x, u_mpc, h, bu);
    if (asif_code != smooth::feedback::QPSolutionStatus::Optimal) {
      std::cerr << "ASIF solver failed with asif_code " << static_cast<int>(asif_code) << std::endl;
    }

    // select input
    u = u_asif;

    // store data
    tvec.push_back(duration_cast<Time>(t).count());
    xvec.push_back(x.template part<0>().r2().x());
    yvec.push_back(x.template part<0>().r2().y());

    u1mpcvec.push_back(u_mpc(0));
    u2mpcvec.push_back(u_mpc(1));
    u1vec.push_back(u_asif(0));
    u2vec.push_back(u_asif(1));

#ifdef ENABLE_ROS
    auto req        = std::make_shared<gazebo_msgs::srv::SetEntityState::Request>();
    req->state.name = "bus::link";
    smooth::Map<geometry_msgs::msg::Pose>(req->state.pose) = x.template part<0>().lift_se3();
    req->state.pose.position.x                             = 8 * req->state.pose.position.x;
    req->state.pose.position.y                             = 8 * req->state.pose.position.y;
    ses_client->async_send_request(req);

    geometry_msgs::msg::Accel u_mpc_msg;
    u_mpc_msg.linear.x  = u_mpc.x();
    u_mpc_msg.angular.z = u_mpc.y();
    u_mpc_pub->publish(u_mpc_msg);

    geometry_msgs::msg::Accel u_asif_msg;
    u_asif_msg.linear.x  = u_asif.x();
    u_asif_msg.angular.z = u_asif.y();
    u_asif_pub->publish(u_asif_msg);

    rate.sleep();
#endif
    // step dynamics
    stepper.do_step(
      ode, x, 0, std::chrono::duration_cast<std::chrono::duration<double>>(dt).count());
  }

#ifdef ENABLE_PLOTTING
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::title("Path");

  matplot::plot(xvec, yvec)->line_width(2);
  matplot::plot(
    matplot::transform(tvec, [&](auto t) { return xdes(t).template part<0>().r2().x(); }),
    matplot::transform(tvec, [&](auto t) { return xdes(t).template part<0>().r2().y(); }),
    "k--")
    ->line_width(2);
  matplot::legend({"actual", "desired"});

  matplot::figure();
  matplot::hold(matplot::on);
  matplot::title("Inputs");
  matplot::plot(tvec, u1vec, "r")->line_width(2);
  matplot::plot(tvec, u2vec, "b")->line_width(2);
  matplot::plot(tvec, u1mpcvec, "--r")->line_width(2);
  matplot::plot(tvec, u2mpcvec, "--b")->line_width(2);
  matplot::legend({"u1", "u2", "u1des", "u2des"});

  matplot::show();
#else
  std::cout << "TRAJECTORY:" << std::endl;
  for (auto i = 0u; i != tvec.size(); ++i) {
    std::cout << "t=" << tvec[i] << ": x=" << xvec[i] << ", y=" << yvec[i] << std::endl;
  }
#endif

#ifdef ENABLE_ROS
  rclcpp::shutdown();
#endif

  return EXIT_SUCCESS;
}


#include <bits/types/FILE.h>
#include <fstream>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <string>
#include <tgmath.h>
#include "image_ppm.h"
#include <filesystem>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

typedef Matrix<u_char, Dynamic, Dynamic> MatrixImg;
typedef Matrix<double, Dynamic, Dynamic> TempMatrixImg;
typedef Vector<u_char,Dynamic> ImgLine;
typedef Vector<double,Dynamic> TempImgLine;

auto max(auto a, auto b){
    if (a<b) return b;
    else return a;
}

auto min(auto a, auto b){
    if (a>b) return b;
    else return a;
}

vector<double> projectOnEigenSpace(vector<TempImgLine> eigenfaces, TempImgLine imToProj,int K){
    vector<double> res = vector<double>();
    for (int i=0;i<K;i++){
        res.push_back(eigenfaces[i].dot(imToProj));
    }
    return res;
}

ImgLine octToVec(OCTET* im, int nH, int nW){
    ImgLine res(nH*nW);
    for (int i=0; i<nH*nW;i++){
        res(i)=im[i];
    }
    return res;
}

double eigenProjsDistance(vector<double> proj1, vector<double> proj2){

    double res =0.0;
    for (int i=0; i<min(proj1.size(),proj2.size());i++){
        res+=(proj1[i]-proj2[i])*(proj1[i]-proj2[i]);
    }
    return res;
}

//db is fileName ; names is labels ; values : values[person][imN][doubleM]
void processDB(string db, vector<string>& names, vector<vector<vector<double>>>& values){
    ifstream file;
    file.open(db);
    values = vector<vector<vector<double>>>();
    string line ;
    while(getline(file, line)){
        size_t exclam = line.find('!');
        if (exclam != std::string::npos){
            names.push_back(line.substr(exclam+1));
            values.push_back(vector<vector<double>>());
        }
        else{
            if (line.size()>1){
                vector<double> res;
                stringstream ss(line);

                double num;
                while(ss>>num){
                    res.push_back(num);
                }
                values[values.size()-1].push_back(res);
            }
        }
    }
}

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

    if (argc<3){
        cout<<"usage : eigenfaces im.pgm"<<endl;
    }

    vector<TempImgLine> eigenfaces;
    int K =42; int nH,nW;

    for (int i=0; i<K;i++){
        OCTET* im;
        char name[100];

        sprintf(name,"/im%d.pgm",i);
        string eigenFolder = string(string(argv[1])+string(name));

        lire_nb_lignes_colonnes_image_pgm(eigenFolder.c_str(),&nH,&nW);
        allocation_tableau(im,OCTET,nH*nW);
        lire_image_pgm(eigenFolder.c_str(),im,nH*nW);
        TempImgLine eigenFace(nH*nW) ;
        double sum=0.0;
        for (int j=0;j<nH*nW;j++){
            eigenFace(j)=im[j]-127;
            sum+=(double)(im[j]-127)*(double)(im[j]-127);
        }
        for (int j=0;j<nH*nW;j++){
            eigenFace(j)=eigenFace(j)/sqrt(sum);
            }
        eigenfaces.push_back(eigenFace);
        free(im);
    }

    OCTET* img;
    lire_nb_lignes_colonnes_image_pgm(argv[2],&nH,&nW);
    TempImgLine imgLine(nH*nW);
    allocation_tableau(img,OCTET, nH*nW);
    lire_image_pgm(argv[2],img,nH*nW);
    double sumIm=0.0;
    for (int i=0;i<nH*nW;i++){sumIm+=(double)(img[i]-127)*(double)(img[i]-127);}
    for (int i=0; i<nH*nW;i++){imgLine(i)=(double)(img[i]-127)/sumIm;}

    //registre[countDirs].push_back(projectOnEigenSpace(eigenfaces,imgLine,K));
    vector<double> imProj = projectOnEigenSpace(eigenfaces,imgLine,K);

    vector<string> labels;
    vector<vector<vector<double>>> values;
    processDB("DBEigen.txt", labels, values);
    double minDist=INFINITY;
    int closestMatch = -1;
    for (int i=0;i<values.size();i++){
        for (int j=0; j<values[i].size();j++){
            double dist = eigenProjsDistance(imProj, values[i][j]);
            //cout<<labels[i]<<" "<<dist<<endl;
            if (dist<minDist){
                //cout<<dist<<" "<<i<<endl;
                minDist=dist;
                closestMatch=i;

            }
        }
    }

     if (closestMatch>=0){cout<<"\n **************** \nCLOSEST MATCH FOUND : "<<labels[closestMatch]<<"\n ****************"<<endl;}
    else {cout<<"No match found !"<<endl;}

}