#include <algorithm>
#include <iostream>
#include <random>
#include <vector>
#include <boost/filesystem.hpp>
#include <boost/thread/thread.hpp>
#include "sampling.h"
#include "utilities.h"

const int MAX_BATCH = 13;
const int MAX_PATHS = 40320;

/*
    Single unit of the Travellers Algorithm

    A set of paths are generated. These paths start at the 'source' point, go
    through each point in 'points' and end at the 'destination' point.

    The maximum number of paths that can be generated is 'max_paths'. If the
    number of possible permutations for the paths is larger than 'max_paths',
    the paths generated will be chosen at random.

    Once the set of paths has been generated, they are sorted on a scale from
    0 to 1. The 0 indicates the path with the shortest distance and 1 indicates
    the path with the longest. The path chosen is the path which is nearest on
    the scale to 'calibration'.

    This function returns the indicies of the 'points' of the chosen path
*/
std::vector<int> travel(
    const point& source,
    const point& destination,
    const std::vector<point>& points,
    const float& calibration,
    const int& max_paths=MAX_PATHS
)
{
    std::random_device rd;
    std::mt19937 gen(rd());

    /*
        Create a 2d matrix which stores the euclidean distances between each of
        the points.
    */
    matrix graph = euclideanMatrix(source, points, destination);

    int n = points.size();
    int n_factorial = factorial(n);
    int permutation_count;
    std::vector<int> values;

    if (n_factorial <= max_paths) {
        for (int i = 0; i < n_factorial; ++i) {
            values.emplace_back(i);
        }
        std::shuffle(values.begin(), values.end(), gen);
        permutation_count = n_factorial;
    }
    else {
        permutation_count = max_paths;
        values = sampling::sample_range(permutation_count, n_factorial, gen);
    }

    permutation permutation;
    std::vector<permutation_cost> permutation_costs;
    for (int value: values) {

        float cost = 0;
        int current_index = 0;

        permutation = integer_to_permutation(value, n);

        for (int next_index: permutation) {
            cost += graph[current_index][next_index];
            current_index = next_index;
        }

        cost += graph[current_index][n+1];

        permutation_costs.push_back(permutation_cost(permutation, cost));
    }

    /*
        Order the permutation_costs from lowest to highest.
        Select the permutation according to the calibration.
    */
    sort(permutation_costs.begin(), permutation_costs.end(), sortByCost);

    return permutation_costs[calibration*(permutation_count-1)].first;
}

std::vector<std::pair<int, permutation>> indexed_permutation;
boost::mutex mutex;

void shall(
    int& index,
    const point& source,
    const point& destination,
    const std::vector<point>& points,
    const float& calibration
)
{
    permutation perm = travel(source, destination, points, calibration);

    mutex.lock();
    indexed_permutation.emplace_back(index, perm);
    mutex.unlock();
}

/*
     Batching unit of the Travellers Algorithm

     Points are batched the the single unit Travellers Algorithm is applied to
     each batch, returning the indicies in the order as if the path sequentially
     passes through each batch.
*/
std::vector<int> travels(std::vector<point> points, const float& calibration)
{
    int n = points.size();
    std::vector<int> batch_counts = get_batches(n, MAX_BATCH);

    std::random_device rd;
    std::mt19937 gen(rd());

    std::vector<int> indicies = sampling::sample_range(n, n, gen);
    std::vector<int> results;

    point source = points[indicies[0]];
    point destination;

    std::vector<point> batch_points;
    int i = 1;

    std::vector<boost::thread> threads;

    for (int k = 0; k < batch_counts.size(); ++k) {

        batch_points.clear();
        for (int j = 0; j < batch_counts[k] - 1; ++j) {
            batch_points.emplace_back(points[indicies[i++]]);
        }

        destination = points[indicies[i++ % n]];

        threads.emplace_back(std::move(boost::thread(
            shall,
            i-batch_counts[k]-1,
            source,
            destination,
            batch_points,
            calibration
        )));

        destination = source;
    }

    for (int i = 0; i < threads.size(); ++i) {
        threads[i].join();
    }

    for (std::pair<int, std::vector<int>> pair: indexed_permutation) {
        results.emplace_back(indicies[pair.first]);
        for (int num: pair.second) {
            results.emplace_back(indicies[pair.first+num]);
        }
    }

    return results;
}

int main(int argc, char* argv[])
{
    const int count = atoi(argv[1]);
    const int dimension = atoi(argv[2]);
    const float calibration = atof(argv[3]);

    std::vector<point> points;
    point temp;

    for (int i = 0; i < count; ++i) {
        temp.clear();
        for (int j = 0; j < dimension; ++j) {
            temp.emplace_back(atof(argv[i*dimension + j + 4]));
        }
        points.emplace_back(temp);
    }

    std::vector<int> indicies = travels(points, calibration);

    std::cout << indicies[0];
    for (int i = 1; i < indicies.size(); ++i) {
        std::cout << " " << indicies[i];
    }

    return 0;
}


#include <cassert>
#include <cmath>
#include <iostream>
#include <limits>

#include <boost/multiprecision/integer.hpp>

#include <fc/uint128.hpp>

using fc::uint128_t;

uint8_t find_msb( const uint128_t& u )
{
   uint64_t x;
   uint8_t places;
   x      = (u.lo ? u.lo : 1);
   places = (u.hi ?   64 : 0);
   x      = (u.hi ? u.hi : x);
   return uint8_t( boost::multiprecision::detail::find_msb(x) + places );
}

uint64_t maybe_sqrt( uint64_t x )
{
   if( x <= 1 )
      return 0;
   assert( x <= std::numeric_limits< uint64_t >::max()/2 );

   uint8_t n = uint8_t( boost::multiprecision::detail::find_msb(x) );
   uint8_t b = n&1;
   uint64_t y = x + (uint64_t(1) << (n-(1-b)));
   y = y >> ((n >> 1)+b+1);
   return y;
}

uint64_t approx_sqrt( const uint128_t& x )
{
   if( (x.lo == 0) && (x.hi == 0) )
      return 0;

   uint8_t msb_x = find_msb(x);
   uint8_t msb_z = msb_x >> 1;

   uint128_t msb_x_bit = uint128_t(1) << msb_x;
   uint64_t  msb_z_bit = uint64_t (1) << msb_z;

   uint128_t mantissa_mask = msb_x_bit - 1;
   uint128_t mantissa_x = x & mantissa_mask;
   uint64_t mantissa_z_hi = (msb_x & 1) ? msb_z_bit : 0;
   uint64_t mantissa_z_lo = (mantissa_x >> (msb_x - msb_z)).lo;
   uint64_t mantissa_z = (mantissa_z_hi | mantissa_z_lo) >> 1;
   uint64_t result = msb_z_bit | mantissa_z;

   return result;
}

int main( int argc, char** argv, char** envp )
{
   uint128_t x = 0;
   while( true )
   {
      uint64_t y = approx_sqrt(x);
      std::cout << std::string(x) << " " << y << std::endl;
      //uint128_t new_x = x + (x >> 10) + 1;
      uint128_t new_x = x + (x / 1000) + 1;
      if( new_x < x )
         break;
      x = new_x;
   }
   return 0;
}


#include <iostream>
#include <boost/math/common_factor.hpp>

int main()
{
	boost::uint32_t x = 250000;
	boost::uint32_t y = 5000000;

	std::cout << "lcm(" << x << ", " << y << ") == " << boost::math::lcm<boost::uint32_t>(x, y) << std::endl;

	return 0;
}


/* Copyright © 2019 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */

#include <algorithm>
#include <cmath>
#include <limits>
#include <vector>
#include <random>

#include <boost/gil/gil_all.hpp>

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

namespace turi {
namespace one_shot_object_detection {
namespace data_augmentation {

class Line {
public:
  /* Equation of Line from two points */
  Line(Eigen::Vector3f P1, Eigen::Vector3f P2);
  bool side_of_line(size_t x, size_t y);

private:
  float m_a, m_b, m_c; // ax + by + c = 0
};

/* Returns true if (x,y) is inside a quadrilateral
 * defined by corners, where corners are in cyclic
 * order from top right to bottom left.
 */
bool is_in_quadrilateral(size_t x, size_t y,
  const std::vector<Eigen::Vector3f> &corners);

/* mask is an image with all pixels set to black and
 * mask_complement is an image with all pixels set to white.
 * This function colors the pixels inside the convex quadrilateral defined by
 * corners with white for the mask and black for the mask_complement.
 */
void color_quadrilateral(const boost::gil::rgba8_image_t::view_t &transformed_view,
                         const std::vector<Eigen::Vector3f> &corners);

} // data_augmentation
} // one_shot_object_detection
} // turi


#include <iostream>
#include <string>
#include <vector>
#include <regex>
#include <optional>

#include <boost/multiprecision/cpp_int.hpp>

using boost::multiprecision::int128_t;

struct Cube {
    Cube(int128_t x1, int128_t x2, int128_t y1, int128_t y2, int128_t z1, int128_t z2) :
        x1(x1), x2(x2), y1(y1), y2(y2), z1(z1), z2(z2) {
        assert(x2 >= x1);
        assert(y2 >= y1);
        assert(z2 >= z1);
    }

    auto operator<=>(Cube const& rhs) const = default;

    bool exceedsLimit(int128_t limit) const {
        if (abs(x1) > limit || abs(x2) > limit || abs(y1) > limit || abs(y2) > limit || abs(z1) > limit || abs(z2) > limit)
            return true;
        return false;
    }

    int128_t Size() const {
        return (1+x2-x1) * (1+y2-y1) * (1+z2-z1);
    }

    std::optional<Cube> Intersects(const Cube& cube) const;

    int128_t x1;
    int128_t x2;
    int128_t y1;
    int128_t y2;
    int128_t z1;
    int128_t z2;
};

std::ostream& operator<<(std::ostream& os, const Cube& cube) {
    os << "(" << cube.x1 << " " << cube.x2 << " " << cube.y1 << " " << cube.y2 << " " << cube.z1 << " " << cube.z2 << ")\n";
    return os;
}

std::optional<Cube> Cube::Intersects(const Cube& cube) const {
    std::optional<Cube> result;
    int128_t xx1 = std::max(x1, cube.x1);
    int128_t xx2 = std::min(x2, cube.x2);
    int128_t yy1 = std::max(y1, cube.y1);
    int128_t yy2 = std::min(y2, cube.y2);
    int128_t zz1 = std::max(z1, cube.z1);
    int128_t zz2 = std::min(z2, cube.z2);
    if (xx2 >= xx1 && yy2 >= yy1 && zz2 >= zz1) {
        result = Cube(xx1, xx2, yy1, yy2, zz1, zz2);
    }
    return result;
}

class Reactor {
public:
    Reactor() {};
    Reactor(const std::optional<int128_t>& limit) : limit_(limit) {};

    void on(const Cube& cube);
    void off(const Cube& cube);

    int128_t count();

private:
    std::optional<int128_t> limit_;
    std::vector<Cube> state_;
};

int128_t Reactor::count() {
    int128_t total = 0;
    for (auto cube : state_) {
        total += cube.Size();
    }
    return total;
}

void Reactor::on(const Cube& cube) {
    if (limit_.has_value() && cube.exceedsLimit(limit_.value()))
        return;
    off(cube);
    state_.emplace_back(cube);
}

void Reactor::off(const Cube& cube) {
    if (limit_.has_value() && cube.exceedsLimit(limit_.value()))
        return;

    std::vector<Cube> new_state;
    for (auto& a : state_) {
        auto intersect = cube.Intersects(a);
        if (intersect.has_value()) {
            {
                int128_t x1 = a.x1;
                int128_t x2 = intersect.value().x1 - 1;
                if (x1 <= x2)
                    new_state.emplace_back(x1, x2, a.y1, a.y2, a.z1, a.z2);
            }
            {
                int128_t x1 = intersect.value().x2 + 1;
                int128_t x2 = a.x2;
                if (x1 <= x2)
                    new_state.emplace_back(x1, x2, a.y1, a.y2, a.z1, a.z2);
            }

            {
                int128_t y1 = a.y1;
                int128_t y2 = intersect.value().y1 - 1;
                if (y1 <= y2)
                    new_state.emplace_back(intersect.value().x1, intersect.value().x2, y1, y2, a.z1, a.z2);
            }
            {
                int128_t y1 = intersect.value().y2 + 1;
                int128_t y2 = a.y2;
                if (y1 <= y2)
                    new_state.emplace_back(intersect.value().x1, intersect.value().x2, y1, y2, a.z1, a.z2);
            }

            {
                int128_t z1 = a.z1;
                int128_t z2 = intersect.value().z1 - 1;
                if (z1 <= z2)
                    new_state.emplace_back(intersect.value().x1, intersect.value().x2, intersect.value().y1, intersect.value().y2, z1, z2);
            }
            {
                int128_t z1 = intersect.value().z2 + 1;
                int128_t z2 = a.z2;
                if (z1 <= z2)
                    new_state.emplace_back(intersect.value().x1, intersect.value().x2, intersect.value().y1, intersect.value().y2, z1, z2);
            }

        } else {
            new_state.emplace_back(a);
        }
    }

    state_ = std::move(new_state);
}

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

    std::vector<std::string> lines;
    while (!std::cin.eof() && !std::cin.fail()) {
        std::string line;
        getline(std::cin, line);
        lines.push_back(line);
    }

    Reactor part_1(50);
    Reactor part_2;

    for (auto& line : lines) {
        const std::regex reg("(on|off) x=(-?\\d*)..(-?\\d*),y=(-?\\d*)..(-?\\d*),z=(-?\\d*)..(-?\\d*)");
        std::smatch match;
        if (regex_search(line, match, reg)) {
            bool on = match.str(1) == "on";
            int128_t x1 = stoi(match.str(2));
            int128_t x2 = stoi(match.str(3));
            int128_t y1 = stoi(match.str(4));
            int128_t y2 = stoi(match.str(5));
            int128_t z1 = stoi(match.str(6));
            int128_t z2 = stoi(match.str(7));
            Cube cube(x1, x2, y1, y2, z1, z2);
            if (on) {
                part_1.on(cube);
                part_2.on(cube);
            } else {
                part_1.off(cube);
                part_2.off(cube);
            }
        }
    }

    std::cout << part_1.count() << std::endl;
    std::cout << part_2.count() << std::endl;

    return 0;
}


