#ifndef TRAJECTORYGENERATOR_HPP
#define TRAJECTORYGENERATOR_HPP

#include <iostream>
#include <vector>
#include <map>
#include <cmath>
#include <random>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>

#include "polynomials.hpp"
#include "tools.hpp"
#include "vehicle.hpp"

using namespace Eigen;
using namespace std;

class Trajectory {

public:

    Trajectory() : _generated(false) {}

    Trajectory(pair<Polynomial, Polynomial> &traj_coeffs,
               int samples)
    {
        generate(traj_coeffs, samples);
    }

    ~Trajectory() {}

    void clear() {
        _traj.clear();
    }

    int size(void) const {
        return _traj.size();
    }

    void removeFirstPoints(int numPoints);
    void removeLastPoints(int numPoints);
    vector<double> operator[](int T);
    vector<double> getState_at(int T);
    vector<vector<double>> getSD();
    void generate(pair<Polynomial, Polynomial> &traj_coeffs,
                  int samples);

private:
    vector<Vehicle> _traj;
    bool _generated = false;
};

class TrajectoryGenerator {

public:
    TrajectoryGenerator() {}
    ~TrajectoryGenerator() {}

    Trajectory get_last_trajectory() const;
    Trajectory generate_trajectory(const vector<double> &start, const double max_speed,
                                   const double horizon, const double update_interval, const int lag,
                                   const vector<Vehicle> &vehicles);

    static int convert_d_to_lane(const double d, const double laneWidth = 4.0);
    static double convert_lane_to_d(const int lane_number, const double laneWidth = 4.0);
    static double logistic(const double x);
    string get_current_action() const;

private:
    Polynomial jmt(const vector<double> &start, const vector<double> &end, const int T);
    void perturb_goal(vector<double> goal, vector<vector<double>> &goal_points, bool no_ahead=false);

    int closest_vehicle_in_lane(const vector<double> &start, const int ego_lane_i,
                                const vector<Vehicle> &vehicles);
    vector<int> closest_vehicle_in_lanes(const vector<double> &start, const vector<Vehicle> &vehicles);

    // cost functions:
    double calculate_cost(const pair<Polynomial, Polynomial> &traj, const vector<double> &goal,
                          const vector<Vehicle> &vehicles, vector<vector<double>> &all_costs);
    double exceeds_speed_limit_cost(const pair<Polynomial, Polynomial> &traj,
                                    const vector<double> &goal, const vector<Vehicle> &vehicles);
    double exceeds_accel_cost(const pair<Polynomial, Polynomial> &traj,
                              const vector<double> &goal, const vector<Vehicle> &vehicles);
    double exceeds_jerk_cost(const pair<Polynomial, Polynomial> &traj,
                             const vector<double> &goal, const vector<Vehicle> &vehicles);
    double collision_cost(const pair<Polynomial, Polynomial> &traj,
                          const vector<double> &goal, const vector<Vehicle> &vehicles);
    double traffic_buffer_cost(const pair<Polynomial, Polynomial> &traj,
                               const vector<double> &goal, const vector<Vehicle> &vehicles);
    double efficiency_cost(const pair<Polynomial, Polynomial> &traj,
                           const vector<double> &goal, const vector<Vehicle> &vehicles);
    double total_accel_d_cost(const pair<Polynomial, Polynomial> &traj,
                              const vector<double> &goal, const vector<Vehicle> &vehicles);
    double total_accel_s_cost(const pair<Polynomial, Polynomial> &traj,
                              const vector<double> &goal, const vector<Vehicle> &vehicles);
    double total_jerk_cost(const pair<Polynomial, Polynomial> &traj,
                           const vector<double> &goal, const vector<Vehicle> &vehicles);
    double lane_depart_cost(const pair<Polynomial, Polynomial> &traj,
                            const vector<double> &goal, const vector<Vehicle> &vehicles);
    double traffic_ahead_cost(const pair<Polynomial, Polynomial> &traj,
                              const vector<double> &goal, const vector<Vehicle> &vehicles);

    Trajectory _trajectory;
    int _horizon                    = 0;
    std::string _current_action     = "straight";
    const double _dt                = 0.02;
    const double _car_width         = 2.0;
    const double _car_length        = 5.0;
    const double _car_col_width     = 0.5 * _car_width;
    const double _car_col_length    = 0.5 * _car_length;
    const double _col_buf_width     = _car_width;
    const double _col_buf_length    = 5 * _car_length;
    const int _goal_perturb_samples = 10;
    const double _hard_max_vel_per_timestep     = mph2mps(48.) * _dt; // 50 miles + a little buffer
    const double _hard_max_acc_per_timestep     = 8.0 * _dt; // m/s
    const double _hard_max_jerk_per_timestep    = 7.0 * _dt; // m/s^2
    double _max_dist_per_timestep               = 0.0;
    double _delta_s_maxspeed                    = 0.0;

    std::default_random_engine _rand_generator;
    std::map<std::string, double> _cost_weights = {
        {"tr_buf_cost",     200.0},
        {"eff_cost",        100.0},
        {"acc_s_cost",      20.0},
        {"acc_d_cost",      20.0},
        {"jerk_cost",       20.0},
        {"lane_dep_cost",   0.5},
        {"traffic_cost",    100.0}
    };
};

#endif // TRAJECTORYGENERATOR_HPP


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


#ifdef BUILD_WITH_MOSEK

#include <Eigen/SparseCore>
#include <catch2/catch.hpp>

#include <igl/mosek/mosek_quadprog.h>

#define INF_D (std::numeric_limits<double>::infinity())

TEST_CASE("Simple tests of MOSEK", "[opt][mosek]")
{
    // Load problem data
    const int N = 4;
    const int M = N;

    // Quadratic matrix
    Eigen::SparseMatrix<double> Q(N, N);
    Q.setIdentity();

    // Quadratic linear term
    Eigen::VectorXd c(N);
    c.setZero();

    // Quadratic constant term
    double cf = 0.0;

    // Linear constraint matrix
    Eigen::SparseMatrix<double> A(M, N);
    A.setIdentity();

    // Linear constraint lower bounds
    Eigen::VectorXd lc(M);
    lc.setOnes();
    Eigen::VectorXd expected_solution(M);
    expected_solution.setOnes();
    SECTION("No Lower Bounds")
    {
        lc *= -INF_D;
        expected_solution *= 0;
    }
    SECTION("Lower Bounds = 1") {}
    SECTION("Lower Bounds = 10")
    {
        lc *= 10;
        expected_solution *= 10;
    }
    // Linear constraint upper bounds
    Eigen::VectorXd uc(M);
    uc.setOnes();
    uc *= INF_D;
    Eigen::VectorXd lx(M);
    lx.setOnes();
    lx *= -INF_D;
    Eigen::VectorXd ux(M);
    ux.setOnes();
    ux *= INF_D;

    igl::mosek::MosekData mosek_data;

    Eigen::VectorXd x(N);
    bool success = igl::mosek::mosek_quadprog(
        Q, c, cf, A, lc, uc, lx, ux, mosek_data, x);

    // I always require success in everything I do!
    REQUIRE(success);
    REQUIRE(x.size() == expected_solution.size());
    CHECK((x - expected_solution).squaredNorm() == Approx(0.0).margin(1e-12));
}

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <Eigen/LU> // required for MatrixBase::determinant
#include <Eigen/SVD> // required for SVD

using namespace Eigen;

//  Constructs a random matrix from the unitary group U(size).
template <typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixUnitary(int size)
{
  typedef T Scalar;
  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;

  MatrixType Q;

  int max_tries = 40;
  double is_unitary = false;

  while (!is_unitary && max_tries > 0)
  {
    // initialize random matrix
    Q = MatrixType::Random(size, size);

    // orthogonalize columns using the Gram-Schmidt algorithm
    for (int col = 0; col < size; ++col)
    {
      typename MatrixType::ColXpr colVec = Q.col(col);
      for (int prevCol = 0; prevCol < col; ++prevCol)
      {
        typename MatrixType::ColXpr prevColVec = Q.col(prevCol);
        colVec -= colVec.dot(prevColVec)*prevColVec;
      }
      Q.col(col) = colVec.normalized();
    }

    // this additional orthogonalization is not necessary in theory but should enhance
    // the numerical orthogonality of the matrix
    for (int row = 0; row < size; ++row)
    {
      typename MatrixType::RowXpr rowVec = Q.row(row);
      for (int prevRow = 0; prevRow < row; ++prevRow)
      {
        typename MatrixType::RowXpr prevRowVec = Q.row(prevRow);
        rowVec -= rowVec.dot(prevRowVec)*prevRowVec;
      }
      Q.row(row) = rowVec.normalized();
    }

    // final check
    is_unitary = Q.isUnitary();
    --max_tries;
  }

  if (max_tries == 0)
    eigen_assert(false && "randMatrixUnitary: Could not construct unitary matrix!");

  return Q;
}