#include <boost/math/special_functions.hpp>
#include <cassert>

template <class T>
void check_float_inputs(T value) {
    assert(!boost::math::isinf(value));
    assert(!boost::math::isnan(value));

    if (boost::math::signbit(value)) {
        value = boost::math::changesign(value);
    }

    // ...
    assert(value + (std::numeric_limits<T>::epsilon)() >= static_cast<T>(0));
} // check_float_inputs

int main() {
    check_float_inputs(0.0);
    check_float_inputs(-110.0f);
    check_float_inputs(-11.0l);

    // Shall fail the `!boost::math::isinf(value)` assertion
    //check_float_inputs((std::numeric_limits<double>::max)() * 2.0);

    // Shall fail the `!boost::math::isnan(value)` assertion
    //check_float_inputs(std::sqrt(-1.0));
}


#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <string.h>
#include <chrono>
#include <ctime>
#include <iostream>
#include <cmath>
#include <cfloat>

#include <posit/posit>
#include <boost/range/combine.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include "defines.hpp"
#include "batch.hpp"
#include "utils.hpp"

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

cpp_dec_float_100 decimal_accuracy(cpp_dec_float_100 exact, cpp_dec_float_100 computed) {
        if (boost::math::isnan(exact) || boost::math::isnan(computed) ||
            (boost::math::sign(exact) != boost::math::sign(computed))) {
                return std::numeric_limits<cpp_dec_float_100>::quiet_NaN();
        } else if (exact == computed) {
                return std::numeric_limits<cpp_dec_float_100>::infinity();
        } else if ((exact == std::numeric_limits<cpp_dec_float_100>::infinity() &&
                    computed != std::numeric_limits<cpp_dec_float_100>::infinity()) ||
                   (exact != std::numeric_limits<cpp_dec_float_100>::infinity() &&
                    computed == std::numeric_limits<cpp_dec_float_100>::infinity()) || (exact == 0 && computed != 0) ||
                   (exact != 0 && computed == 0)) {
                return -std::numeric_limits<cpp_dec_float_100>::infinity();
        } else {
                return -log10(abs(log10(computed / exact)));
        }
}

void writeBenchmark(PairHMMFloat<cpp_dec_float_100> &pairhmm_dec50, PairHMMFloat<float> &pairhmm_float,
                    PairHMMPosit &pairhmm_posit, DebugValues<posit<NBITS, ES> > &hw_debug_values, std::string filename,
                    bool printDate, bool overwrite) {
        time_t t = chrono::system_clock::to_time_t(chrono::system_clock::now());

        ofstream outfile(filename, ios::out);
        if (printDate)
                outfile << endl << ctime(&t) << endl << "===================" << endl;

        auto dec_values = pairhmm_dec50.debug_values.items;
        auto float_values = pairhmm_float.debug_values.items;
        auto posit_values = pairhmm_posit.debug_values.items;
        auto hw_values = hw_debug_values.items;

        outfile << "name,dE_f,dE_p,dE_hw,log(abs(dE_f)),log(abs(dE_p)),log(abs(dE_hw)),E,E_f,E_p,E_hw,da_F,da_P,da_HW"
                << endl;
        for (int i = 0; i < dec_values.size(); i++) {
                cpp_dec_float_100 E, E_f, E_p, E_hw, dE_f, dE_p, dE_hw;
                cpp_dec_float_100 da_F, da_P, da_HW; // decimal accuracies

                string name = dec_values[i].name;
                E = dec_values[i].value;

                auto E_f_entry = std::find_if(float_values.begin(), float_values.end(), find_entry(name));
                E_f = E_f_entry->value;

                auto E_p_entry = std::find_if(posit_values.begin(), posit_values.end(), find_entry(name));
                E_p = E_p_entry->value;

                auto E_hw_entry = std::find_if(hw_values.begin(), hw_values.end(), find_entry(name));
                E_hw = E_hw_entry->value;

                if (name != E_f_entry->name || name != E_p_entry->name || name != E_hw_entry->name) {
                        cout << "Error: mismatching names! Could not find name '" << E_f_entry->name << endl;
                }

                da_F = decimal_accuracy(E, E_f);
                da_P = decimal_accuracy(E, E_p);
                da_HW = decimal_accuracy(E, E_hw);

                if (E == 0) {
                        dE_f = 0;
                        dE_p = 0;
                        dE_hw = 0;
                } else {
                        dE_f = (E_f - E) / E;
                        dE_p = (E_p - E) / E;
                        dE_hw = (E_hw - E) / E;
                }

                // Relative error values
                outfile << name << ",";
                outfile << setprecision(100) << fixed << dE_f << "," << dE_p << "," << dE_hw << "," << flush;
                outfile << setprecision(100) << fixed << log10(abs(dE_f)) << "," << log10(abs(dE_p)) << "," << log10(abs(dE_hw)) << "," << flush;
                outfile << setprecision(100) << fixed << E << "," << E_f << "," << E_p << "," << E_hw << "," << flush;
                outfile << setprecision(100) << fixed << da_F << "," << da_P << "," << da_HW << endl << flush;
        }
        outfile.close();
}

void print_batch_info(t_batch& batch) {
        DEBUG_PRINT("X:%d, PX:%d, PBPX:%d, Y:%d, PY:%d\n",
                    batch.init.x_size,
                    batch.init.x_padded,
                    batch.init.x_bppadded,
                    batch.init.y_size,
                    batch.init.y_padded
                    );
}

// padded read size
int px(int x, int y) {
        if (py(y) > PES)     // if feedback fifo is used
        {
                if (x <= PES) // and x is equal or smaller than number of PES
                {
                        x = PES + 1; // x will be no. PES + 1, +1 is due to delay in the feedback fifo path
                }
        } else          // feedback fifo is not used
        {
                if (x < PES) // x is smaller than no. PES
                {
                        x = PES; // pad x to be equal to no. PES
                }
        }
        return (x);
}

int pbp(int x) {
        return ((x / BASE_STEPS + (x % BASE_STEPS != 0)) * BASE_STEPS);
}

// padded haplotype size
int py(int y) {
        // divide Y by PES and round up and multiply:
        return ((y / PES + (y % PES != 0)) * PES);
}

t_workload *gen_workload(unsigned long pairs, unsigned long fixedX, unsigned long fixedY) {
        DEBUG_PRINT("Generating workload for %d pairs, with X=%d and Y=%d\n", (int) pairs, (int) fixedX, (int) fixedY);
        t_workload *workload = (t_workload *) malloc(sizeof(t_workload));

        if (fixedY < fixedX) {
                //printf("Haplotype cannot be smaller than read.\n");
                //exit(EXIT_FAILURE);
        }

        workload->pairs = pairs;

        if (workload->pairs % PIPE_DEPTH != 0) {
                printf("Number of pairs must be an integer multiple of %d.\n", PIPE_DEPTH);
                exit(EXIT_FAILURE);
        }

        workload->batches = pairs / PIPE_DEPTH;

        // Allocate memory
        workload->hapl = (uint32_t *) malloc(workload->pairs * sizeof(uint32_t));
        workload->read = (uint32_t *) malloc(workload->pairs * sizeof(uint32_t));
        workload->bx = (uint32_t *) malloc(workload->batches * sizeof(uint32_t));
        workload->by = (uint32_t *) malloc(workload->batches * sizeof(uint32_t));
        workload->bbytes = (size_t *) malloc(workload->batches * sizeof(size_t));
        workload->cups = 0;

        for (int i = 0; i < workload->pairs; i++) {
                workload->hapl[i] = fixedY;
                workload->read[i] = fixedX;
                workload->cups += fixedY * fixedX;
        }

        // Set batch info
        DEBUG_PRINT("Batch ║ MAX X ║ MAX Y ║ Passes ║\n");
        DEBUG_PRINT("════════════════════════════════\n");

        for (int b = 0; b < workload->pairs / PIPE_DEPTH; b++) {
                int xmax = 0;
                int ymax = 0;
                for (int p = 0; p < PIPE_DEPTH; p++) {
                        if (workload->read[b * PIPE_DEPTH + p] > xmax) {
                                xmax = workload->read[b * PIPE_DEPTH + p];
                        }
                        if (workload->hapl[b * PIPE_DEPTH + p] > ymax) {
                                ymax = workload->hapl[b * PIPE_DEPTH + p];
                        }
                }
                workload->bx[b] = xmax;
                workload->by[b] = ymax;

                DEBUG_PRINT("%5d ║ %5d ║ %5d ║ %6d ║\n", b, xmax, ymax, PASSES(ymax));
        }

        return (workload);
} // gen_workload

void copyProbBytes(t_probs& probs, uint8_t bytesArray[]) {
        int pos = 0;
        for(int i = 0; i < 8; i++) {
                bytesArray[pos++] = probs.p[i].x[0];
                bytesArray[pos++] = probs.p[i].x[1];
                bytesArray[pos++] = probs.p[i].x[2];
                bytesArray[pos++] = probs.p[i].x[3];
        }
}

int roundToMultiple(int toRound, int multiple) {
    toRound += multiple / 2;
    return toRound - (toRound%multiple);
}

// From: https://stackoverflow.com/questions/440133/how-do-i-create-a-random-alpha-numeric-string-in-c
std::string randomBasepairs(int len) {
    auto randchar = []() -> char
    {
        const char charset[] = "ACTG";
        const size_t max_index = sizeof(charset) - 1;
        return charset[rand() % max_index];
    };
    std::string str(len, 0);
    std::generate_n(str.begin(), len, randchar);

    return str;
}


#ifndef EXAMPLE_HANDCRAFTED_HPP
#define EXAMPLE_HANDCRAFTED_HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu)  2012.
// 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)

#include <double_number.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/less.hpp>

typedef boost::mpl::int_<11> val;

struct fib
{
  typedef fib type;

  template <class N>
  struct impl;

  template <class N>
  struct apply :
    boost::mpl::eval_if<
      typename boost::mpl::less<N, boost::mpl::int_<2> >::type,
      boost::mpl::int_<1>,
      impl<N>
    >
  {};
};

template <class N>
struct fib::impl :
  boost::mpl::plus<
    typename fib::apply<
      typename boost::mpl::minus<N, boost::mpl::int_<1> >::type
    >::type,
    typename fib::apply<
      typename boost::mpl::minus<N, boost::mpl::int_<2> >::type
    >::type
  >
{};

struct fact
{
  typedef fact type;

  template <class N>
  struct impl;

  template <class N>
  struct apply :
    boost::mpl::eval_if<
      typename boost::mpl::less<N, boost::mpl::int_<1> >::type,
      boost::mpl::int_<1>,
      impl<N>
    >
  {};
};

template <class N>
struct fact::impl :
  boost::mpl::times<
    N,
    typename fact::apply<
      typename boost::mpl::minus<N, boost::mpl::int_<1> >::type
    >::type
  >
{};

struct times4
{
  typedef times4 type;

  template <class N>
  struct apply : double_number<typename double_number<N>::type> {};
};

struct times11
{
  typedef times11 type;

  template <class N>
  struct apply : boost::mpl::times<N, val> {};
};

#endif


// Ben Martin
// January 31, 2006

// This currently works only on undirected graphs...
// Graph must model Adjacency Graph, Incidence Graph, VertexListGraph

#ifndef BOOST_GRAPH_SPARSE_SPECTRUM_HPP
#define BOOST_GRAPH_SPARSE_SPECTRUM_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/properties.hpp>
#include <boost/vector_property_map.hpp>
#include <utility> // for pair

//#include <iostream.h>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <ietl/vectorspace.h>
#include <ietl/lanczos.h>
#include <ietl/iteration.h>
#include <boost/random/linear_congruential.hpp>
#include <ietl/interface/ublas.h>

typedef long int integer;
typedef double doublereal;

namespace boost {

  template <typename Graph, typename EigenvectorMatrix >
  void sparse_spectrum(Graph& g,
		       int first_eigenvector_index,
		       int num_eigenvectors,
		       EigenvectorMatrix &eigenvectors,
		       double rel_tol = 100,
		       double abs_tol = 1000)
  {
    std::vector<double> evals(num_eigenvectors);
    sparse_spectrum(g, first_eigenvector_index, num_eigenvectors, eigenvectors, evals, rel_tol, abs_tol);
  }

  // Parameters:
  //   first_eigenvector_index:
  //     Since the smallest eigenvector is not useful, often this will
  //       be set to 1, for the "Fiedler vector," though if all
  //       eigenvectors are desired, 0 may be a more logical value.
  //     Negative values are interpreted as allowing a default choice of 1.
  //   num_eigenvectors:
  //     The number of eigencectors to return.

  template <typename Graph, typename EigenvectorMatrix, typename EVector >
  void sparse_spectrum(Graph& g,
		       int first_eigenvector_index,
		       int num_eigenvectors,
		       EigenvectorMatrix &eigenvectors,
		       EVector &eigenalues,
		       double rel_tol = 100,
		       double abs_tol = 1000)
  {

    //    Matrix eigenvectors = *(in_eigenvectors);

    if (first_eigenvector_index < 0)
      first_eigenvector_index = 1;

    typedef typename property_map<Graph, vertex_index_t>::const_type IndexMap;
    typedef typename Graph::vertex_iterator VertexIterator;
    typedef typename Graph::edge_iterator EdgeIterator;
    typedef typename boost::graph_traits<Graph>::adjacency_iterator AdjacencyIterator;

    IndexMap index_map = get(vertex_index, g);

    VertexIterator v, vs, ve;
    using std::pair;
    std::pair<VertexIterator, VertexIterator> p;
    p = vertices(g);
    vs = p.first;
    ve = p.second;

    EdgeIterator e, es, ee;
    using std::pair;
    std::pair<EdgeIterator, EdgeIterator> ep;
    ep = edges(g);
    es = ep.first;
    ee = ep.second;
    typename Graph::vertex_descriptor src, tgt;

    integer N = num_vertices(g);

    typedef boost::numeric::ublas::compressed_matrix<double> Matrix;
    Matrix A(N, N);

    int i;

    AdjacencyIterator a, as, ae;
    std::pair<AdjacencyIterator, AdjacencyIterator> ap;

    /*
    for (v = vs; v != ve; ++v) {
      A(index_map[*v], index_map[*v]) = (double)out_degree(*v, g);
    }
    */
    for (e = es; e != ee; ++e) {
      src = source(*e, g);
      tgt = target(*e, g);
      if (src != tgt && A(index_map[src], index_map[tgt]) != -1) {
	A(index_map[src], index_map[tgt]) = (double)(-1);
	A(index_map[tgt], index_map[src]) = (double)(-1);
	A(index_map[src], index_map[src]) += (double)1;
	A(index_map[tgt], index_map[tgt]) += (double)1;
      }
    }

    /*
    cout << "index_map = " << endl;
    for (v = vs; v != ve; ++v) {
      cout << index_map[*v] << " ";
    }
    cout << endl;
    cout << "A = " << endl;
    for (i = 0; i < N; i++)
    {
      for (int j = 0; j < N; j++)
      {
        cout << A[i*N + j] << " ";
      }
        cout << endl;
    }
    */

    using namespace ietl;

    vectorspace<boost::numeric::ublas::vector<double> > VS(N);
    lanczos<Matrix, vectorspace<boost::numeric::ublas::vector<double> > > LanczosObject(A, VS);

    double _rel_tol = 1000.*std::numeric_limits<double>::epsilon();
    double _abs_tol = 10000.*std::numeric_limits<double>::epsilon();

    //    _rel_tol = 100.*std::numeric_limits<double>::epsilon();
    //    _abs_tol = 100.*std::numeric_limits<double>::epsilon();

    // SO far these are the best I have found:
    //    _rel_tol = 1000.*std::numeric_limits<double>::epsilon();
    //    _abs_tol = 10000.*std::numeric_limits<double>::epsilon();

    _rel_tol = rel_tol*std::numeric_limits<double>::epsilon();
    _abs_tol = abs_tol*std::numeric_limits<double>::epsilon();

    lanczos_iteration_nlowest<double> LanczosIterationControl(1000000, first_eigenvector_index + num_eigenvectors, _rel_tol, _abs_tol);
    //    std::cout << std::numeric_limits<double>::epsilon() << std::endl;
    boost::minstd_rand gen(1);
    //boost::rand48 gen(1);
    LanczosObject.calculate_eigenvalues(LanczosIterationControl, gen);
    std::vector<double> evals = LanczosObject.eigenvalues();

    //    for (int i = 0; i < N; i++)
    //      std::cout << evals[i] << " ";
    //    std::cout << std::endl;

    /*
    std::vector<int> multiplicities = LanczosObject.multiplicities();
    int lowest_eval_multiplicity = multiplicities[1];
    int second_eval_multiplicity;
    if (lowest_eval_multiplicity == 1 && multiplicities[2] > 1)
      second_eval_multiplicity = 2;
    else
      second_eval_multiplicity = 1;

    for (int i = 0; i < N; i++)
      std::cout << multiplicities[i] << " ";
    std::cout << std::endl;
    */

    std::vector<boost::numeric::ublas::vector<double> > evecs(num_eigenvectors);
    for (int j = 0; j < num_eigenvectors; j++)
      evecs[j] = *(new boost::numeric::ublas::vector<double>(N));
    std::vector<boost::numeric::ublas::vector<double> >::iterator out_it = evecs.begin();

    Info<double> info;
    std::vector<double>::iterator ebegin, eend;
    ebegin = evals.begin();
    ebegin += first_eigenvector_index;
    eend = evals.begin();
    //    eend += 4 + (1 - lowest_eval_multiplicity) + (1 - second_eval_multiplicity);
    eend += first_eigenvector_index + num_eigenvectors;
    LanczosObject.eigenvectors(ebegin, eend, out_it, info, gen, 100000);
    //    LanczosObject.eigenvectors(ebegin, eend, out_it, info, gen);

    /*
    // If eigenvalues are repeated, we need to copy eigenvectors
    if (lowest_eval_multiplicity > 1) {
      for (int i = 0; i < N; i++)
	evecs[1][i] = evecs[0][i];
      if (lowest_eval_multiplicity > 2) {
	for(int i = 0; i < N; i++)
	  evecs[2][i] = evecs[0][i];
      }
    }
    if (second_eval_multiplicity > 1) {
      for (int i = 0; i < N; i++)
	evecs[2][i] = evecs[1][i];
    }
    */

    //    for (int i = 0; i < 3; i++)
    //      for (int j = 0; j < N; j++)
    //	std::cout << evecs[i][j] << " ";
    //    cout << std::endl;

    //    std::cout << A << std::endl;
    //    std::cout << LanczosIterationControl.error_code() << endl;;
    //    std::cout << info.error_info(1) << " " << info.error_info(2) << " " << info.error_info(3) << std::endl;

    //    std::vector<Vector> retval(num_eigenvectors);
    for (int j = 0; j < num_eigenvectors; j++) {
      // retval[j] = *(new Vector(N));
      i = 0;
      for (v = vs; v != ve; ++v) {
	//	  retval[j][i] = evecs[first_eigenvector_index+j-1][i];
	eigenvectors[j][i] = evecs[first_eigenvector_index+j-1][i];

	i++;
      }
    }

    //    return retval;

  } // end spectrum()

} // end namespace boost

#endif // BOOST_GRAPH_SPARSE_SPECTRUM_HPP


#include <iostream>
#include <vector>

#include <boost/random.hpp>
#include <Eigen/Dense>

#include <algorithm>

#include <bitset>
#include <iomanip>

#include <state-observation/observer/extended-kalman-filter.hpp>
#include <state-observation/observer/linear-kalman-filter.hpp>

#include <boost/utility/binary.hpp>
#include <state-observation/tools/probability-law-simulation.hpp>

double testExtendedKalmanFilter()
{
  /// the number of samples
  const static stateObservation::Index kmax = 1000;

  /// define the type of the extended Kalman filter
  typedef stateObservation::ExtendedKalmanFilter ekf;

  /// instanciation of the extended Kalman filter
  static ekf f(4, 3, 1);

  /// The functor that describes the dynamics of the state
  /// and the measurement
  class KalmanFunctor : public stateObservation::DynamicalSystemFunctorBase
  {

  public:
    /// Constructor
    KalmanFunctor()
    {
      s_ = f.stateVectorRandom() * 0.1;
      m_ = f.measureVectorRandom() * 0.1;
      t_ = f.stateVectorRandom();
      n_ = f.measureVectorRandom();
      a_ = f.getAmatrixRandom() * 0.6;
      c_ = f.getCmatrixRandom();
    }

    /// The dynamics of the state xk1=f(xk,u,k)
    virtual ekf::StateVector stateDynamics(const ekf::StateVector & xk,
                                           const ekf::InputVector & u,
                                           stateObservation::TimeIndex k)
    {
      (void)k; // unused
      (void)u; // unused

      ekf::StateVector xk1;

      xk1 = a_ * xk + cos(10 * (xk.transpose() * xk)[0]) * s_ + t_ + (u.transpose() * u)(0, 0) * s_;
      return xk1;
    }

    /// The dynamics of the state yk=h(xk,u,k)
    virtual ekf::MeasureVector measureDynamics(const ekf::StateVector & xk,
                                               const ekf::InputVector & u,
                                               stateObservation::TimeIndex k)
    {
      (void)k; // unused
      (void)u; // unused

      ekf::MeasureVector yk;
      yk = c_ * xk + cos(10 * (xk.transpose() * xk)[0]) * m_ + n_;
      return yk;
    }

    virtual stateObservation::Index getStateSize() const
    {
      return f.getStateSize();
    }

    virtual stateObservation::Index getInputSize() const
    {
      return f.getInputSize();
    }

    virtual stateObservation::Index getMeasurementSize() const
    {
      return f.getMeasureSize();
    }

  private:
    /// containers for the vectors and matrices
    ekf::StateVector s_;
    ekf::StateVector t_;
    ekf::MeasureVector m_;
    ekf::MeasureVector n_;

    ekf::Amatrix a_;
    ekf::Cmatrix c_;
  };

  /// containers for the state, the measurements and the input
  ekf::StateVector xk[kmax + 1];
  ekf::MeasureVector yk[kmax];
  ekf::InputVector uk[kmax + 1];

  /// the standard deviation matrix to generate the gaussian noise
  ekf::Rmatrix r1 = f.getRmatrixRandom() * 0.01;
  ekf::Qmatrix q1 = f.getQmatrixRandom() * 0.01;

  /// instanciate the functor
  KalmanFunctor func;

  { /// Construction of the sequence of states measurements and inputs

    /// initializations
    ekf::StateVector x = f.stateVectorZero();
    xk[0] = x;
    uk[0] = f.inputVectorRandom();

    for(stateObservation::Index k = 1; k <= kmax; ++k)
    {
      /// generation of random inputs
      uk[k] = f.inputVectorRandom();

      /// generation of Gaussian white noises
      ekf::StateVector v = stateObservation::tools::ProbabilityLawSimulation::getGaussianMatrix(f.stateVectorZero(), q1,
                                                                                f.getStateSize(), 1);

      ekf::MeasureVector w = stateObservation::tools::ProbabilityLawSimulation::getGaussianMatrix(
          f.measureVectorZero(), r1, f.getMeasureSize());

      /// the dynamics is executed here
      xk[k] = x = func.stateDynamics(x, uk[k - 1], k - 1) + v;
      yk[k - 1] = func.measureDynamics(x, uk[k], k) + w;
    }
  }

  /// set the functor of the extended Kalman filter
  f.setFunctor(&func);

  /// generation of a random initial estimation of the state
  ekf::StateVector xh = f.stateVectorRandom();

  /// set the initial state of the estimator
  f.setState(xk[0], 0);

  /// set the covariance matrix of the initial estimation error
  ekf::Pmatrix p = f.getPmatrixZero();
  for(unsigned i = 0; i < f.getStateSize(); ++i)
  {
    p(i, i) = xh[i];
  }
  p = p * p.transpose();
  f.setStateCovariance(p);

  /// the covariance matrices for the process noise and the measurements noise
  ekf::Rmatrix r(r1 * r1.transpose());
  ekf::Qmatrix q(q1 * q1.transpose());

  /// set the covariance matrices for the extended Kalman filter
  f.setR(r);
  f.setQ(q);

  /// set initial input
  f.setInput(uk[0], 0);

  /// set the derivation step for the finite difference method
  ekf::StateVector dx = f.stateVectorConstant(1) * 1e-8;

  stateObservation::Index i;
  for(i = 1; i <= kmax; ++i)
  {
    /// give the measurements and the inputs at instant i to the ekf
    f.setMeasurement(yk[i - 1], i);
    f.setInput(uk[i], i);

    /// obtain jacobians by finite differences method
    ekf::Amatrix a = f.getAMatrixFD(dx);
    ekf::Cmatrix c = f.getCMatrixFD(dx);

    /// set the jacobians to the ekf
    f.setA(a);
    f.setC(c);

    /// get the estimation of the state at instant i;
    xh = f.getEstimatedState(i);
  }

  ekf::StateVector error = xh - xk[kmax];

  return error.norm();
}