//  Constructs a random matrix from the special unitary group SU(size).
template <typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixSpecialUnitary(int size)
{
  typedef T Scalar;

  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;

  // initialize unitary matrix
  MatrixType Q = randMatrixUnitary<Scalar>(size);

  // tweak the first column to make the determinant be 1
  Q.col(0) *= numext::conj(Q.determinant());

  return Q;
}

template <typename MatrixType>
void run_test(int dim, int num_elements)
{
  using std::abs;
  typedef typename internal::traits<MatrixType>::Scalar Scalar;
  typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
  typedef Matrix<Scalar, Eigen::Dynamic, 1> VectorX;

  // MUST be positive because in any other case det(cR_t) may become negative for
  // odd dimensions!
  const Scalar c = abs(internal::random<Scalar>());

  MatrixX R = randMatrixSpecialUnitary<Scalar>(dim);
  VectorX t = Scalar(50)*VectorX::Random(dim,1);

  MatrixX cR_t = MatrixX::Identity(dim+1,dim+1);
  cR_t.block(0,0,dim,dim) = c*R;
  cR_t.block(0,dim,dim,1) = t;

  MatrixX src = MatrixX::Random(dim+1, num_elements);
  src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));

  MatrixX dst = cR_t*src;

  MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements));

  const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm();
  VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon());
}

template<typename Scalar, int Dimension>
void run_fixed_size_test(int num_elements)
{
  using std::abs;
  typedef Matrix<Scalar, Dimension+1, Dynamic> MatrixX;
  typedef Matrix<Scalar, Dimension+1, Dimension+1> HomMatrix;
  typedef Matrix<Scalar, Dimension, Dimension> FixedMatrix;
  typedef Matrix<Scalar, Dimension, 1> FixedVector;

  const int dim = Dimension;

  // MUST be positive because in any other case det(cR_t) may become negative for
  // odd dimensions!
  const Scalar c = abs(internal::random<Scalar>());

  FixedMatrix R = randMatrixSpecialUnitary<Scalar>(dim);
  FixedVector t = Scalar(50)*FixedVector::Random(dim,1);

  HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1);
  cR_t.block(0,0,dim,dim) = c*R;
  cR_t.block(0,dim,dim,1) = t;

  MatrixX src = MatrixX::Random(dim+1, num_elements);
  src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));

  MatrixX dst = cR_t*src;

  Block<MatrixX, Dimension, Dynamic> src_block(src,0,0,dim,num_elements);
  Block<MatrixX, Dimension, Dynamic> dst_block(dst,0,0,dim,num_elements);

  HomMatrix cR_t_umeyama = umeyama(src_block, dst_block);

  const Scalar error = ( cR_t_umeyama*src - dst ).array().square().sum();

  VERIFY(error < Scalar(10)*std::numeric_limits<Scalar>::epsilon());
}

void test_umeyama()
{
  for (int i=0; i<g_repeat; ++i)
  {
    const int num_elements = internal::random<int>(40,500);

    // works also for dimensions bigger than 3...
    for (int dim=2; dim<8; ++dim)
    {
      CALL_SUBTEST_1(run_test<MatrixXd>(dim, num_elements));
      CALL_SUBTEST_2(run_test<MatrixXf>(dim, num_elements));
    }

    CALL_SUBTEST_3((run_fixed_size_test<float, 2>(num_elements)));
    CALL_SUBTEST_4((run_fixed_size_test<float, 3>(num_elements)));
    CALL_SUBTEST_5((run_fixed_size_test<float, 4>(num_elements)));

    CALL_SUBTEST_6((run_fixed_size_test<double, 2>(num_elements)));
    CALL_SUBTEST_7((run_fixed_size_test<double, 3>(num_elements)));
    CALL_SUBTEST_8((run_fixed_size_test<double, 4>(num_elements)));
  }

  // Those two calls don't compile and result in meaningful error messages!
  // umeyama(MatrixXcf(),MatrixXcf());
  // umeyama(MatrixXcd(),MatrixXcd());
}


#include "Prime.h"
#include <cassert>
#include <thread>
#include <utility>
#include <iostream>

#include <boost/filesystem.hpp>
#include <boost/multiprecision/miller_rabin.hpp>

#include <rt/tasq.h>

#define PRIME_LIST_FILENAME "Primes.inc"
static const boost::multiprecision::cpp_int Primes[] = {
#include "Primes.inc"
};
static const size_t PrimeItems = sizeof(Primes) / sizeof(Primes[0]);

namespace {
#ifdef rt_SRC_DIR
boost::filesystem::path PrimeListDir = rt_SRC_DIR;
auto PrimeListPath = PrimeListDir / PRIME_LIST_FILENAME;
#endif
} // namespace

namespace omnn::rt {

bool GrowPrime(const boost::multiprecision::cpp_int& upto,
               std::function<bool(boost::multiprecision::cpp_int)> is_prime) {
    auto& prev = Primes[PrimeItems - 1];
    auto next = prev;
    ++next;
    static auto from = next;
    auto range = upto;
    range -= prev;
    auto chunks = std::thread::hardware_concurrency() - 1; // One thread left free for GC
    auto chunk = range;
    chunk /= chunks;
    if (from + chunk * chunks < upto) {
        ++chunk;
    }
    std::deque<std::future<std::string>> primining;
    std::cout << "new prime table target: " << upto << '(' << from + chunk * chunks << ')'<<std::endl;
    for (decltype(chunks) i = 0; i < chunks; ++i) {
        primining.emplace_back(std::async(std::launch::async, [=]() {
            std::stringstream ss;
            auto j = chunk;
            j *= i;
            j += from;
            auto up = j;
            up += chunk; // NOTE: preserving up type same as j. Expression (j+chunk)  type differs from j type.
            {
                static std::mutex m;
                std::lock_guard l(m);
                std::cout << '[' << j << ',' << up << ')';
            }
            for (; j < up; ++j) {
                if (is_prime(j)) {
                    std::cout << j << ',';
                    ss << j << ',';
                }
            }
            return std::move(ss.str());
        }));
    }

    int i = -1;
    while (primining.size()) {
        auto line = primining.front().get();
        std::cout << "Chunk " << ++i << std::endl << line << std::endl << "\n chunk is ready. Appending." << std::endl;
        std::ofstream PrimesIncFile(PrimeListPath.string(), std::ios_base::app);
        PrimesIncFile << std::endl << line;
        PrimesIncFile.close();
        primining.pop_front();
    }
    from++ = upto;
    return true;
}

const boost::multiprecision::cpp_int& prime(size_t idx) {
#ifndef NDEBUG
    auto haveThePrime = idx < PrimeItems;
    if (!haveThePrime) {
        std::cerr << "There is no " << idx
                  << "th prime in the table yet. Use GrowPrime call if you use debug version to increase the table for "
                     "next compile time. Currently the table has "
                  << PrimeItems << " elements" << std::endl;
        assert(haveThePrime);
    }
#endif
    return Primes[idx];
}

size_t primes() { return PrimeItems - 1; }

} // namespace omnn::rt


#include <iostream>
#include <boost/date_time.hpp>
#include "../mylib/timeit.hpp"

std::vector<long> nsquared(std::vector<double>& changes){
    double value = 0; //{
    double direction = 0;
    long buyday = -1;
    long sellday = -1;
    double best = -DBL_MAX;
    std::vector<double> values;
    values.emplace_back(value); //} = θ(1)
    for (auto dx : changes){ // løkke = θ(n)
        value += dx;
        values.emplace_back(value); // innhold = θ(1)
    } //1*n = n
    //+
    for (auto i = values.begin(); i<values.end(); ++i){ // løkke = θ(n)
        for (auto j = i+1; j<values.end(); j++){ //sum 1 to n-1 = θ(n²)
            if (*j - *i > best){
                best = *j - *i;
                buyday = i-values.begin();
                sellday = j-values.begin(); //innhold = θ(1)
            }
        }
    } //n²*1 = n²
    return {buyday, sellday, (long)best};

}