double testExtendedKalmanFilterLTV()
{
  const static stateObservation::TimeIndex kmax = 1000;

  typedef stateObservation::ExtendedKalmanFilter ekf;

  static ekf f(4, 3, 1);

  struct KalmanFunctorLTV : public stateObservation::DynamicalSystemFunctorBase
  {

  public:
    KalmanFunctorLTV()
    {
      s_ = f.stateVectorRandom();
      n_ = f.measureVectorRandom();

      for(stateObservation::Index i = 0; i <= kmax; ++i)
      {
        a.push_back(f.getAmatrixRandom() * 0.5);
        c.push_back(f.getCmatrixRandom());
      }
    }

    virtual ekf::StateVector stateDynamics(const ekf::StateVector & x,
                                           const ekf::InputVector & u,
                                           stateObservation::TimeIndex k)
    {
      ekf::StateVector xk1;
      stateObservation::TimeIndex kk = std::min(k, kmax);
      xk1 = a[kk] * x + (u.transpose() * u)(0, 0) * s_;

      return xk1;
    }

    virtual ekf::MeasureVector measureDynamics(const ekf::StateVector & x,
                                               const ekf::InputVector & u,
                                               stateObservation::TimeIndex k)
    {
      (void)k; // unused
      (void)u; // unused

      ekf::MeasureVector yk;
      stateObservation::TimeIndex kk = std::min(k, kmax);
      yk = c[kk] * x + (u.transpose() * u)(0, 0) * n_;
      return yk;
    }

    virtual stateObservation::Index getStateSize() const
    {
      return f.getStateSize();
    }

    virtual stateObservation::Index getInputSize() const
    {
      return f.getInputSize();
    }

    virtual stateObservation::Index getMeasurementSize() const
    {
      return f.getMeasureSize();
    }

    std::vector<ekf::Amatrix, Eigen::aligned_allocator<ekf::Amatrix>> a;
    std::vector<ekf::Cmatrix, Eigen::aligned_allocator<ekf::Cmatrix>> c;

  private:
    ekf::StateVector s_;
    ekf::MeasureVector n_;
  };

  KalmanFunctorLTV func;

  f.setFunctor(&func);

  ekf::StateVector xk[kmax + 1];
  ekf::MeasureVector yk[kmax];
  ekf::InputVector uk[kmax + 1];

  ekf::StateVector x = f.stateVectorZero();

  boost::lagged_fibonacci1279 gen_;

  ekf::Rmatrix r1 = f.getRmatrixRandom() * 0.01;

  ekf::Qmatrix q1 = f.getQmatrixRandom() * 0.01;

  ekf::Rmatrix r(r1 * r1.transpose());
  ekf::Qmatrix q(q1 * q1.transpose());

  xk[0] = x;
  uk[0] = f.inputVectorRandom();

  for(stateObservation::Index k = 1; k <= kmax; ++k)
  {
    ekf::StateVector v(f.stateVectorZero());
    for(unsigned i = 0; i < f.getStateSize(); ++i)
    {
      boost::normal_distribution<> g(0, 1);
      v[i] = g(gen_);
    }
    v = q1 * v;

    ekf::MeasureVector w(f.measureVectorZero());
    for(unsigned i = 0; i < f.getMeasureSize(); ++i)
    {
      boost::normal_distribution<> g(0, 1);
      w[i] = g(gen_);
    }
    w = r1 * w;

    uk[k] = f.inputVectorRandom();

    x = func.stateDynamics(x, uk[k - 1], k - 1) + v;

    xk[k] = x;
    yk[k - 1] = func.measureDynamics(x, uk[k], k) + w;
  }

  ekf::StateVector xh = f.stateVectorRandom();

  f.setState(xh, 0);

  ekf::Pmatrix p = f.getPmatrixZero();

  for(unsigned i = 0; i < f.getStateSize(); ++i)
  {
    p(i, i) = xh[i];
  }
  p = p * p.transpose();

  f.setStateCovariance(p);

  f.setR(r);
  f.setQ(q);

  f.setInput(uk[0], 0);

  for(unsigned i = 0; i < f.getStateSize(); ++i)
  {
    p(i, i) = xh[i];
  }
  p = p * p.transpose();

  ekf::StateVector dx = f.stateVectorConstant(1) * 1e-8;

  stateObservation::Index i;
  for(i = 1; i <= kmax; ++i)
  {
    f.setMeasurement(yk[i - 1], i);
    f.setInput(uk[i], i);

    ekf::Amatrix a = f.getAMatrixFD(dx);
    ekf::Cmatrix c = f.getCMatrixFD(dx);

    f.setA(a);
    f.setC(c);

    xh = f.getEstimatedState(i);
  }

  ekf::StateVector error = xh - xk[kmax];

  return error.norm();
}

double testExtendedKalmanFilterZeroInput()
{
  const static stateObservation::Index kmax = 1000;

  typedef stateObservation::ExtendedKalmanFilter ekf;

  static ekf f(4, 3);

  class KalmanFunctor : public stateObservation::DynamicalSystemFunctorBase
  {

  public:
    KalmanFunctor()
    {
      m_ = f.measureVectorRandom() * 0.1;
      s_ = f.stateVectorRandom() * 0.1;
      t_ = f.stateVectorRandom();
      n_ = f.measureVectorRandom();
    }

    virtual ekf::StateVector stateDynamics(const ekf::StateVector & x,
                                           const ekf::InputVector & u,
                                           stateObservation::TimeIndex k)
    {
      (void)k; // unused
      (void)u; // unused

      ekf::StateVector xk1;
      xk1 = a_ * x + cos(10 * (x.transpose() * x)[0]) * s_ + t_;
      return xk1;
    }

    virtual ekf::MeasureVector measureDynamics(const ekf::StateVector & x,
                                               const ekf::InputVector & u,
                                               stateObservation::TimeIndex k)
    {
      (void)k; // unused
      (void)u; // unused

      ekf::MeasureVector yk;
      yk = c_ * x + cos(10 * (x.transpose() * x)[0]) * m_ + n_;
      return yk;
    }

    void setA(const ekf::Amatrix & a)
    {
      a_ = a;
    }

    void setC(const ekf::Cmatrix & c)
    {
      c_ = c;
    }

    virtual stateObservation::Index getStateSize() const
    {
      return f.getStateSize();
    }

    virtual stateObservation::Index getInputSize() const
    {
      return f.getInputSize();
    }

    virtual stateObservation::Index getMeasurementSize() const
    {
      return f.getMeasureSize();
    }

  private:
    ekf::StateVector s_;
    ekf::StateVector t_;
    ekf::MeasureVector m_;
    ekf::MeasureVector n_;

    ekf::Amatrix a_;
    ekf::Cmatrix c_;
  };

  KalmanFunctor func;

  func.setA(f.getAmatrixRandom() * 0.5);
  func.setC(f.getCmatrixRandom());

  f.setFunctor(&func);

  ekf::StateVector xk[kmax + 1];
  ekf::MeasureVector yk[kmax];
  ekf::InputVector u = f.inputVectorZero();

  ekf::StateVector x = f.stateVectorZero();

  boost::lagged_fibonacci1279 gen_;

  ekf::Rmatrix r1 = f.getRmatrixRandom() * 0.01;

  ekf::Qmatrix q1 = f.getQmatrixRandom() * 0.01;

  ekf::Rmatrix r(r1 * r1.transpose());
  ekf::Qmatrix q(q1 * q1.transpose());

  xk[0] = x;

  for(stateObservation::Index k = 1; k <= kmax; ++k)
  {
    ekf::StateVector v(f.stateVectorZero());
    for(unsigned i = 0; i < f.getStateSize(); ++i)
    {
      boost::normal_distribution<> g(0, 1);
      v[i] = g(gen_);
    }
    v = q1 * v;

    ekf::MeasureVector w(f.measureVectorZero());
    for(unsigned i = 0; i < f.getMeasureSize(); ++i)
    {
      boost::normal_distribution<> g(0, 1);
      w[i] = g(gen_);
    }
    w = r1 * w;

    x = func.stateDynamics(x, u, k - 1) + v;

    xk[k] = x;
    yk[k - 1] = func.measureDynamics(x, u, k) + w;
  }

  ekf::StateVector xh = f.stateVectorRandom();

  f.setState(xk[0], 0);

  ekf::Pmatrix p = f.getPmatrixZero();

  f.setStateCovariance(p);

  f.setR(r);
  f.setQ(q);

  for(unsigned i = 0; i < f.getStateSize(); ++i)
  {
    p(i, i) = xh[i];
  }
  p = p * p.transpose();

  ekf::StateVector dx = f.stateVectorConstant(1) * 1e-8;

  stateObservation::Index i;
  for(i = 1; i <= kmax; ++i)
  {
    f.setMeasurement(yk[i - 1], i);

    ekf::Amatrix a = f.getAMatrixFD(dx);
    ekf::Cmatrix c = f.getCMatrixFD(dx);

    f.setA(a);
    f.setC(c);
    xh = f.getEstimatedState(i);
  }

  ekf::StateVector error = xh - xk[kmax];

  return error.norm();
}

double testKalmanFilter()
{

  typedef stateObservation::LinearKalmanFilter filter;

  filter f(4, 3, 2);
  Eigen::Matrix<double, 4, 4> a;

  a << -0.6785714, 0.1156463, 0.4392517, 0.2863946, 0.0865306, -0.0273810, 0.3355102, 0.0184150, -0.4172789, -0.2036735,
      -0.4434014, -0.2666667, 0.4200680, 0.5387075, 0.4883673, -0.6598639;

  filter::Bmatrix b = f.getBmatrixRandom();
  filter::Cmatrix c = f.getCmatrixRandom();
  filter::Dmatrix d = f.getDmatrixRandom();

  const stateObservation::Index kmax = 1000;

  filter::StateVector xk[kmax + 1];
  filter::MeasureVector yk[kmax];
  filter::InputVector uk[kmax + 1];

  filter::StateVector x = f.stateVectorZero();

  xk[0] = x;

  filter::Rmatrix r1 = f.getRmatrixRandom() * 0.01;

  filter::Qmatrix q1 = f.getQmatrixRandom() * 0.01;

  filter::Rmatrix r(r1 * r1.transpose());
  filter::Qmatrix q(q1 * q1.transpose());

  uk[0] = f.inputVectorRandom();

  boost::lagged_fibonacci1279 gen_;
  for(stateObservation::Index k = 1; k <= kmax; ++k)
  {

    filter::StateVector v = f.stateVectorZero();
    for(unsigned i = 0; i < f.getStateSize(); ++i)
    {
      boost::normal_distribution<> g(0, 1);
      v[i] = g(gen_);
    }
    v = q1 * v;

    filter::MeasureVector w = f.measureVectorZero();
    for(unsigned i = 0; i < f.getMeasureSize(); ++i)
    {
      boost::normal_distribution<> g(0, 1);
      w[i] = g(gen_);
    }
    w = r1 * w;

    uk[k] = f.inputVectorRandom();

    xk[k] = x = a * x + v + b * uk[k - 1];
    yk[k - 1] = c * x + w + d * uk[k];
  }

  filter::StateVector xh = f.stateVectorRandom();

  f.setState(xh, 0);

  filter::Pmatrix p = f.getPmatrixZero();

  for(unsigned i = 0; i < f.getStateSize(); ++i)
  {
    p(i, i) = xh[i];
  }
  p = p * p.transpose();

  f.setStateCovariance(p);

  f.setA(a);
  f.setB(b);
  f.setC(c);
  f.setD(d);

  f.setR(r);
  f.setQ(q);

  f.setInput(uk[0], 0);

  stateObservation::Index i;
  for(i = 1; i <= kmax; ++i)
  {
    f.setMeasurement(yk[i - 1], i);
    f.setInput(uk[i], i);
  }

  filter::StateVector error = f.getEstimatedState(kmax) - xk[kmax];

  return error.norm();
}

double testKalmanFilterZeroInput()
{
  typedef stateObservation::LinearKalmanFilter filter;

  filter f(4, 3);
  Eigen::Matrix<double, 4, 4> a;

  a << -0.6785714, 0.1156463, 0.4392517, 0.2863946, 0.0865306, -0.0273810, 0.3355102, 0.0184150, -0.4172789, -0.2036735,
      -0.4434014, -0.2666667, 0.4200680, 0.5387075, 0.4883673, -0.598639;

  filter::Bmatrix b = f.getBmatrixRandom();
  filter::Cmatrix c = f.getCmatrixRandom();
  filter::Dmatrix d = f.getDmatrixRandom();

  const stateObservation::Index kmax = 1000;

  filter::StateVector xk[kmax + 1];
  filter::MeasureVector yk[kmax];

  filter::StateVector x = f.stateVectorZero();

  xk[0] = x;

  boost::lagged_fibonacci1279 gen_;

  filter::Rmatrix r1 = f.getRmatrixRandom() * 0.01;

  filter::Qmatrix q1 = f.getQmatrixRandom() * 0.01;

  filter::Rmatrix r(r1 * r1.transpose());
  filter::Qmatrix q(q1 * q1.transpose());

  filter::InputVector u = f.inputVectorZero();

  for(stateObservation::Index k = 1; k <= kmax; ++k)
  {

    filter::StateVector v = f.stateVectorZero();
    for(unsigned i = 0; i < f.getStateSize(); ++i)
    {
      boost::normal_distribution<> g(0, 1);
      v[i] = g(gen_);
    }
    v = q1 * v;

    filter::MeasureVector w = f.measureVectorZero();
    for(unsigned i = 0; i < f.getMeasureSize(); ++i)
    {
      boost::normal_distribution<> g(0, 1);
      w[i] = g(gen_);
    }
    w = r1 * w;

    xk[k] = x = a * x + v;
    yk[k - 1] = c * x + w;
  }

  filter::StateVector xh = f.stateVectorRandom();

  f.setState(xh, 0);

  filter::Pmatrix p = f.getPmatrixZero();

  for(unsigned i = 0; i < f.getStateSize(); ++i)
  {
    p(i, i) = xh[i];
  }
  p = p * p.transpose();

  f.setStateCovariance(p);

  f.setA(a);
  f.setB(b);
  f.setC(c);
  f.setD(d);

  f.setR(r);
  f.setQ(q);

  stateObservation::Index i;

  for(i = 1; i <= kmax; ++i)
  {
    f.setMeasurement(yk[i - 1], i);
  }

  filter::StateVector error = f.getEstimatedState(kmax) - xk[kmax];

  return error.norm();
}

int main()
{
  short exit = 0;
  double error;
  std::cout << "Starting" << std::endl;

  if((error = testKalmanFilter()) < 0.1)
  {
    std::cout << "Test Kalman filter SUCCEEDED: estimationError = " << error << std::endl;
  }
  else
  {
    exit = exit | BOOST_BINARY(1);
    std::cout << "Test Kalman filter FAILED: estimationError = " << error << std::endl;
  }
  if((error = testKalmanFilterZeroInput()) < 0.1)
  {
    std::cout << "Test Kalman filter (zero input) SUCCEEDED: estimationError = " << error << std::endl;
  }
  else
  {
    exit = exit | BOOST_BINARY(10);
    std::cout << "Test Kalman filter (zero input) FAILED: estimationError = " << error << std::endl;
  }
  if((error = testExtendedKalmanFilter()) < 0.1)
  {
    std::cout << "Test extended Kalman filter SUCCEEDED: estimationError = " << error << std::endl;
  }
  else
  {
    exit = exit | BOOST_BINARY(100);
    std::cout << "Test extended Kalman filter FAILED: estimationError = " << error << std::endl;
  }
  if((error = testExtendedKalmanFilterLTV()) < 0.1)
  {
    std::cout << "Test extended Kalman filter (LTV) SUCCEEDED: estimationError = " << error << std::endl;
  }
  else
  {
    exit = exit | BOOST_BINARY(1000);
    std::cout << "Test extended Kalman filter (LTV) FAILED: estimationError = " << error << std::endl;
  }
  if((error = testExtendedKalmanFilterZeroInput()) < 0.1)
  {
    std::cout << "Test extended Kalman filter (zero input) SUCCEEDED: estimationError = " << error << std::endl;
  }
  else
  {
    exit = exit | BOOST_BINARY(10000);
    std::cout << "Test extended Kalman filter (zero input) FAILED: estimationError = " << error << std::endl
              << std::endl;
  }

  std::cout << "Test exit code " << std::bitset<16>(exit) << std::endl;

  return exit;
}


#include <Eigen/Core>
#include <iostream>

#include <vsnakes/utils.h>
#include <vsnakes/vsnakes.h>

namespace
{
std::vector<Eigen::Vector2i> generateInitialAnchors(int rows = 64, int cols = 64, int numAnchors = 8)
{
    const auto center = cols / 2;
    const auto delta = rows / numAnchors;

    std::vector<Eigen::Vector2i> anchors;

    for (int aidx = 0; aidx < numAnchors; ++aidx)
    {
        Eigen::Vector2i anchor =
            (aidx % 2 == 0) ? Eigen::Vector2i{center - 5, aidx * delta} : Eigen::Vector2i{center + 5, aidx * delta};
        anchors.push_back(std::move(anchor));
    }

    return anchors;
}

Eigen::MatrixXf generateEnergyMap(int rows = 64, int cols = 64)
{
    Eigen::MatrixXf energyMap(rows, cols);

    const int half = cols / 2;

    for (int i = 0; i < rows; ++i)
    {
        for (int j = 0; j < half; ++j)
        {
            energyMap(i, j) = static_cast<float>(cols - j);
        }

        for (int j = half; j < cols; ++j)
        {
            energyMap(i, j) = static_cast<float>(cols - half + 1 + j);
        }
    }

    return energyMap;
}

}  // namespace

int main(int argc, char* argv[])
{
    /// TODO: Allow passing a set of anchor and an image for the optimization

    // Inputs for the optimization
    const int rows = 64;
    const int cols = 64;
    const int numAnchors = 8;
    const auto spacing = 8;

    const std::vector<Eigen::Vector2i> anchors = generateInitialAnchors(rows, cols, numAnchors);
    const Eigen::MatrixXf energyMap = generateEnergyMap(rows, cols);

    const auto optimizedAnchors = vsnakes::optimizeAnchorsFirstOrder(0.001f, 1.f, spacing, 200, anchors, energyMap);

    vsnakes::printAnchors("Initial", anchors);
    vsnakes::printAnchors("Optimized", optimizedAnchors);
}


#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Dense>
#include <queue>
// #include<Eigen/SparseCore>
using namespace Rcpp;
using namespace Eigen;
using namespace std;

// [[Rcpp::depends(RcppEigen)]]

//
using Eigen::Map;               	// 'maps' rather than copies
using Eigen::Matrix;                  //  matrix generic
using Eigen::MatrixXd;                  // variable size matrix, double precision
using Eigen::VectorXd;                  // variable size vector, double precision
using Eigen::Transpositions;
using Eigen::HouseholderQR;    // Fast scalable QR solver
using Eigen::ColPivHouseholderQR;    // Fast scalable QR solver
using Eigen::FullPivHouseholderQR; // slow full (colsand rows pivoting)
using Eigen::JacobiSVD;
using Eigen::GeneralizedSelfAdjointEigenSolver;    // one of the eigenvalue solvers
using Eigen::SelfAdjointEigenSolver;    // one of the eigenvalue solvers
using Eigen::LLT;
using Eigen::LDLT;
using Rcpp::List;
using Rcpp::wrap;

// ##########  OK vrsione Sept 04 works

// copied to fspca_sept.cpp

// =========================================================================

// creates a sub-mat of S with indices in e base 0
Eigen::MatrixXd makeSubS(Eigen::MatrixXd S, Eigen::VectorXi e){
  int n = S.cols();
  int r = S.rows();
  int d = e.size();
  if (d >= n) {
    Rf_error("Too many indices to eliminate.\n");
  }
  if (e.maxCoeff() > n){
    Rf_error("largest index greater than the number of columns.\n");
  }

  Eigen::MatrixXd M(r, d );
  for (int i = 0; i < d; ++i){
    M.col(i) = S.col(e(i));
  }
  for (int i = 0; i < d; ++i){
    M.row(i) = M.row(e(i));
  }

  return M.topLeftCorner(d, d);
}

// retruns the rows in e and keeps first c columns
Eigen::MatrixXd selectRowsC(Eigen::MatrixXd A, Eigen::VectorXi e, int c){
  // ATTENZIONE INDICES BASE 0
  // ATTENZIONE e must be sorted e(0) < e(1)

  int n = A.cols();
  int r = A.rows();
  int d = e.size();
  if (d >= n) {
    Rf_error("Too many indices to eliminate.\n");
  }
  if (e.maxCoeff() > n){
    Rf_error("largest index greater than the number of columns.\n");
  }

  Eigen::MatrixXd M(A.topLeftCorner(r,c));
  for (int i = 0; i < d; ++i){
    M.row(i) = M.row(e(i));
  }

  return M.topLeftCorner(d, c);
}

void   makeSdAndM(Eigen::MatrixXd S, Eigen::VectorXi e, Eigen::MatrixXd& M, Eigen::MatrixXd& N,
                  int n, int d){
  // M(d, r) N(d, d)
  if (d >= n) {
    Rf_error("Too many indices to eliminate.\n");
  }
  if (e.maxCoeff() > n){
    Rf_error("largest index greater than the number of columns.\n");
  }

  //  Eigen::MatrixXd M(d, r);
  for (int i = 0; i < d; ++i){
    M.row(i) = S.row(e(i));
  }
  //  Eigen::MatrixXd N = M.topLeftCorner(d, d);
  for (int i = 0; i < d; ++i){
    N.col(i) = M.col(e(i));
  }
}

// Deflates S (pass already deflated and vector current loads)
// returns vexp by ref
void deflSC(Eigen::VectorXd a, Eigen::MatrixXd& K, Eigen::VectorXi ind, double& vexp){
  // # pass only a nonzero loads
  // K = deflated matrix
  // #  K <-- (S - Saa'S/(a'Sa) // deflated S matrix
  // ## ===
  const int n = ind.size();
  const int p = K.cols();

  // t = Sa
  Eigen::VectorXd t = Eigen::VectorXd::Zero(p);
  for (int i = 0; i < p; i++)
    for(int k = 0; k < n; k++)
      t(i) += K(i, ind(k)) * a(k ); // only elements in ind
  // tt = a'Sa = t'a

  double tt = 0.0;
  for(int k = 0; k < n; k++)
    tt += a(k) * t(ind(k));
  if (tt > 0)
    tt = 1/tt;
  else
    Rf_error("defSC: tt is not > 0");

  // O = Sa/(tt)
  const Eigen::VectorXd O = (t.array()*tt).matrix();

  const double cvk = K.trace();
  // K = S - Saa'S/(a'Sa) deflated S
  Eigen::MatrixXd L = t * O.transpose();
  K = K - t * O.transpose(); //deflated S
  vexp =  cvk - K.trace() ;

  return;
}
//

// finds max part corr exclude small ss, pdates indnot returns ind
int findmax(Eigen::VectorXi& indnot, Eigen::VectorXd vt){

  double p = indnot.size();
  double m = 0.0;
  int ind = 0;
  for (int i = 0; i < p; i++){
    if (indnot(i) == -2){
      if(vt(i) > m){
        m = vt(i);
        ind = i;
      }
    }
  }
  indnot(ind) = ind;
  return ind;
}

// indnot could be used for extracting the indices later, so use -2, -1 and {0:(p-1)}
// fixed
void fwd_selectC(Eigen::MatrixXd S, Eigen::VectorXi& ind, int& card,
                 Eigen::VectorXd si, double totvexp, double pvexp,
                 double fullrank = 0.0){
  Eigen::VectorXd sik = si;
  int p = S.cols();
  // int induno;
  double tmp;
  Eigen::VectorXd vexpt(p);
  Eigen::VectorXd cvexpt(p);
  Eigen::VectorXd vt(p);
  Eigen::VectorXi indnot = Eigen::VectorXi::Constant(p, -2);
  Eigen::VectorXd ba(p);

  for (int i=0; i < p; i++)
    vt(i) = sik(i) * sik(i) / S(i,i);

  ind(0) = findmax(indnot, vt);

  vexpt(0) = vt(ind(0));
  cvexpt(0) = vt(ind(0));
  int i = 1;
  bool stopSelect = false;
  // start looping ============================================
  while (stopSelect == false){

    tmp = sik(ind(i - 1))/S(ind(i - 1), ind(i - 1));
    for (int j = 0; j < p; j++){
      if ( indnot(j) == -2){
        sik(j) = sik(j) -  (tmp * S(ind(i-1), j));
      }
      else{
        sik(j) = 0;
      }
    }

    ba = (S.col(ind(i-1)).array()/sqrt(S(ind(i-1), ind(i-1)))).matrix();
    S = S - ba * ba.transpose();

    for (int j = 0; j < p; j++){
      if ( indnot(j) == -2){
        if (S(j,j)> fullrank)
          vt(j) = sik(j) * sik(j)/S(j,j);
        else{
          indnot(j) = -1;
          vt(j) = 0;
        }
      }
      else{
        vt(j) = 0;
      }
    }

    ind(i) = findmax(indnot, vt);
    indnot(ind(i)) = 0;

    vexpt(i) =  vt(ind(i));
    cvexpt(i) = cvexpt(i-1) + vexpt(i);

    if (cvexpt(i) >= pvexp*totvexp){
      card = i + 1;
      stopSelect = true;
    }
    else{
      i = i + 1;
    }
    //    Rcpp::checkUserInterrupt();

  }
}

// power method computes only first eigvec, about 82 times faster tha eigen!
Eigen::VectorXd eigvecPMC(Eigen::MatrixXd& X, double& val, double eps = 10E-5){
  const int p = X.cols();
  double sqp = sqrt(double(p));
  Eigen::VectorXd v0 = VectorXd::Constant(p, 1.0/sqp);
  Eigen::VectorXd v = VectorXd::Constant(p, 0.0);
  double stp = 1.0;
  int k = 0;
  while (stp > eps){
    v = X * v0;
    val = v.norm();
    v = v.array()/val;
    stp = (v0.array() - v.array()).matrix().norm();
    v0 = v;
    k++;
    if (k > 100){
      Rf_warning("Powermethod: not converged in 100 iterations. Error is", k);
      break;//here should use try-catch
    }
  }
  //  Rcout << "k = " << k << "; stp = " << stp << endl;
  return (v.array() * val);
}