std::vector<long> nlinear(std::vector<double>& changes){
    double lowest = 0;
    long lowestDay = 0;
    double highest = 0;
    long highestDay = 0;
    double value = 0;
    std::vector<long> bestHigh; //low high == buy sell
    std::vector<long> bestLow; //low high == buy sell
    std::vector<double> values; //deklareringer er θ(1)

    for(auto dx = changes.begin(); dx < changes.end(); dx++){ //løkke gjennom liste så θ(n)
        value += *dx;
        if(value < lowest){
            bestHigh.emplace_back(highestDay);
            bestLow.emplace_back(lowestDay);
            values.emplace_back(highest-lowest);
            lowest = highest = value;
            lowestDay = highestDay = dx-changes.begin();
        }
        if(value > highest){
            highest = value;
            highestDay = dx-changes.begin();
        }
        //ifs og emplace etc θ(1)
    } // 1*n=n

    int besti = 0;
    for (int i = 0; i < bestHigh.size(); ++i) { //størrelse på disse er mellom 1 og n, altså omega(1) og O(n)
        if (values[i] > values[besti]){
            besti = i;
        } //lookup er θ(1)
    }
    std::vector<long> results;
    results.emplace_back(bestLow[besti]+1);
    results.emplace_back(bestHigh[besti]+1);
    results.emplace_back(values[besti]); //lookups er θ(1)
    return results; //totalt θ(n)

}

int main(){

    using namespace std;

    /*
    vector<double> referencedata = {-1, +3, -9, +2, +2, -1, +2, -1, -5}; //rett svar: kjøp 3 selg 7 val 5
    for(auto i : nlinear(referencedata)) cout << i << " ";
    cout << endl;
    */ //sjekk svar

    // /*
    for (int i = 1; i<1000000000    ; i++) { //40 datapunkter
        vector<double> row;
        for (int j = 0; j < i*10000000; j++){
            row.emplace_back(rand() - RAND_MAX/2);
        }
        cout << row.size() << ", " << my::timeit(nlinear, row) << endl;
    }
    cout << "DONE" << endl;
    // */timing

    return EXIT_SUCCESS;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands.
//
// Use, modification and distribution is 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)
//
// Projection example 4, reworked version of example 3
// Now using svg mapper, multi polygons and specific transform strategy

#include <fstream>

#include <boost/foreach.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/geometry/io/svg/svg_mapper.hpp>
#include <boost/geometry/extensions/gis/latlong/latlong.hpp>

#include <boost/geometry/extensions/gis/projections/parameters.hpp>
#include <boost/geometry/extensions/gis/projections/proj/robin.hpp>

// Define a specific projection transformer
// (NOTE: this might become part of the library)
template <typename Projection>
struct projection_transformer
{
    Projection const& m_prj;

    inline projection_transformer(Projection const& prj)
        : m_prj(prj)
    {}

    inline bool apply(typename Projection::geographic_point_type const& p1,
                typename Projection::cartesian_point_type& p2) const
    {
        return m_prj.forward(p1, p2);
    }
};

void read_wkt_and_project_and_map_svg(std::string const& wkt_filename,
        std::string const& svg_filename)
{
    using namespace boost::geometry;

    typedef model::ll::point<degree> point_ll_deg;
    typedef model::d2::point_xy<double> point_xy;

    typedef model::multi_polygon<model::polygon<point_ll_deg> > mp_ll;
    typedef model::multi_polygon<model::polygon<point_xy> > mp_xy;

    typedef projections::robin_spheroid<point_ll_deg, point_xy> robin;

    std::vector<mp_ll> countries_in_ll;

    // Read polygons from WKT
    std::ifstream cpp_file(wkt_filename.c_str());
    if (! cpp_file.is_open())
    {
        throw std::string("File not found: ") + wkt_filename;
    }

    while (! cpp_file.eof() )
    {
        std::string line;
        std::getline(cpp_file, line);
        if (boost::starts_with(line, "MULTIPOLYGON"))
        {
            countries_in_ll.resize(countries_in_ll.size() + 1);
            boost::geometry::read_wkt(line, countries_in_ll.back());
        }
    }

    robin prj(projections::init("+ellps=WGS84 +units=m"));
    projection_transformer<robin> projection(prj);

    // Project the polygons, and at the same time get the bounding box (in xy)
    std::vector<mp_xy> countries_in_xy;
    model::box<point_xy> bbox;
    assign_inverse(bbox);
    BOOST_FOREACH(mp_ll const& country_ll, countries_in_ll)
    {
        mp_xy country_xy;
        if (transform(country_ll, country_xy, projection))
        {
            expand(bbox, return_envelope<model::box<point_xy> >(country_xy));
            countries_in_xy.push_back(country_xy);
        }
    }

    // Create an SVG image
    std::ofstream svg(svg_filename.c_str());
    boost::geometry::svg_mapper<point_xy> mapper(svg, 1000, 800);
    mapper.add(bbox);

    BOOST_FOREACH(mp_xy const& country, countries_in_xy)
    {
        mapper.map(country, "fill-opacity:0.6;fill:rgb(153,204,0);stroke:rgb(0,128,0);stroke-width:0.2");
    }
}

int main(int argc, char** argv)
{
    try
    {
        // Note, file location: trunk/libs/geometry/example/data
        // update path below if necessary
        read_wkt_and_project_and_map_svg(
            "../../../../example/data/world.wkt",
            "world4.svg");
    }
    catch(std::exception const& e)
    {
        std::cout << "Exception: " << e.what() << std::endl;
        return 1;
    }
    catch(std::string const& s)
    {
        std::cout << "Exception: " << s << std::endl;
        return 1;
    }

    return 0;
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file linear_programming_long_test.cpp
 * @brief
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2014-04-03
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_lp.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/lp/glp.hpp"
#include "paal/utils/parse_file.hpp"

#include <boost/mpl/list.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/numeric.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>

#include <fstream>
#include <unordered_map>

using lp_types = boost::mpl::list<paal::lp::glp>;

template <typename LP, typename RowBounds, typename ColBounds,
          typename CostCoefs, typename Coefficients>
void run_test(RowBounds &row_bounds, ColBounds &col_bounds,
              CostCoefs &cost_coefs, Coefficients &coefs, double best_cost,
              int rows_num, int cols_num, int non_zeros,
              paal::lp::simplex_type type) {

    LP lp("LP long test", paal::lp::MINIMIZE);
    std::unordered_map<std::string, paal::lp::col_id> col_ids;

    std::string col_name;
    for (auto c : col_bounds) {
        col_name = c.first;
        col_ids[col_name] =
            lp.add_column(0., c.second.first, c.second.second, col_name);
    }

    int non_zero_cost_coefs = 0;
    for (auto c : cost_coefs) {
        lp.set_col_cost(col_ids[c.first], c.second);
        if (!paal::utils::compare<double>().e(c.second, 0.)) {
            ++non_zero_cost_coefs;
        }
    }

    std::string row_name;
    std::pair<double, double> row_bound;
    for (auto row : row_bounds) {
        std::tie(row_name, row_bound) = row;

        paal::lp::linear_expression expr;

        for (auto elem :
             boost::make_iterator_range(coefs.equal_range(row_name))) {
            expr += elem.second.second * col_ids[elem.second.first];
        }

        lp.add_row(row_bound.first <= std::move(expr) <= row_bound.second);
    }

    BOOST_CHECK_EQUAL(rows_num, lp.rows_number() + 1);
    BOOST_CHECK_EQUAL(cols_num, lp.columns_number());
    auto lp_non_zeros =
        boost::accumulate(lp.get_rows(), 0, [&](int sum, paal::lp::row_id row) {
        return sum + lp.get_row_degree(row);
    });
    BOOST_CHECK_EQUAL(non_zeros, lp_non_zeros + non_zero_cost_coefs);

    auto status = lp.solve_simplex(type);
    BOOST_CHECK_EQUAL(status, paal::lp::OPTIMAL);
    static const double EPSILON = 6e-5;
    BOOST_CHECK_CLOSE(best_cost, lp.get_obj_value(), EPSILON);
}

BOOST_AUTO_TEST_SUITE(linear_programming_long)

BOOST_AUTO_TEST_CASE_TEMPLATE(linear_programming_long, LP, lp_types) {
    std::string test_dir = paal::system::get_test_data_dir("LP");
    using paal::system::build_path;
    paal::parse(build_path(test_dir, "cases.txt"),
                [&](const std::string & fname, std::istream & is_test_cases) {
        double best_cost;
        int rows_num, cols_num, non_zeros;
        is_test_cases >> best_cost >> rows_num >> cols_num >> non_zeros;

        LOGLN(fname);
        std::ifstream ifs(build_path(test_dir, "/cases/" + fname + ".mps"));
        assert(ifs.good());

        std::unordered_map<std::string, std::pair<double, double>> row_bounds;
        std::unordered_map<std::string, std::pair<double, double>> col_bounds;
        std::unordered_map<std::string, double> cost_coefs;
        std::unordered_multimap<std::string, std::pair<std::string, double>>
            coefs;

        paal::read_lp(ifs, row_bounds, col_bounds, cost_coefs, coefs);
        LOGLN("primal");
        run_test<LP>(row_bounds, col_bounds, cost_coefs, coefs, best_cost,
                     rows_num, cols_num, non_zeros, paal::lp::PRIMAL);
        LOGLN("dual");
        run_test<LP>(row_bounds, col_bounds, cost_coefs, coefs, best_cost,
                     rows_num, cols_num, non_zeros, paal::lp::DUAL);
    });
}

BOOST_AUTO_TEST_SUITE_END()


#include <cmath>
#include <iomanip>
#include <iostream>

#include <boost/numeric/odeint.hpp>
#include <catch/catch.hpp>
#include <rxcpp/rx.hpp>

#include "../include/Plant.hpp"
#include "../include/pid.hpp"
#include "../include/util/util.hpp"

#include "calculations/analytical_solutions.cpp"

#ifdef PLOT
#include "../include/plotting/gnuplot-iostream.h"
#include "../include/plotting/plot-helpers.hpp"
#endif  // PLOT

using namespace std::string_literals;

namespace ode = boost::numeric::odeint;
using CState = PIDState<>;                       // AKA U
using PState = SignalPt<std::array<double, 2>>;  // AKA X
// NB:
// PState = SignalPt<std::array<double, 2>>
//          ┌                ┐
//          │        ┌   ┐   │
//        = │  · ,   │ · │   │
//          │        │ · │   │
//          │        └   ┘   │
//          └                ┘
//            ^ Time   ^ [Position, Speed]
//
// On the other hand, sim::PState is just for Boost.odeint. It doesn't need
// time, but must be augmented with the control variable:
//
// sim::Pstate = std::array<double, 3>
//                ┌   ┐
//                │ · │  // Position
//             =  │ · │  // Speed
//                │ · │  // control variable for Boost.odeint.
//                └   ┘

constexpr double dt = 0.001;  // seconds.
constexpr auto dts = util::double_to_duration(dt);
const auto now = chrono::steady_clock::now();

constexpr double mass = 1.;
constexpr double damp = 10. / mass;
constexpr double spring = 20. / mass;
constexpr double staticForce = 1. / mass;
constexpr auto simDuration = 2s;  // seconds

// position_error : (PState, PState) → SignalPt<double>
inline SignalPt<double> position_error(const std::tuple<PState, PState>& ab) {
  auto& [a, b] = ab;
  return {std::max(a.time, b.time), a.value[0] - b.value[0]};
}

struct WorldInterface {
  const rxcpp::subjects::behavior<PState> txSubject;
  const rxcpp::observable<PState> setPoint =
      // Setpoint to x = 1, for step response.
      rxcpp::observable<>::just(PState{now, {1., 0.}});

  WorldInterface(PState x0) : txSubject(x0) {}

  void controlled_step(CState u) {
    auto x = txSubject.get_value();
    sim::PState xAugmented = {x.value[0], x.value[1], u.ctrlVal};

    if ((x.time - now) >= simDuration)
      txSubject.get_subscriber().on_completed();

    // do_step uses the second argument for both input and output.
    _stepper.do_step(_plant, xAugmented, 0, dt);
    x.time += dts;
    x.value = {xAugmented[0], xAugmented[1]};
    txSubject.get_subscriber().on_next(x);
  };

  auto get_state_observable() { return txSubject.get_observable(); }
  auto time_elapsed() { return txSubject.get_value().time - now; }

 private:
  ode::runge_kutta4<sim::PState> _stepper;
  const sim::Plant _plant = sim::Plant(staticForce, damp, spring);
};

void step_response_test(const std::string testTitle, const double Kp,
                        const double Ki, const double Kd,
                        const std::function<double(double)> expected_fn,
                        const double margin) {
  const CState u0 = {now, 0., 0., 0.};
  const PState x0 = {now, {0., 0.}};
  WorldInterface worldIface(x0);

  std::vector<PState> plantStateRecord;
  worldIface.get_state_observable().subscribe(
      [&](PState x) { plantStateRecord.push_back(x); });

  // A classical confiuration for a feedback controller is illustrated as:
  //                  err   u
  //   setPoint ──➤ ⊕ ──➤ C ──➤ P ──┬──➤ plantState
  //               -↑               │
  //                │               │
  //                ╰───────────────┘
  // If you open the loop and place an interface to the imperative world, ◼,
  // at the endpoints, the controller becomes:
  //
  // setPoint  err   u
  //    ◼ ─➤ ⊕ ──➤ C ──➤ ◼
  //        -↑
  //         │ plantState
  //         ◼
  // which is in 1:1 correspondence with the code below (read backward from C)
  //
  const auto sControls =
      worldIface
          .get_state_observable()               // worldIface == ◼.
          .combine_latest(worldIface.setPoint)  // ┐ Combine state and setPt,
          .map(&position_error)                 // ┘   and then ⊕.
          .observe_on(rxcpp::identity_current_thread())
          .scan(u0, pid_algebra(Kp, Ki, Kd));  //   This is C

  sControls.subscribe(
      [&worldIface](CState u) { worldIface.controlled_step(u); });

  {
    auto simulatedPositions =
        util::fmap([](auto x) { return x.value[0]; }, plantStateRecord);
    auto theoreticalPositions = util::fmap(
        [&](auto x) { return expected_fn(util::unchrono_sec(x.time - now)); },
        plantStateRecord);

    if constexpr (plot) {
      const auto testData = util::fmap(
          [](const auto& x) {
            return std::make_pair(util::unchrono_sec(x.time - now), x.value[0]);
          },
          plantStateRecord);

      plot_with_tube(testTitle, testData, expected_fn, margin);
    }

    REQUIRE(
        util::compareVectors(simulatedPositions, theoreticalPositions, margin));
  }
}

TEST_CASE(
    "Given system and controller parameters, simulation should reproduce "
    "analytically computed step responses to within a margin of error. See "
    "src/calculations for details. Simulations performed using FRx.") {
  SECTION("Test A (Proportional Control) FRx.") {
    constexpr double Kp = 300.;
    constexpr double Ki = 0.;
    constexpr double Kd = 0.;
    const auto title = "Test A; (Kp, Ki, Kd) = (300., 0., 0.); FRx"s;
    step_response_test(title, Kp, Ki, Kd, &analyt::test_A, 0.03);
  }

  SECTION("Test B (Proportional-Derivative Control) FRx.") {
    constexpr double Kp = 300.;
    constexpr double Ki = 0.;
    constexpr double Kd = 10.;
    const auto title = "Test B; (Kp, Ki, Kd) = (300., 0., 10.); FRx"s;
    step_response_test(title, Kp, Ki, Kd, &analyt::test_B, 0.03);
  }

  SECTION("Test C (Proportional-Integral Control) FRx.") {
    constexpr double Kp = 30.;
    constexpr double Ki = 70.;
    constexpr double Kd = 0.;
    const auto title = "Test C; (Kp, Ki, Kd) = (30., 70., 0.); FRx"s;
    step_response_test(title, Kp, Ki, Kd, &analyt::test_C, 0.03);
  }

  SECTION("Test D (Proportional-Integral-Derivative Control) FRx.") {
    constexpr double Kp = 350.;
    constexpr double Ki = 300.;
    constexpr double Kd = 50.;
    const auto title = "Test D; (Kp, Ki, Kd) = (350., 300., 50.); FRx"s;
    step_response_test(title, Kp, Ki, Kd, &analyt::test_D, 0.07);
  }
}


#ifndef ED_TITFI_HPP
#define ED_TITFI_HPP
#include <cstdint>
#include <cassert>
#include <algorithm>
#include <map>
#include <boost/dynamic_bitset.hpp>
//#include "BitOperations.h"
#include "../Basis/AbstractBasis1D.hpp"

template<typename UINT>
class TITFIsing
{
private:
	const edlib::AbstractBasis1D<UINT>& basis_;
	double J_;
	double h_;

public:
	TITFIsing(const edlib::AbstractBasis1D<UINT>& basis, double J, double h)
		: basis_(basis), J_(J), h_(h)
	{

	}

	std::map<std::size_t,double> getCol(UINT n) const
	{
		unsigned int N = basis_.getN();

		UINT a = basis_.getNthRep(n);
		const boost::dynamic_bitset<> bs(N, a);

		std::map<std::size_t, double> m;
		for(unsigned int i = 0; i < N; i++)
		{
			//Next-nearest
			{
				unsigned int j = (i+1)%N;
				int sgn = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += -J_*sgn; //ZZ

				UINT s = a;
				s ^= basis_.mask({i});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += -h_*coeff;
			}
		}
		return m;
	}
};
#endif //ED_TITFI_HPP


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/integral.hpp>

#include <boost/hana/detail/assert.hpp>
using namespace boost::hana;

int main() {
    // Arithmetic
    BOOST_HANA_CONSTANT_ASSERT(+int_<1> == int_<1>);
    BOOST_HANA_CONSTANT_ASSERT(-int_<1> == int_<-1>);
    BOOST_HANA_CONSTANT_ASSERT(int_<1> + int_<2> == int_<3>);
    BOOST_HANA_CONSTANT_ASSERT(int_<1> - int_<2> == int_<-1>);
    BOOST_HANA_CONSTANT_ASSERT(int_<3> * int_<2> == int_<6>);
    BOOST_HANA_CONSTANT_ASSERT(int_<6> / int_<3> == int_<2>);
    BOOST_HANA_CONSTANT_ASSERT(int_<6> % int_<4> == int_<2>);
    BOOST_HANA_CONSTANT_ASSERT(~int_<6> == int_<~6>);
    BOOST_HANA_CONSTANT_ASSERT((int_<6> & int_<3>) == int_<6 & 3>);
    BOOST_HANA_CONSTANT_ASSERT(int_<6> | int_<3> == int_<6 | 3>);
    BOOST_HANA_CONSTANT_ASSERT(int_<6> ^ int_<3> == int_<6 ^ 3>);
    BOOST_HANA_CONSTANT_ASSERT((int_<6> << int_<3>) == int_<(6 << 3)>);
    BOOST_HANA_CONSTANT_ASSERT((int_<6> >> int_<3>) == int_<(6 >> 3)>);

    // Comparison
    BOOST_HANA_CONSTANT_ASSERT(int_<0> == int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(int_<1> != int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(int_<0> < int_<1>);
    BOOST_HANA_CONSTANT_ASSERT(int_<0> <= int_<1>);
    BOOST_HANA_CONSTANT_ASSERT(int_<0> <= int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(int_<1> > int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(int_<1> >= int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(int_<0> >= int_<0>);

    // Logical
    BOOST_HANA_CONSTANT_ASSERT(int_<3> || int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(int_<3> && int_<1>);
    BOOST_HANA_CONSTANT_ASSERT(!int_<0>);
    BOOST_HANA_CONSTANT_ASSERT(!!int_<3>);
}


#ifndef MWTRANS_H_
#define MWTRANS_H_

#include <Eigen/Dense>
#include <complex>
#include <functional>

class MWtransIntComplex {
public:
  MWtransIntComplex(double lb, double v, double abserr, double referr);

  std::complex<double>
  perform(std::function<std::complex<double>(double)> functor);

private:
  const int nzero_ = 100;
  const int niter_ = 100;
  const unsigned max_depth_ = 15;

  Eigen::ArrayXd zeros_;
  double lb_, ub_, v_;
  const double abserr_;
  const double referr_;
};

#endif


//######################################################################
//#   GT Writter Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                Roman Kaskman under supervision of Slobodan Ilic
//#######################################################################

#include "scene.h"
#include <cmath>
#include "frame.h"
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;
#define _USE_MATH_DEFINES

Scene::Scene(const Configuration &p_configuration, const std::string &p_scene_dir,
		const std::vector<Model> &p_models, const std::vector<Eigen::Matrix4f> &p_camera_poses):
configuration(p_configuration), models(p_models), frame_poses(p_camera_poses), scene_dir(p_scene_dir)
{
	for (const auto& model : models)
	{
		scaled_renderers.emplace_back(ModelRenderer(model, configuration, true));
	}

}

size_t Scene::get_number_of_frames()
{
	return frame_poses.size();
}

inline int get_scaled_coordinate(int x, int cx, float focal_length_scale)
{
	return static_cast<int>(round(1.0f / focal_length_scale * (x - cx * (1.0f - focal_length_scale))));
}

Vector4i get_bounding_box(const cv::Mat& scaled_depth, const Matrix3f& scaled_intrinsics, float focal_length_scale)
{
	float cx = scaled_intrinsics(0, 2);
	float cy = scaled_intrinsics(1, 2);

	int min_y = scaled_depth.rows - 1;
	int min_x = scaled_depth.cols - 1;
	int max_y = 0;
	int max_x = 0;

	for (int i = 0; i < scaled_depth.rows; ++i)
	{
		for (int j = 0; j < scaled_depth.cols; ++j)
		{
			float val = scaled_depth.at<float>(i, j);

			if (isfinite(val) && val > 1e-3f)
			{
				if (i < min_y)
				{
					min_y = i;
				}
				else if (i > max_y)
				{
					max_y = i;
				}

				if (j < min_x)
				{
					min_x = j;
				}
				else if (j > max_x)
				{
					max_x = j;
				}
			}
		}
	}

	int min_y_scaled = get_scaled_coordinate(min_y, cy, focal_length_scale) - 1;
	int max_y_scaled = get_scaled_coordinate(max_y, cy, focal_length_scale) + 1;

	int min_x_scaled = get_scaled_coordinate(min_x, cx, focal_length_scale) - 1;
	int max_x_scaled = get_scaled_coordinate(max_x, cx, focal_length_scale) + 1;

	return { min_x_scaled, min_y_scaled, max_x_scaled - min_x_scaled, max_y_scaled - min_y_scaled };
}

vector<Frame> Scene::convert_to_scene_frames()
{
	size_t number_of_frames = frame_poses.size();
	size_t number_of_models = models.size();

	Matrix3f scaled_intrinsics = configuration.get_intrinsics();
	float focal_length_scale = configuration.get_focal_length_scale();

	scaled_intrinsics(0, 0) = focal_length_scale * scaled_intrinsics(0, 0);
	scaled_intrinsics(1, 1) = focal_length_scale * scaled_intrinsics(1, 1);

	vector<Frame> scene_frames(number_of_frames);

	for (int frame_idx = 0; frame_idx < number_of_frames; frame_idx++)
	{
		Frame frame;
		frame.frame_id = frame_idx;
		frame.scene_dir = scene_dir;

		cout << "Frame : " << frame_idx << endl;
		Matrix4f world_to_cam = frame_poses[frame_idx].inverse();

		for (int model_idx = 0; model_idx < number_of_models; model_idx++)
		{
			const Model& model = models[model_idx];

			ModelRenderer& scaled_renderer = scaled_renderers[model_idx];

			cv::Mat scaled_depth;
			cv::Mat scaled_color;

			Matrix4f model_pose = world_to_cam * model.canonical_pose;
			scaled_renderer.render(model_pose, scaled_depth, scaled_color);

			auto bbox = get_bounding_box(scaled_depth, scaled_intrinsics, focal_length_scale);
			frame.frame_models.emplace_back(FrameModel(model.model_id, model_pose, bbox));
		}

		scene_frames[frame_idx] = frame;
	}

	return scene_frames;
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

// エラトステネスの篩
template <typename T>
std::vector<T> sieve_of_eratosthenes(T n) {
	std::vector<T> sieve(n, 0);

	for(int i = 2; i < n; i++) {
		sieve[i] = i;
	}

	T i = 2;
	while(i * i < n) {
		if(sieve[i]) {
			for(T j = i * i; j < n; j += i) {
				sieve[j] = 0;
			}
		}
		i++;
	}

	return sieve;
}

// 素数リスト
template <typename T>
std::vector<T> prime_list(T n) {
	std::vector<T> primes = sieve_of_eratosthenes(n);
	primes.erase(std::remove(primes.begin(), primes.end(), 0), primes.end());
	return primes;
}

int main() {
	int q;
	cin >> q;
	VI l(q);
	VI r(q);
	REP(i, q) { cin >> l[i] >> r[i]; }
	VI sieve = sieve_of_eratosthenes(100030);
	VI primes = prime_list(100030);
	VI count(100001, 0);
	for(int i = 1; i <= 100000; i += 2) {
		count[i] += (sieve[i] && sieve[(i + 1) / 2]) ? 1 : 0;
	}
	VI s(100002, 0);
	for(int i = 1; i <= 100001; i++) {
		s[i] = s[i - 1] + count[i - 1];
	}
	REP(i, q) { cout << s[r[i] + 1] - s[l[i]] << endl; }
	return 0;
}


//   Copyright (c) 2018 Shahrzad Shirzad
//
//   Distributed under the Boost Software License, Version 1.0.0. (See accompanying
//   file LICENSE_1_0.0.txt or copy at http://www.boost.org/LICENSE_1_0.0.txt)

#include <phylanx/phylanx.hpp>
#include <hpx/hpx_init.hpp>

#include <iostream>

#include <blaze/Math.h>
#include <boost/program_options.hpp>
#include <cstdint>
#include <string>

///////////////////////////////////////////////////////////////////////////////
char const* const read_x_code = R"(
    //
    // Read input-data from given CSV file
    //
    define(read_x, filepath, row_start, row_stop, col_start, col_stop,
        slice(file_read_csv(filepath), make_list(row_start , row_stop),
              make_list(col_start , col_stop))
    )
    read_x
)";

char const* const als_code = R"(
    //
    // Alternating Least squares algorithm
    //
    define(als, ratings, regularization, num_factors, iterations, alpha, enable_output,
        block(
            define(num_users, shape(ratings, 0)),
            define(num_items, shape(ratings, 1)),
            define(conf, alpha * ratings),

            define(conf_u, constant(0.0, make_list(num_items))),
            define(conf_i, constant(0.0,make_list(num_users))),

            define(c_u, constant(0.0, make_list(num_items, num_items))),
            define(c_i, constant(0.0, make_list(num_users, num_users))),
            define(p_u, constant(0.0, make_list(num_items))),
            define(p_i, constant(0.0, make_list(num_users))),

            set_seed(0),
            define(X, random(make_list(num_users, num_factors))),
            define(Y, random(make_list(num_items, num_factors))),
            define(I_f, identity(num_factors)),
            define(I_i, identity(num_items)),
            define(I_u, identity(num_users)),
            define(k, 0),
            define(i, 0),
            define(u, 0),

            define(XtX, constant(0.0, make_list(num_factors, num_factors))),
            define(YtY, constant(0.0, make_list(num_factors, num_factors))),
            define(A, constant(0.0, make_list(num_factors, num_factors))),
            define(b, constant(0.0, make_list(num_factors))),

            while(k < iterations,
                block(
                    if(enable_output,
                            block(
                                    cout("iteration ",k),
                                    cout("X: ",X),
                                    cout("Y: ",Y)
                            )
                    ),
                    store(YtY, dot(transpose(Y), Y) + regularization * I_f),
                    store(XtX, dot(transpose(X), X) + regularization * I_f),

                    while(u < num_users,
                        block(
                            store(conf_u, slice_row(conf, u)),
                            store(c_u, diag(conf_u)),
                            store(p_u, __ne(conf_u,0.0,true)),
                            store(A, dot(dot(transpose(Y), c_u), Y)+ YtY),
                            store(b, dot(dot(transpose(Y), (c_u + I_i)), transpose(p_u))),
                            store(slice(X, list(u, u + 1, 1),nil), dot(inverse(A), b)),
                            store(u, u + 1)
                        )
                    ),
                    store(u, 0),
                    while(i < num_items,
                        block(
                            store(conf_i, slice_column(conf, i)),
                            store(c_i, diag(conf_i)),
                            store(p_i, __ne(conf_i, 0.0, true)),
                            store(A, dot(dot(transpose(X), c_i),X) + XtX),
                            store(b, dot(dot(transpose(X), (c_i + I_u)), transpose(p_i))),
                            store(slice(Y, list(i, i + 1, 1),nil), dot(inverse(A), b)),
                            store(i, i + 1)
                        )
                    ),
                    store(i, 0),
                    store(k, k + 1)
                )
            ),
            list(X, Y)
        )
    )
    als
)";

int hpx_main(boost::program_options::variables_map& vm)
{
    if (vm.count("data_csv") == 0)
    {
        std::cerr << "Please specify '--data_csv=data-file'";
        return hpx::finalize();
    }

    // evaluate generated execution tree
    auto row_start = static_cast<int64_t>(0);
    auto col_start = static_cast<int64_t>(0);
    auto row_stop = vm["row_stop"].as<std::int64_t>();
    auto col_stop = vm["col_stop"].as<std::int64_t>();

    auto regularization = vm["regularization"].as<double>();
    auto iterations = vm["iterations"].as<int64_t>();
    auto num_factors = vm["factors"].as<int64_t>();
    auto alpha = vm["alpha"].as<double>();
    auto filepath = vm["data_csv"].as<std::string>();

    bool enable_output = vm.count("enable_output") != 0;

    // compile the given code
    phylanx::execution_tree::compiler::function_list snippets;
    auto const& code_read_x =
        phylanx::execution_tree::compile("read_x", read_x_code, snippets);
    auto read_x = code_read_x.run();

    auto ratings = read_x(filepath, row_start, row_stop, col_start, col_stop);

    auto const& code_als = phylanx::execution_tree::compile(als_code, snippets);
    auto als = code_als.run();

    hpx::evaluate_active_counters(true, "start");
    hpx::util::high_resolution_timer t;
    auto result = als(
        ratings, regularization, num_factors, iterations, alpha, enable_output);
    auto time_diff = t.elapsed();
    std::cout << t.elapsed();

    hpx::evaluate_active_counters(true, " finish");

    auto result_r = phylanx::execution_tree::extract_list_value(result);
    auto it = result_r.begin();
    std::cout << "X: \n"
              << phylanx::execution_tree::extract_numeric_value(*it++)
              << "\nY: \n"
              << phylanx::execution_tree::extract_numeric_value(*it)
              << std::endl;

    return hpx::finalize();
}

int main(int argc, char* argv[])
{
    // command line handling
    boost::program_options::options_description desc("usage: als [options]");
    desc.add_options()
            ("enable_output,e", "enable progress output (default: false)")
            ("iterations,i",
             boost::program_options::value<std::int64_t>()->default_value(3),
             "number of iterations (default: 10.0)")
            ("factors,f",
             boost::program_options::value<std::int64_t>()->default_value(10),
             "number of factors (default: 10)")
            ("alpha,a",
             boost::program_options::value<double>()->default_value(40),
             "alpha (default: 40)")
            ("regularization,r",
             boost::program_options::value<double>()->default_value(0.1),
             "regularization (default: 0.1)")
            ("data_csv",
             boost::program_options::value<std::string>(),
             "file name for reading data")
            ("row_stop",
             boost::program_options::value<std::int64_t>()->default_value(10),
             "row_stop (default: 10)")
            ("col_stop",
             boost::program_options::value<std::int64_t>()->default_value(100),
             "col_stop (default: 100)")
            ;
    return hpx::init(desc, argc, argv);
}


//
//  Copyright (c) 2018-2019, Cem Bassoy, cem.bassoy@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer IOSB, Ettlingen, Germany
//

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

int main()
{
	using namespace boost::numeric::ublas;

	using format_t  = column_major;
	using value_t   = float;
	using tensor_t = tensor<value_t,format_t>;
	using matrix_t = matrix<value_t,format_t>;
	using namespace boost::numeric::ublas::index;

	// Tensor-Vector-Multiplications - Including Transposition
	{

		auto n = shape{3,4,2};
		auto A = tensor_t(n,1);
		auto B1 = matrix_t(n[1],n[2],2);
		auto v1 = tensor_t(shape{n[0],1},2);
		auto v2 = tensor_t(shape{n[1],1},2);
//		auto v3 = tensor_t(shape{n[2],1},2);

		// C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);
		// tensor_t C1 = B1 + prod(A,vector_t(n[0],1),1);
//		tensor_t C1 = B1 + A(_i,_,_) * v1(_i,_);

		// C2(i,k) = A(i,j,k)*v2(j) + 4;
		//tensor_t C2 = prod(A,vector_t(n[1],1),2) + 4;
//		tensor_t C2 = A(_,_i,_) * v2(_i,_) + 4;

		// not yet implemented!
		// C3() = A(i,j,k)*T1(i)*T2(j)*T2(k);
		// tensor_t C3 = prod(prod(prod(A,v1,1),v2,1),v3,1);
		// tensor_t C3 = A(_i,_j,_k) * v1(_i,_) * v2(_j,_) * v3(_k,_);

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(j,k) = B1(j,k) + A(i,j,k)*v1(i);" << std::endl << std::endl;
//		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C2(i,k) = A(i,j,k)*v2(j) + 4;" << std::endl << std::endl;
//		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

	}

	// Tensor-Matrix-Multiplications - Including Transposition
	{
		auto n = shape{3,4,2};
		auto m = 5u;
		auto A = tensor_t(n,2);
		auto B  = tensor_t(shape{n[1],n[2],m},2);
		auto B1 = tensor_t(shape{m,n[0]},1);
		auto B2 = tensor_t(shape{m,n[1]},1);

		// C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);
		// tensor_t C1 = B + prod(A,B1,1);
//		tensor_t C1 = B + A(_i,_,_) * B1(_,_i);

		// C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;
		// tensor_t C2 = prod(A,B2) + 4;
//		tensor_t C2 =  A(_,_j,_) * B2(_,_j) + 4;

		// C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);
		// not yet implemented.

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(l,j,k) = B(j,k,l) + A(i,j,k)*B1(l,i);" << std::endl << std::endl;
//		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C2(i,l,k) = A(i,j,k)*B2(l,j) + 4;" << std::endl << std::endl;
//		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

//		// formatted output
//		std::cout << "% --------------------------- " << std::endl;
//		std::cout << "% --------------------------- " << std::endl << std::endl;
//		std::cout << "% C3(i,l1,l2) = A(i,j,k)*T1(l1,j)*T2(l2,k);" << std::endl << std::endl;
//		std::cout << "C3=" << C3 << ";" << std::endl << std::endl;
	}

	// Tensor-Tensor-Multiplications Including Transposition
	{
		auto na = shape{3,4,5};
		auto nb = shape{4,6,3,2};
		auto A = tensor_t(na,2);
		auto B = tensor_t(nb,3);
		auto T1 = tensor_t(shape{na[2],na[2]},2);
		auto T2 = tensor_t(shape{na[2],nb[1],nb[3]},2);

		// C1(j,l) = T1(j,l) + A(i,j,k)*A(i,j,l) + 5;
		// tensor_t C1 = T1 + prod(A,A,perm_t{1,2}) + 5;
//		tensor_t C1 = T1 + A(_i,_j,_m)*A(_i,_j,_l) + 5;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "% C1(k,l) = T1(k,l) + A(i,j,k)*A(i,j,l) + 5;" << std::endl << std::endl;
//		std::cout << "C1=" << C1 << ";" << std::endl << std::endl;

		// C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;
		//tensor_t C2 = T2 + prod(A,B,perm_t{1,2},perm_t{3,1}) + 5;
//		tensor_t C2 = T2 + A(_i,_j,_k)*B(_j,_l,_i,_m) + 5;

		// formatted output
		std::cout << "% --------------------------- " << std::endl;
		std::cout << "% --------------------------- " << std::endl << std::endl;
		std::cout << "%  C2(k,l,m) = T2(k,l,m) + A(i,j,k)*B(j,l,i,m) + 5;" << std::endl << std::endl;
//		std::cout << "C2=" << C2 << ";" << std::endl << std::endl;

	}
}


/**
 * @author Alessandro Bianco
 */

/**
 * @addtogroup DFNs
 * @{
 */

#include "EssentialMatrixDecomposition.hpp"

#include <Errors/Assert.hpp>
#include <Macros/YamlcppMacros.hpp>

#include <opencv2/calib3d.hpp>
#include <Eigen/Geometry>

#include <stdlib.h>
#include <fstream>

using namespace PoseWrapper;
using namespace MatrixWrapper;
using namespace CorrespondenceMap2DWrapper;
using namespace Converters;
using namespace Helpers;

namespace CDFF
{
namespace DFN
{
namespace CamerasTransformEstimation
{

EssentialMatrixDecomposition::EssentialMatrixDecomposition()
{
	parameters = DEFAULT_PARAMETERS;

	parametersHelper.AddParameter<int>("GeneralParameters", "NumberOfTestPoints", parameters.numberOfTestPoints, DEFAULT_PARAMETERS.numberOfTestPoints);
	parametersHelper.AddParameter<double>("FirstCameraMatrix", "FocalLengthX", parameters.firstCameraMatrix.focalLengthX, DEFAULT_PARAMETERS.firstCameraMatrix.focalLengthX);
	parametersHelper.AddParameter<double>("FirstCameraMatrix", "FocalLengthY", parameters.firstCameraMatrix.focalLengthY, DEFAULT_PARAMETERS.firstCameraMatrix.focalLengthY);
	parametersHelper.AddParameter<double>("FirstCameraMatrix", "PrinciplePointX", parameters.firstCameraMatrix.principlePoint.x, DEFAULT_PARAMETERS.firstCameraMatrix.principlePoint.x);
	parametersHelper.AddParameter<double>("FirstCameraMatrix", "PrinciplePointY", parameters.firstCameraMatrix.principlePoint.y, DEFAULT_PARAMETERS.firstCameraMatrix.principlePoint.y);
	parametersHelper.AddParameter<double>("SecondCameraMatrix", "FocalLengthX", parameters.secondCameraMatrix.focalLengthX, DEFAULT_PARAMETERS.secondCameraMatrix.focalLengthX);
	parametersHelper.AddParameter<double>("SecondCameraMatrix", "FocalLengthY", parameters.secondCameraMatrix.focalLengthY, DEFAULT_PARAMETERS.secondCameraMatrix.focalLengthY);
	parametersHelper.AddParameter<double>("SecondCameraMatrix", "PrinciplePointX", parameters.secondCameraMatrix.principlePoint.x, DEFAULT_PARAMETERS.secondCameraMatrix.principlePoint.x);
	parametersHelper.AddParameter<double>("SecondCameraMatrix", "PrinciplePointY", parameters.secondCameraMatrix.principlePoint.y, DEFAULT_PARAMETERS.secondCameraMatrix.principlePoint.y);

	firstCameraMatrix = ConvertToMat(DEFAULT_PARAMETERS.firstCameraMatrix);
	secondCameraMatrix = ConvertToMat(DEFAULT_PARAMETERS.secondCameraMatrix);

	configurationFilePath = "";
}

EssentialMatrixDecomposition::~EssentialMatrixDecomposition()
{
}

void EssentialMatrixDecomposition::configure()
{
	parametersHelper.ReadFile(configurationFilePath);
	ValidateParameters();

	firstCameraMatrix = ConvertToMat(parameters.firstCameraMatrix);
	secondCameraMatrix = ConvertToMat(parameters.secondCameraMatrix);
}

void EssentialMatrixDecomposition::process()
{
	// Read data from input port
	cv::Mat correspondenceMap = Convert(&inMatches);
	cv::Mat fundamentalMatrix = ConvertToMat(&inFundamentalMatrix);

	// Process data
	ValidateInputs(fundamentalMatrix, correspondenceMap);
	std::vector<cv::Mat> transformsList = ComputeTransformMatrix(fundamentalMatrix);
	int validTransformIndex = FindValidTransform(transformsList, correspondenceMap);

	// Write data to output port
	if (validTransformIndex < 0)
	{
		// outTransform =  new Transform3D();
		outSuccess = false;
	}
	else
	{
		Pose3DConstPtr tmp = matToTransform3DConverter.Convert(transformsList.at(validTransformIndex));
		Copy(*tmp, outTransform);
		delete(tmp);
		outSuccess = true;
	}
}

const EssentialMatrixDecomposition::EssentialMatrixDecompositionOptionsSet EssentialMatrixDecomposition::DEFAULT_PARAMETERS =
{
	/*.numberOfTestPoints =*/ 20,
	//.firstCameraMatrix =
	{
		/*.focalLengthX =*/ 1.0,
		/*.focalLengthY =*/ 1.0,
		/*.principlePoint =*/ cv::Point2d(0, 0)
	},
	//.secondCameraMatrix =
	{
		/*.focalLengthX =*/ 1.0,
		/*.focalLengthY =*/ 1.0,
		/*.principlePoint =*/ cv::Point2d(0, 0)
	}
};

cv::Mat EssentialMatrixDecomposition::ConvertToMat(CameraMatrix cameraMatrix)
{
	cv::Mat conversion(3, 3, CV_64FC1, cv::Scalar(0));
	conversion.at<double>(0,0) = cameraMatrix.focalLengthX;
	conversion.at<double>(1,1) = cameraMatrix.focalLengthY;
	conversion.at<double>(0,2) = cameraMatrix.principlePoint.x;
	conversion.at<double>(1,2) = cameraMatrix.principlePoint.y;
	conversion.at<double>(2,2) = 1.0;
	return conversion;
}

cv::Mat EssentialMatrixDecomposition::ConvertToMat(MatrixWrapper::Matrix3dConstPtr matrix)
{
	cv::Mat cvMatrix(3, 3, CV_64FC1);
	for (unsigned row = 0; row < 3; row++)
	{
		for (unsigned column = 0; column < 3; column++)
		{
			cvMatrix.at<double>(row,column) = GetElement(*matrix, row, column);
		}
	}
	return cvMatrix;
}

cv::Mat EssentialMatrixDecomposition::Convert(CorrespondenceMap2DConstPtr correspondenceMap)
{
	cv::Mat cvCorrespondenceMap(4, GetNumberOfCorrespondences(*correspondenceMap), CV_64FC1);
	for (int correspondenceIndex = 0; correspondenceIndex < GetNumberOfCorrespondences(*correspondenceMap); correspondenceIndex++)
	{
		BaseTypesWrapper::Point2D firstPoint = GetSource(*correspondenceMap, correspondenceIndex);
		BaseTypesWrapper::Point2D secondPoint = GetSink(*correspondenceMap, correspondenceIndex);
		cvCorrespondenceMap.at<double>(0, correspondenceIndex) = firstPoint.x;
		cvCorrespondenceMap.at<double>(1, correspondenceIndex) = firstPoint.y;
		cvCorrespondenceMap.at<double>(2, correspondenceIndex) = secondPoint.x;
		cvCorrespondenceMap.at<double>(3, correspondenceIndex) = secondPoint.y;
	}
	return cvCorrespondenceMap;
}

std::vector<cv::Mat> EssentialMatrixDecomposition::ComputeTransformMatrix(cv::Mat fundamentalMatrix)
{
	cv::Mat essentialMatrix = secondCameraMatrix.t() * fundamentalMatrix * firstCameraMatrix;

	cv::Mat firstRotationMatrix, secondRotationMatrix, translationMatrix;
	cv::decomposeEssentialMat(essentialMatrix, firstRotationMatrix, secondRotationMatrix, translationMatrix);

	std::vector<cv::Mat> projectionMatricesList(4);
	cv::hconcat(firstRotationMatrix, translationMatrix, projectionMatricesList[0]);
	cv::hconcat(firstRotationMatrix, -translationMatrix, projectionMatricesList[1]);
	cv::hconcat(secondRotationMatrix, translationMatrix, projectionMatricesList[2]);
	cv::hconcat(secondRotationMatrix, -translationMatrix, projectionMatricesList[3]);

	return projectionMatricesList;
}

int EssentialMatrixDecomposition::FindValidTransform(std::vector<cv::Mat> projectionsList, cv::Mat correspondenceMap)
{
	static const float EPSILON = 1e-2;

	int validMatrixIndex = -1;
	bool validMatrixFound = false;
	for (unsigned matrixIndex = 0; matrixIndex < projectionsList.size(); matrixIndex++)
	{
		cv::Mat currentProjectionMatrix = projectionsList.at(matrixIndex);
		double rotationDeterminant = cv::determinant( currentProjectionMatrix( cv::Rect(0,0,2,2) ));
		bool orientationPreserved = std::abs(rotationDeterminant - 1) < EPSILON;

		bool matrixIsValid = orientationPreserved && ProjectionMatrixIsValidForTestPoints(currentProjectionMatrix, correspondenceMap);

		if (matrixIsValid)
		{
			if (validMatrixFound)
			{
				// There should only be one valid matrix. If multiple valid matrices are found, the decomposition has failed.
				return -1;
			}
			validMatrixFound = true;
			validMatrixIndex = matrixIndex;
		}
	}

	return validMatrixIndex;
}

//Projection test as described by described in Richard Hartley and Andrew Zisserman, in "Multiple View Geometry in Computer Vision".
bool EssentialMatrixDecomposition::ProjectionMatrixIsValidForTestPoints(cv::Mat projectionMatrix, cv::Mat correspondenceMap)
{
	static const float EPSILON = 1e-2;

	double rotationDeterminant = cv::determinant( projectionMatrix(cv::Rect(0,0,3,3)) );
	int rotationDeterminantSign = rotationDeterminant >= 0 ? 1 : -1;
	double principleRayX = projectionMatrix.at<double>(2, 0);
	double principleRayY = projectionMatrix.at<double>(2, 1);
	double principleRayZ = projectionMatrix.at<double>(2, 2);
	double principleRayNorm = std::sqrt (principleRayX*principleRayX + principleRayY*principleRayY + principleRayZ*principleRayZ);

	cv::Mat identityProjection(3, 4, CV_64FC1, cv::Scalar(0));
	identityProjection.at<double>(0,0) = 1;
	identityProjection.at<double>(1,1) = 1;
	identityProjection.at<double>(2,2) = 1;

	unsigned testPointsNumber = (correspondenceMap.cols > parameters.numberOfTestPoints) ? parameters.numberOfTestPoints : correspondenceMap.cols;
	cv::Mat testPointCloudMatrix;
	cv::triangulatePoints(
		firstCameraMatrix * identityProjection,
		secondCameraMatrix * projectionMatrix,
		correspondenceMap(cv::Rect(0, 0, testPointsNumber, 2 )),
		correspondenceMap(cv::Rect(0, 2, testPointsNumber, 2 )),
		testPointCloudMatrix
	);

	bool matrixIsValid = false;
	bool matrixIsInvalid = false;
	unsigned validPointsCount = 0;
	unsigned invalidPointsCount = 0;
	for (unsigned pointIndex = 0; pointIndex < testPointCloudMatrix.cols && !matrixIsValid && !matrixIsInvalid; pointIndex++)
	{
		cv::Mat testPoint = testPointCloudMatrix(cv::Rect(pointIndex, 0, 1, 4));
		cv::Mat projectedPoint = projectionMatrix * testPoint;
		float t = testPoint.at<double>(3, 0);
		float w = projectedPoint.at<double>(2, 0);
		double depth = (rotationDeterminantSign * w ) / (t * principleRayNorm);

		double scale = 4;
		cv::Mat scaledTestPoint = scale * testPoint;
		cv::Mat scaledProjectedPoint = projectionMatrix * scaledTestPoint;
		double scaledT = scaledTestPoint.at<double>(3, 0);
		double scaledW = scaledProjectedPoint.at<double>(2, 0);
		double scaledDepth = (rotationDeterminantSign * scaledW ) / (scaledT * principleRayNorm);

		bool validPoint = (depth < scaledDepth + EPSILON && depth > scaledDepth - EPSILON && depth > 0);
		if (validPoint)
		{
			validPointsCount++;
		}
		else
		{
			invalidPointsCount++;
		}
		matrixIsValid = validPointsCount >= testPointsNumber / 2;
		matrixIsInvalid = invalidPointsCount >= testPointsNumber / 2;
	}

	return matrixIsValid || !matrixIsInvalid;
}

void EssentialMatrixDecomposition::ValidateParameters()
{
	ASSERT(parameters.numberOfTestPoints > 1, "EssentialMatrixComputation Configuration Error: number of test points has to be positive and greater than 1");
	ASSERT(parameters.firstCameraMatrix.focalLengthX > 0 && parameters.firstCameraMatrix.focalLengthY > 0, "EssentialMatrixComputation Configuration Error: focalLength is not positive");
	ASSERT(parameters.secondCameraMatrix.focalLengthX > 0 && parameters.secondCameraMatrix.focalLengthY > 0, "EssentialMatrixComputation Configuration Error: focalLength is not positive");
}

void EssentialMatrixDecomposition::ValidateInputs(cv::Mat fundamentalMatrix, cv::Mat correspondenceMap)
{
	ASSERT(fundamentalMatrix.cols == 3 && fundamentalMatrix.rows == 3, "EssentialMatrixComputation Error, unexpected fundamental matrix size");
}

}
}
}

/** @} */