// This is the main function for R
// S correl matrix
// pvexpfs is proportion of PC to explain by each block
// pvexp is proportion total variance of matrix to explain to terminate computing comps
// ncomps nistead of pvexp maximum number of comps (priority)
// full rank small eps to discard vars from selection
// simply projects current full rank PC onto set of variables in ind
// it does not compute the LS SPCA components, it seems to work as well as that
// uses power method to compute PCs
// new version reduces K and S in one function (+6% ) < check cost of resizing
// [[Rcpp::export]]
List fspcaCpmNoe(Eigen::MatrixXd S, double pvexpfs = 0.95, double pvexp = 0.95,
               int ncomps = 0, double fullrank = 0,  double eps = 10E-5){
  int p = S.cols();
  if (ncomps == 0)
    ncomps = p;
  Eigen::MatrixXd K(S);

  double totvexp = S.trace();// total variance S
  double maxvexp;// this is vexp by first PC for fow_select

  Eigen::VectorXd si(p);

  si = eigvecPMC(S, maxvexp);
//Rcout << "done si first " << endl;
  // here could compute D as   vec * diag(val^2) * vec.transpose

  Eigen::MatrixXd Sinv(p, p);
  Eigen::MatrixXd M(p, p);

  Eigen::VectorXd a(p);
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(p, ncomps);
  //  List load(p);
  List indout(p);

  Eigen::VectorXd vexp = Eigen::VectorXd::Zero(ncomps);
  Eigen::VectorXd cvexp = vexp;
  double cvt;
  Eigen::VectorXi indj(p);//this to pass to fwd_select

  Eigen::MatrixXd Sd(p, p);// maybe better leave dynamic? this takes S[onlyind, onlyind]

  int cardt = 0;
  Eigen::VectorXi card(p);
  int nc = 0;
  bool stopComp = false;

  int j = 0;
  while (stopComp == false){
    fwd_selectC(S, indj, cardt, si, maxvexp, pvexpfs, fullrank);
//Rcout << "comp "<< j + 1 << "done fwd_swlwct " << endl;

    card(j) = cardt;
    std::sort(indj.data(),indj.data() + cardt);

    // if ( j == 2)
    //   Rf_error("done 1");

    // need make one function that does bot Sd and M
    // create submatrices for computing loaidngs

    Sd.resize(cardt, cardt);
    M.resize(cardt, NoChange);
    makeSdAndM(S, indj.head(cardt), M, Sd, p,cardt);
//    Sd.topLeftCorner(cardt, cardt) = makeSubS(S, indj.head(cardt));
//    M.topLeftCorner(cardt, p) = selectRowsC(K, indj.head(cardt), p);
//Rcout << "comp "<< j + 1 << "done selectRows" << endl;

    //  compute loadings
  //  Sinv.topLeftCorner(cardt, cardt)  = Sd.topLeftCorner(cardt, cardt).llt().solve(MatrixXd::Identity(cardt, cardt));
    Sinv.topLeftCorner(cardt, cardt)  = Sd.llt().solve(MatrixXd::Identity(cardt, cardt));
    //Rcout << "comp "<< j + 1 << "done Sinv" << endl;

    // save loadings
    a.head(cardt) = ((Sinv.topLeftCorner(cardt, cardt) * M * si).array() / maxvexp).matrix();
//Rcout << "comp "<< j + 1 << "done loadings" << endl;
    // save loadings in column j
    for (int i = 0; i < cardt; i++){
      A(indj(i), j) = a(i);
    }
    // save loadings in list
    indout[j] = indj.head(cardt).array() + 1;

    nc = nc + 1;

    // this new func deflates S and D using only last vector of loads
    // returns deflated matr by references and vexp (not cum vexp)
    deflSC(a.head(cardt), K, indj.head(cardt), cvt);
//Rcout << "comp "<< j + 1 << "done deflSC" << endl;

    vexp(j) = cvt;
    if (j > 0)
      cvexp(j) = cvt + cvexp(j-1);
    else
      cvexp(j) = cvt;

    // checks if stopComp met
    if ((cvexp(j) > pvexp * totvexp) || ((j + 1) == ncomps)){
      stopComp = true;
      ncomps = nc;
    }
    else{
      // this power method, returns si and passes maxvexp byref
      si = eigvecPMC(K, maxvexp);
//Rcout << "comp "<< j + 1 << "done si" << endl;

      j = j + 1;
    }
  }//end compute comps

  IntegerVector idx = Rcpp::seq(0, nc - 1);

  return  List::create(Named("loadings") = A.topLeftCorner(p,nc), Named("ncomps") = nc,
                       Named("ind") = indout[idx], Named("card") = card.head(nc),
                       Named("vexp") = vexp.head(nc), Named("cvexp") = cvexp.head(nc));
}


#ifndef _OPT_PLACEMENT_HPP_
#define _OPT_PLACEMENT_HPP_

//#include <boost/graph/graph_traits.hpp>
//#include <boost/graph/adjacency_list.hpp>
//#include <boost/graph/graphviz.hpp>
//#include <boost/graph/iteration_macros.hpp>
//#include <boost/foreach.hpp>
//#include <Eigen/Core>
//#include <Eigen/LU>
//#include "mod2.hpp"
#include <modularity.hpp>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>

namespace opt_placement
{

  inline gsl_matrix* _laplacian(const Cppn* g, const std::set<Node*>& verts)
  {
	  gsl_matrix* l = gsl_matrix_calloc (g->getNrOfNodes(), g->getNrOfNodes());

	  size_t i = 0;
	  foreach(Node* node1, g->getNodes()){
		  	size_t j = 0;
		    foreach(Node* node2, g->getNodes()){
		    	if (verts.find(node1) == verts.end()|| verts.find(node2) == verts.end()){
		    		gsl_matrix_set(l, i,j,0.0);
		    	} else if (i == j){
		    		gsl_matrix_set(l, i,j,node1->incomingEdges().count() + node1->outgoingEdges().count());
		    	} else if (mod::_linked(node1, node2) || mod::_linked(node2, node1)){
		    		gsl_matrix_set(l, i,j,-1.0);
		    	} else {
		    		gsl_matrix_set(l, i,j,0.0);
		    	}
		    	++j;
		    }
		    ++i;
	  }

	  return l;
  }

   // include in the adjacency iff source _or_ target is in verts
  inline gsl_matrix* _adjacency(const Cppn* g, const std::set<Node*>& verts)
  {
	  gsl_matrix* a = gsl_matrix_calloc (g->getNrOfNodes(), g->getNrOfNodes());

	  size_t i = 0;
	  foreach(Node* node1, g->getNodes()){
		  	size_t j = 0;
		    foreach(Node* node2, g->getNodes()){
				  if ((verts.find(node1) != verts.end() || verts.find(node2) != verts.end()) && (mod::_linked(node1, node2) || mod::_linked(node2, node1)))
					  gsl_matrix_set(a, i,j,1.0);
				  else
					  gsl_matrix_set(a, i,j,0.0);

		    	++j;
		    }
		    ++i;
	  }

	  return a;
  }

   // include in the degree matrix iff vertex is in verts
  inline gsl_matrix* _degree(const Cppn* g, const std::set<Node*>& verts)
  {
	  gsl_matrix* d = gsl_matrix_calloc (g->getNrOfNodes(), g->getNrOfNodes());

	  size_t i = 0;
	  foreach(Node* node1, g->getNodes()){
		  if (verts.find(node1) != verts.end())
			  gsl_matrix_set(d, i,i,node1->incomingEdges().count() + node1->outgoingEdges().count());

		  ++i;
	  }
	  return d;
  }

  template<typename V, typename T>
  inline int _find_index(const V& vect, const T& e)
  {
    for (int i = 0; i < vect.size(); ++i)
      if (e == vect[i]){
//    	  std::cout << i << std::endl;
          return i;
      }

    return -1;
  }

  inline gsl_vector* _f(const Cppn* g,
		  const QList<Node*>& inputs,
		  const QVector<double>& coords_inputs,
		  const QList<Node*>& outputs,
		  const QVector<double>& coords_outputs)
  {
	  gsl_vector* f = gsl_vector_calloc(g->getNrOfNodes());

	  size_t i = 0;

	  foreach(Node* node1, g->getNodes()){
		  int in = _find_index(inputs, node1);
		  if (in != -1){
		  	  gsl_vector_set(f, i, coords_inputs[in]);
		  }
		  int out = _find_index(outputs, node1);
		  if (out != -1){
		  	  gsl_vector_set(f, i, coords_outputs[out]);
		  }
		  ++i;
	  }

	  return f;
  }

  inline QVector<double> compute(const Cppn* g,
                                 const QList<Node*>& inputs,
                                 const QVector<double>& coords_inputs,
                                 const QList<Node*>& outputs,
                                 const QVector<double>& coords_outputs)
  {
    typedef Node* v_d_t;
    std::set<v_d_t> all_set, io_set, no_io_set;

    foreach(Node* node1, g->getNodes()) all_set.insert(node1);
    foreach(Node* node, inputs) io_set.insert(node);
    foreach(Node* node, outputs) io_set.insert(node);

    std::set_difference(all_set.begin(), all_set.end(),
                        io_set.begin(), io_set.end(),
                        std::insert_iterator<std::set<v_d_t> >(no_io_set,
                                                               no_io_set.begin()));
    gsl_matrix* l = _laplacian(g, no_io_set);
    gsl_matrix* b = _adjacency(g, io_set);
    gsl_matrix* d = _degree(g, io_set);
    gsl_vector* f = _f(g, inputs, coords_inputs, outputs, coords_outputs);

	// Define all the used matrices
	gsl_matrix_add (l, d);

	//Calculate inverse of l+d
    int s;
	gsl_matrix* temp1 = gsl_matrix_alloc (l->size1, l->size2);
	gsl_matrix* temp2 = gsl_matrix_alloc (l->size1, l->size2);

	gsl_vector* result = gsl_vector_alloc(l->size1);
	QVector<double> qresult(l->size1);

	gsl_permutation * perm = gsl_permutation_alloc (f->size);
	gsl_linalg_LU_decomp (l, perm, &s);
	gsl_linalg_LU_invert (l, perm, temp1);

	//Multiplying the inverse of l+d with b
	gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, temp1, b, 0.0, temp2);

	//Multiplying the inverse of l+d multiplied with b with f
	gsl_blas_dgemv (CblasNoTrans, 1.0, temp2, f, 0.0, result);

	//Fix positions of input nodes
	int i=0;
	foreach(Node* node1, g->getNodes()){
		if (io_set.find(node1) != io_set.end()){
			qresult[i] = gsl_vector_get(f, i);
		} else {
			qresult[i] = gsl_vector_get(result, i);
		}
		i++;
	}

	gsl_matrix_free(temp1);
	gsl_matrix_free(temp2);
	gsl_vector_free(result);

    return qresult;
  }
}
#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "test_stats"

#include <boost/test/unit_test.hpp>
#include "nanocv/math/abs.hpp"
#include "nanocv/math/stats.hpp"
#include "nanocv/math/random.hpp"

namespace test
{
        using namespace ncv;

        void check_stats(double avg, double var, size_t count)
        {
                ncv::stats_t<double> stats;
                ncv::random_t<double> rgen(-var, +var);

                // generate random values
                std::vector<double> values;
                for (size_t i = 0; i < count; i ++)
                {
                        values.push_back(avg + rgen());

                        stats(*values.rbegin());
                }

                // check count
                BOOST_CHECK_EQUAL(stats.count(), count);

                // check range
                BOOST_CHECK_GE(stats.min(), avg - var);
                BOOST_CHECK_LE(stats.max(), avg + var);

                // check average
                BOOST_CHECK_GE(stats.avg(), avg - var);
                BOOST_CHECK_LE(stats.avg(), avg + var);

                // check variance
                BOOST_CHECK_GE(stats.var(), 0.0);
                BOOST_CHECK_LE(std::sqrt(stats.var()), var);

                // check sum
                BOOST_CHECK_LE(math::abs(stats.sum() - std::accumulate(values.begin(), values.end(), 0.0)), 1e-8);
        }
}

BOOST_AUTO_TEST_CASE(test_stats)
{
        test::check_stats(0.03, 0.005, 32);
        test::check_stats(1.03, 13.005, 37);
        test::check_stats(-0.54, 0.105, 13);
        test::check_stats(-7.03, 10.005, 11);
}


/*
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 * Unit Tests for the methods of the Pyramid class
 */

#define BOOST_TEST_MODULE Pyramid
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
#include <stdexcept>

#include "Pyramid.h"
#include "EuclideanPoint.h"

using namespace cupcfd::geometry::shapes;
namespace euc = cupcfd::geometry::euclidean;
namespace utf = boost::unit_test;

// === Constructor ===
// Test 1: Create a QuadPyramid
BOOST_AUTO_TEST_CASE(constructor_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double, 3> p1(0.0, 0.0, 0.0);
	euc::EuclideanPoint<double, 3> p2(0.0, 10.0, 0.0);
	euc::EuclideanPoint<double, 3> p3(10.0, 10.0, 0.0);
	euc::EuclideanPoint<double, 3> p4(10.0, 0.0, 0.0);

	euc::EuclideanPoint<double, 3> apex(5.0, 5.0, 5.0);

	Quadrilateral3D<double> base(p1, p2, p3, p4);
	Pyramid<Quadrilateral3D<double>, double> shape(apex, base);

	BOOST_CHECK_EQUAL(shape.apex.cmp[0], 5.0);
	BOOST_CHECK_EQUAL(shape.apex.cmp[1], 5.0);
	BOOST_CHECK_EQUAL(shape.apex.cmp[2], 5.0);

	BOOST_CHECK_EQUAL(shape.base.vertices[0].cmp[0], 0.0);
	BOOST_CHECK_EQUAL(shape.base.vertices[0].cmp[1], 0.0);
	BOOST_CHECK_EQUAL(shape.base.vertices[0].cmp[2], 0.0);

	BOOST_CHECK_EQUAL(shape.base.vertices[1].cmp[0], 0.0);
	BOOST_CHECK_EQUAL(shape.base.vertices[1].cmp[1], 10.0);
	BOOST_CHECK_EQUAL(shape.base.vertices[1].cmp[2], 0.0);

	BOOST_CHECK_EQUAL(shape.base.vertices[2].cmp[0], 10.0);
	BOOST_CHECK_EQUAL(shape.base.vertices[2].cmp[1], 10.0);
	BOOST_CHECK_EQUAL(shape.base.vertices[2].cmp[2], 0.0);

	BOOST_CHECK_EQUAL(shape.base.vertices[3].cmp[0], 10.0);
	BOOST_CHECK_EQUAL(shape.base.vertices[3].cmp[1], 0.0);
	BOOST_CHECK_EQUAL(shape.base.vertices[3].cmp[2], 0.0);
}

// === isPointInside ===
// Test 1:
BOOST_AUTO_TEST_CASE(isPointInside_test1, * utf::tolerance(0.00001))
{

}

// === getVolume ===
// Test 1: Compute Volume of Pyramid
BOOST_AUTO_TEST_CASE(getVolume_test1, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double, 3> p1(0.0, 0.0, 0.0);
	euc::EuclideanPoint<double, 3> p2(0.0, 10.0, 0.0);
	euc::EuclideanPoint<double, 3> p3(10.0, 10.0, 0.0);
	euc::EuclideanPoint<double, 3> p4(10.0, 0.0, 0.0);
	euc::EuclideanPoint<double, 3> apex(5.0, 5.0, 5.0);

	Quadrilateral3D<double> base(p1, p2, p3, p4);
	Pyramid<Quadrilateral3D<double>, double> shape(apex, base);

	double volume = shape.getVolume();

	BOOST_TEST(volume == 166.666667);
}

// Test 2: Compute Volume of Oblique Pyramid
BOOST_AUTO_TEST_CASE(getVolume_test2, * utf::tolerance(0.00001))
{
	euc::EuclideanPoint<double, 3> p1(0.0, 0.0, 0.0);
	euc::EuclideanPoint<double, 3> p2(0.0, 10.0, 0.0);
	euc::EuclideanPoint<double, 3> p3(10.0, 10.0, 0.0);
	euc::EuclideanPoint<double, 3> p4(10.0, 0.0, 0.0);
	euc::EuclideanPoint<double, 3> apex(2.7, 3.7, 8.9);

	Quadrilateral3D<double> base(p1, p2, p3, p4);
	Pyramid<Quadrilateral3D<double>, double> shape(apex, base);

	double volume = shape.getVolume();

	BOOST_TEST(volume == 296.666667);
}


#include "fib_matrix.h"
#include <boost/multiprecision/cpp_int.hpp>
#include <fstream>
#include <sstream>
#include <iostream>

using namespace boost::multiprecision;
using uint2048_t = number<cpp_int_backend<2048, 2048, unsigned_magnitude, unchecked, void>>;

int main(int argc, char *argv[])
{
    const char* fname = (argc > 1)? argv[1] : "fib.txt";
    std::ifstream f(fname);
    if (f.good()) {
        std::cout << "reading from " << fname << "\n";
        uint2048_t n;

        std::string expected;
        while (f >> n >> expected) {
            auto r = fib(n);
            std::ostringstream os;
            os << r;
            if (os.str() != expected) {
                std::cout << n
                          << "\n got unexpected result " << r
                          << "\n expected              " << expected << "\n";
            }
        }
    } else {
        std::cout << "unable to open " << fname << "\n";
    }
    return 0;
}


/**
 * fit test
 * @author Tobias Weber <tweber@ill.fr>
 * @date 28-aug-20
 * @license GPLv3, see 'LICENSE' file
 *
 * g++-10 -std=c++20 -I.. -o fit1 fit1.cpp ../libs/log.cpp -lMinuit2 -lpthread
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Fit Test
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include "libs/fit.h"
#include "libs/maths.h"

using t_types_real = std::tuple<double, float>;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_expr_real, t_real, t_types_real)
{
	t_real amp = 1.;
	t_real freq = 2*tl2::pi<t_real>;
	t_real offs = 12.;

	std::vector<t_real> xs, ys, yerrs;
	for(t_real x=0.; x<1.; x+=0.05)
	{
		xs.push_back(x);
		ys.push_back(amp*std::sin(freq*x) + offs);
		yerrs.push_back(0.1);
	}

	auto func = [](t_real x, t_real amp, t_real freq, t_real offs)
	{
		return amp*std::sin(freq*x) + offs;
	};

	std::vector<std::string> params{{"amp", "freq", "offs"}};
	std::vector<t_real> vals{{amp*t_real{1.2}, freq*t_real{0.8}, offs*t_real{0.9}}};
	std::vector<t_real> errs{{0.5, 0.1, 1.}};
	std::vector<bool> fixed{false, false, false};

	bool ok = tl2::fit<t_real, 4>(func, xs, ys, yerrs, params, vals, errs, &fixed);

	BOOST_TEST(ok);
	BOOST_TEST(tl2::equals<t_real>(vals[0], amp, 1e-3));
	BOOST_TEST(tl2::equals<t_real>(vals[1], freq, 1e-3));
	BOOST_TEST(tl2::equals<t_real>(vals[2], offs, 1e-3));
}


#include "easing/easing.hpp"
#include "interpolation/interpolation.hpp"
#include <boost/process.hpp>
#include <iostream>
#include <string>
#define FMT_HEADER_ONLY
#include <fmt/format.h>

static void plot(const std::string &plot_name,
                 const std::string &points_data_path,
                 const std::string &output_png_path, bool is_dynamic) {
  boost::process::opstream write;
  boost::filesystem::path command("/usr/local/bin/gnuplot");
  boost::process::child plot(command, boost::process::std_in = write);

  write << "set terminal png\n";
  write << fmt::format("unset key\n");
  write << fmt::format("set output \"{}\"\n", output_png_path);
  write << fmt::format("set xlabel \"{}\"\n", "axis-x");
  write << fmt::format("set ylabel \"{}\"\n", "axis-y");
  write << fmt::format("set xrange {}\n", "[0:1]");
  write << fmt::format("set yrange {}\n", is_dynamic ? "[-0.5:1.5]" : "[0:1]");
  write << fmt::format("plot \"{}\" with points pt 7 lc 1,", points_data_path);

  write.flush();
  write.pipe().close();
  plot.wait();
  std::cout << plot_name << " was done, exit code: " << plot.exit_code()
            << std::endl;
}

template <typename F>
static void
plot(const std::string &plot_name, const std::string &points_data_path,
     const std::string &output_png_path, F easing, bool is_dynamic = false) {
  std::vector<float> xs(101), ys(101);
  for (int i = 0; i <= 100; i++) {
    xs[i] = i * 0.01f;
    ys[i] = interpolation::lerp(0.0f, 1.0f, easing(xs[i]));
  }
  std::ofstream points_file(points_data_path);
  for (int i = 0; i <= 100; i++) {
    points_file << xs[i] << " " << ys[i] << "\n";
  }
  points_file.close();

  plot(plot_name, points_data_path, output_png_path, is_dynamic);
}

template <typename T>
static void plot(const std::string &type_name, bool is_dynamic = false) {
  plot(fmt::format("{} in plot", type_name),
       fmt::format("data/{}_in.dat", type_name),
       fmt::format("data/{}_in.png", type_name), T::in, is_dynamic);
  plot(fmt::format("{} out plot", type_name),
       fmt::format("data/{}_out.dat", type_name),
       fmt::format("data/{}_out.png", type_name), T::out, is_dynamic);
  plot(fmt::format("{} in-out plot", type_name),
       fmt::format("data/{}_inout.dat", type_name),
       fmt::format("data/{}_inout.png", type_name), T::inout, is_dynamic);
}

int main() {
  plot("linear plot", "data/linear.dat", "data/linear.png",
       easing::linear<float>);
  plot("ease in plot", "data/ease_in.dat", "data/ease_in.png",
       easing::ease_in<float>);
  plot("ease out plot", "data/ease_out.dat", "data/ease_out.png",
       easing::ease_out<float>);
  plot("ease inout plot", "data/ease_inout.dat", "data/ease_inout.png",
       easing::ease_inout<float>);

  plot<easing::ease<easing::sine<float>, float>>("sine");
  plot<easing::ease<easing::quad<float>, float>>("quad");
  plot<easing::ease<easing::cubic<float>, float>>("cubic");
  plot<easing::ease<easing::quart<float>, float>>("quart");
  plot<easing::ease<easing::quint<float>, float>>("quint");
  plot<easing::ease<easing::expo<float>, float>>("expo");
  plot<easing::ease<easing::circ<float>, float>>("circ");
  plot<easing::ease<easing::back<float>, float>>("back", true);
  plot<easing::ease<easing::elastic<float>, float>>("elastic", true);
  plot<easing::ease<easing::bounce<float>, float>>("bounce");
  return 0;
}


// hpr-itl cg.cpp: HPR Conjugate Gradient algorithms
//
// Copyright (C) 2017-2018 Stillwater Supercomputing, Inc.
//
// This file is part of the universal numbers project, which is released under an MIT Open Source license.

#include "common.hpp"

// enable posit arithmetic exceptions
#define POSIT_THROW_ARITHMETIC_EXCEPTION 1
#include <posit>

// MTL
#include <boost/numeric/itl/itl.hpp>
// defines all Krylov solvers
// CG, CGS, BiCG, BiCGStab, BiCGStab2, BiCGStab_ell, FSM, IDRs, GMRES TFQMR, QMR, PC

namespace hpr {
	template<typename Vector, size_t nbits, size_t es, size_t capacity = 10>
	sw::unum::posit<nbits, es> fused_dot(const Vector& x, const Vector& y) {
		sw::unum::quire<nbits, es, capacity> q = 0;
		size_t ix, iy, n = size(x);
		for (ix = 0, iy = 0; ix < n && iy < n; ix = ix + 1, iy = iy + 1) {
			q += sw::unum::quire_mul(x[ix], y[iy]);
		}
		sw::unum::posit<nbits, es> sum;
		convert(q.to_value(), sum);     // one and only rounding step of the fused-dot product
		return sum;
	}

	/// Conjugate Gradients without preconditioning
	template < typename LinearOperator, typename HilbertSpaceX, typename HilbertSpaceB,
		typename Iteration >
	int cg(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
			Iteration& iter)
	{
		mtl::vampir_trace<7001> tracer;
		using std::abs; using mtl::conj; using mtl::lazy;
		typedef HilbertSpaceX Vector;
		typedef typename mtl::Collection<HilbertSpaceX>::value_type Scalar;
		typedef typename Iteration::real                            Real;

		constexpr size_t nbits = Scalar::nbits;
		constexpr size_t es = Scalar::es;

		Scalar rho(0), rho_1(0), alpha(0), alpha_1(0);
		Vector p(resource(x)), q(resource(x)), r(resource(x)), z(resource(x));

		r = b - A*x;
		rho = fused_dot<Vector, nbits, es>(r, r);
		while (!iter.finished(Real(sqrt(abs(rho))))) {
			++iter;
			if (iter.first())
				p = r;
			else
				p = r + (rho / rho_1) * p;

			q = A * p; alpha = rho / fused_dot<Vector, nbits, es>(p, q);
//			(lazy(q) = A * p) || (lazy(alpha_1) = lazy_dot(p, q));
//			alpha = rho / alpha_1;

			x += alpha * p;
			rho_1 = rho;
//			(lazy(r) -= alpha * q) || (lazy(rho) = lazy_unary_dot(r));
			r -= alpha * q; rho = fused_dot<Vector, nbits, es>(r, r);

		}

		return iter;
	}
}

template<typename Scalar>
int regular_CG()
{
	using Matrix = mtl::mat::compressed2D< Scalar >;
	using Vector = mtl::vec::dense_vector< Scalar >;

	// Create a 1,600 x 1,600 matrix using a 5-point Laplacian stencil
	const size_t size = 40, N = size * size;
	Matrix A(N, N);
	mtl::mat::laplacian_setup(A, size, size);

	// Set b such that x == 1 is solution; start with x == 0
	mtl::vec::dense_vector<Scalar>       x(N, 1.0), b(N);
	b = A * x; x = 0;

	// Termination criterion: r < 1e-6 * b or N iterations
	//noisy_iteration< Scalar >  iter(b, 500, 1.e-6);
	itl::cyclic_iteration< Scalar >  iter(b, 500, 1.e-6);

	// Solve Ax == b without a preconditioner P
	itl::cg(A, x, b, iter);

	int nrOfIterations = -1;
	if (iter.is_converged()) nrOfIterations = iter.iterations();
	return nrOfIterations;
}

template<typename Scalar>
int fdp_CG()
{
	using Matrix = mtl::mat::compressed2D< Scalar >;
	using Vector = mtl::vec::dense_vector< Scalar >;

	// Create a 1,600 x 1,600 matrix using a 5-point Laplacian stencil
	const size_t size = 40, N = size * size;
	Matrix A(N, N);
	mtl::mat::laplacian_setup(A, size, size);

	// Set b such that x == 1 is solution; start with x == 0
	mtl::vec::dense_vector<Scalar>       x(N, 1.0), b(N);
	b = A * x; x = 0;

	// Termination criterion: r < 1e-6 * b or N iterations
	//itl::noisy_iteration< Scalar >  iter(b, 500, 1.e-6);
	itl::cyclic_iteration< Scalar >  iter(b, 500, 1.e-6);

	// Solve Ax == b without a preconditioner P
	hpr::cg(A, x, b, iter);

	int nrOfIterations = -1;
	if (iter.is_converged()) nrOfIterations = iter.iterations();
	return nrOfIterations;
}

int main(int argc, char** argv)
try {
	using namespace std;
	using namespace sw::unum;
	using namespace mtl;
	using namespace mtl::mat;
	using namespace itl;

	bool bSuccess = true;

	fdp_CG< posit<32, 2> >();

	return 0;

#if 0
	cout << "CG<double> #iterations: " << regular_CG<float>() << endl;
	cout << "CG<posit<32,3> #iterations: " << fdp_CG< posit<32, 3> >() << endl;
	cout << "CG<posit<32,2> #iterations: " << fdp_CG< posit<32, 2> >() << endl;
	cout << "CG<posit<32,1> #iterations: " << fdp_CG< posit<32, 1> >() << endl;
//	cout << "CG<posit<32,0> #iterations: " << fdp_CG< posit<32, 0> >() << endl;
	cout << "CG<posit<28,3> #iterations: " << fdp_CG< posit<28, 3> >() << endl;
	cout << "CG<posit<28,2> #iterations: " << fdp_CG< posit<28, 2> >() << endl;
	cout << "CG<posit<28,1> #iterations: " << fdp_CG< posit<28, 1> >() << endl;
//	cout << "CG<posit<28,0> #iterations: " << fdp_CG< posit<28, 0> >() << endl;
	cout << "CG<posit<24,3> #iterations: " << fdp_CG< posit<24, 3> >() << endl;
	cout << "CG<posit<24,2> #iterations: " << fdp_CG< posit<24, 2> >() << endl;
	cout << "CG<posit<24,1> #iterations: " << fdp_CG< posit<24, 1> >() << endl;
//	cout << "CG<posit<24,0> #iterations: " << fdp_CG< posit<24, 0> >() << endl;
	cout << "CG<posit<20,3> #iterations: " << fdp_CG< posit<20, 3> >() << endl;
	cout << "CG<posit<20,2> #iterations: " << fdp_CG< posit<20, 2> >() << endl;
	cout << "CG<posit<20,1> #iterations: " << fdp_CG< posit<20, 1> >() << endl;
//	cout << "CG<posit<20,0> #iterations: " << fdp_CG< posit<20, 0> >() << endl;
	cout << "CG<posit<16,3> #iterations: " << fdp_CG< posit<16, 3> >() << endl;
	cout << "CG<posit<16,2> #iterations: " << fdp_CG< posit<16, 2> >() << endl;
	cout << "CG<posit<16,1> #iterations: " << fdp_CG< posit<16, 1> >() << endl;
#endif

	return (bSuccess ? EXIT_FAILURE : EXIT_SUCCESS);
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_SUCCESS; //as we manually throwing the not supported yet it should not fall through the cracks     EXIT_FAILURE;
}
catch (const posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const std::runtime_error& err) {
	std::cerr << "Uncaught runtime exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


//  (C) Copyright Nick Thompson, 2019
//  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)

#define BOOST_TEST_MODULE condition_number_test

#include <cmath>
#include <limits>
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/lambert_w.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/tools/condition_numbers.hpp>

using std::abs;
using boost::math::constants::half;
using boost::math::constants::ln_two;
using boost::multiprecision::cpp_bin_float_50;
using boost::math::tools::summation_condition_number;
using boost::math::tools::evaluation_condition_number;

template<class Real>
void test_summation_condition_number()
{
    Real tol = 1000*std::numeric_limits<float>::epsilon();
    auto cond = summation_condition_number<Real>();
    // I've checked that the condition number increases with max_n,
    // and that the computed sum gets more accurate with increasing max_n.
    // But the CI system would die with more terms.
    Real max_n = 10000;
    for (Real n = 1; n < max_n; n += 2)
    {
        cond += 1/n;
        cond -= 1/(n+1);
    }

    BOOST_CHECK_CLOSE_FRACTION(cond.sum(), ln_two<Real>(), tol);
    BOOST_TEST(cond() > 14);
}

template<class Real>
void test_exponential_sum()
{
    using std::exp;
    using std::abs;
    Real eps = std::numeric_limits<float>::epsilon();
    for (Real x = -20; x <= -1; x += 0.5)
    {
        auto cond = summation_condition_number<Real>(1);
        size_t n = 1;
        Real term = x;
        while(n++ < 1000)
        {
            cond += term;
            term *= (x/n);
        }
        BOOST_CHECK_CLOSE_FRACTION(exp(x), cond.sum(), eps*cond());
        BOOST_CHECK_CLOSE_FRACTION(exp(2*abs(x)), cond(), eps*cond());
    }
}

template<class Real>
void test_evaluation_condition_number()
{
    using std::abs;
    using std::log;
    using std::sqrt;
    using std::exp;
    using std::sin;
    using std::tan;
    Real tol = sqrt(std::numeric_limits<Real>::epsilon());

    auto f1 = [](auto x) { return log(x); };
    for (Real x = 1.125; x < 8; x += 0.125)
    {
        Real cond = evaluation_condition_number(f1, x);
        BOOST_CHECK_CLOSE_FRACTION(cond, 1/log(x), tol);
    }

    auto f2 = [](auto x) { return exp(x); };
    for (Real x = 1.125; x < 8; x += 0.125)
    {
        Real cond = evaluation_condition_number(f2, x);
        BOOST_CHECK_CLOSE_FRACTION(cond, x, tol);
    }

    auto f3 = [](auto x) { return sin(x); };
    for (Real x = 1.125; x < 8; x += 0.125)
    {
        Real cond = evaluation_condition_number(f3, x);
        BOOST_CHECK_CLOSE_FRACTION(cond, abs(x/tan(x)), tol);
    }

    // Test a function which right differentiable:
    using boost::math::constants::e;
    auto f4 = [](Real x) { return boost::math::lambert_w0(x); };
    Real cond = evaluation_condition_number(f4, -1/e<Real>());
    if (std::is_same_v<Real, float>)
    {
        BOOST_CHECK_GE(cond, 30);
    }
    else
    {
        BOOST_CHECK_GE(cond, 4900);
    }
}

BOOST_AUTO_TEST_CASE(numerical_differentiation_test)
{
    test_summation_condition_number<float>();
    test_summation_condition_number<cpp_bin_float_50>();
    test_evaluation_condition_number<float>();
    test_evaluation_condition_number<double>();
    test_evaluation_condition_number<long double>();
    test_evaluation_condition_number<cpp_bin_float_50>();
    test_exponential_sum<double>();
}


#include "ilp/eisenbrand_weismantel.hpp"
#include "ilp/detail/debug.hpp"
#include "ilp/detail/bellman_ford.hpp"

#include <iostream>
#include <stack>
#include <algorithm>

#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/named_function_params.hpp>

// original boost header:
//#include <boost/graph/bellman_ford_shortest_paths.hpp>

// modified boost header:
//#include "ilp/detail/boost/bellman_ford_shortest_paths.hpp"

namespace ilp
{
    EWDigraph::EWDigraph(const ilp::ilp_task& ilpTask) : DigraphAdaptor{ilpTask}
    {
        detail::debug_log("building EW digraph ...");
    }

    bool EWDigraph::populate_condition(const cvector<int>& b,
                                       const cvector<int>& p,
                                       int bound) const
    {
        double dot_product_lhs = b.dot(p);
        double dot_product_rhs = b.dot(b);
        double coeff = dot_product_lhs / dot_product_rhs;
        if (coeff > 1.0)
        {
            coeff = 1.0;
        }

        for (int i = 0; i < ilpTask.A.rows(); ++i)
        {
            auto b_val = static_cast<double>(b(i, 0));
            double b_val_cut = b_val * coeff;
            double p_val = p(i, 0);
            if (std::abs(p_val - b_val_cut) > bound)
            {
                return false;
            }
        }

        return true;
    }

    void EWDigraph::populate_from(VertexDescriptor vertex,
                                  int bound)
    {
        std::stack<VertexDescriptor> populated;
        populated.push(vertex);

        while (!populated.empty())
        {
            auto current_vertex = populated.top();
            auto current_point = this->m_base[current_vertex].point;
            populated.pop();

            // iterate over A's columns
            for (index_t i = 0; i != ilpTask.A.cols(); i++)
            {
                const cvector<int>& column = ilpTask.A.col(i);
                current_point += column;

                if (populate_condition(ilpTask.b, current_point, bound))
                {
                    // try to insert and get descriptor of a new or an existing vertex
                    auto[new_vertex, is_new] = this->add_vertex(current_point);

                    if (new_vertex == current_vertex)
                    {
                        continue;
                    }

                    // insert the new edge to the graph
                    this->add_edge(current_vertex,
                                   new_vertex,
                                   EdgeProperty{-1 * ilpTask.c(i), static_cast<int>(i)}
                    );

                    // save the vertex descriptor for later recursive call of the populate function
                    if (is_new)
                    {
                        populated.push(new_vertex);
                    }
                }

                current_point -= column;
            }
        }
    }

    void EWDigraph::populate_graph()
    {
        const index_t m = ilpTask.A.rows();
        const index_t n = ilpTask.A.cols();
        const int delta = ilpTask.A.lpNorm<Eigen::Infinity>();
        const int bound = 2 * static_cast<int>(m) * delta;

        this->start = this->add_vertex(cvector<int>::Zero(m, 1)).first;
        this->m_base[start].distance = 0;

        populate_from(start, bound);

        auto b_it = m_points_set.find(ilpTask.b);
        this->bIsFeasible = (b_it != m_points_set.end());
        if (bIsFeasible)
        {
            this->end = b_it->second;
        }
    }

    ilp_solution eisenbrand_weismantel(const ilp_task& ilpTask)
    {
        const index_t m = ilpTask.A.rows();
        const index_t n = ilpTask.A.cols();

        ilp_solution result;
        result.x = cvector<int>::Zero(n, 1);

        ilp::EWDigraph graph{ilpTask};

        {
            graph.populate_graph();
        }

        result.is_feasible = graph.is_feasible();
        if (result.is_feasible)
        {
            ilp::detail::debug_log("feasible");

/*            {
                result.is_bounded = bellman_ford_shortest_paths(
                        graph.m_base,
                        num_vertices(graph.m_base),
                        predecessor_map(get(&VertexProperty::predecessor, graph.m_base))
                                .distance_map(get(&VertexProperty::distance, graph.m_base))
                                .weight_map(get(&EdgeProperty::weight, graph.m_base))
                );
            }*/

            {
                result.is_bounded = ilp::detail::bellman_ford(graph.start,
                                                              graph.m_base);
            }

            if (result.is_bounded)
            {
                VertexDescriptor vd = graph.end;
                result.c_result = -1 * graph.m_base[vd].distance;

                while (vd != graph.start)
                {
                    const auto& vertex = graph.m_base[vd];
                    EdgeDescriptor ed = boost::edge(vertex.predecessor, vd, graph.m_base).first;
                    const auto& edge = graph.m_base[ed];
                    result.x(edge.column, 0) += 1;

                    vd = vertex.predecessor;
                }

            }
        }

        return result;
    }

} // namespace ilp


//  (C) Copyright Matt Borland 2021.
//  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)

#include <cmath>
#include <cfloat>
#include <cstdint>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/fpclassify.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

template <typename T>
void test()
{
    if constexpr (std::numeric_limits<T>::has_quiet_NaN)
    {
        static_assert(boost::math::ccmath::fpclassify(std::numeric_limits<T>::quiet_NaN()) == FP_NAN);
    }

    static_assert(boost::math::ccmath::fpclassify(T(0)) == FP_ZERO);
    static_assert(boost::math::ccmath::fpclassify(std::numeric_limits<T>::infinity()) == FP_INFINITE);
    static_assert(boost::math::ccmath::fpclassify((std::numeric_limits<T>::min)() / T(2)) == FP_SUBNORMAL);
    static_assert(boost::math::ccmath::fpclassify(T(1)) == FP_NORMAL);
}

#ifndef BOOST_MATH_NO_CONSTEXPR_DETECTION
int main()
{
    test<float>();
    test<double>();

    #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test<long double>();
    #endif

    #if defined(BOOST_HAS_FLOAT128) && !defined(BOOST_MATH_USING_BUILTIN_CONSTANT_P)
    test<boost::multiprecision::float128>();
    #endif

    return 0;
}
#else
int main()
{
    return 0;
}
#endif


// 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, EVecPRESS 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 <gtest/gtest.h>

#include <Eigen/Core>
#include <smooth/compat/autodiff.hpp>

#include "smooth/feedback/ocp_flatten.hpp"

constexpr auto DT = smooth::diff::Type::Autodiff;
// constexpr auto DT = smooth::diff::Type::Numerical;

#include "ocp.hpp"

TEST(OcpFlatten, Basic)
{
  std::srand(10);

  const auto t1 = smooth::feedback::test_ocp_derivatives<DT>(ocp_test, 5);
  ASSERT_TRUE(t1);

  const auto xl = []<typename T>(const T & t) -> smooth::CastT<T, OcpTest::X> {
    const Eigen::Vector<T, Nx> vel{1, 2, 3};
    return smooth::exp<smooth::CastT<T, OcpTest::X>>(t * vel);
  };
  const auto ul = []<typename T>(const T & t) -> smooth::CastT<T, OcpTest::U> {
    const Eigen::Vector<T, Nu> vel{1, 2};
    return smooth::exp<smooth::CastT<T, OcpTest::U>>(t * vel);
  };

  // test twice to catch allocation/compression issues (first call allocates)
  auto ocp_flat  = smooth::feedback::flatten_ocp(ocp_test, xl, ul);
  const auto t2a = smooth::feedback::test_ocp_derivatives<DT>(ocp_flat, 5);
  ASSERT_TRUE(t2a);

  const auto t2b = smooth::feedback::test_ocp_derivatives<DT>(ocp_flat, 5);
  ASSERT_TRUE(t2b);
}