// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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

\brief radar_beam_height.cpp

\details
Demonstrate library usage for user test cases suggested by Michael Fawcett.

Output:
@verbatim

//[radar_beam_height_output
radar range        : 300 nmi
earth radius       : 6.37101e+06 m
beam height 1      : 18169.7 m
beam height 2      : 9.81085 nmi
beam height 3      : 18169.7 m
beam height 4      : 9.81085 nmi
beam height approx : 59488.4 ft
beam height approx : 18132.1 m
//]

@endverbatim
**/

#include <iostream>

#include <boost/units/conversion.hpp>
#include <boost/units/io.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/prefixes.hpp>

using boost::units::length_dimension;
using boost::units::pow;
using boost::units::root;
using boost::units::quantity;
using boost::units::unit;

//[radar_beam_height_class_snippet_1
namespace nautical {

struct length_base_unit :
    boost::units::base_unit<length_base_unit, length_dimension, 1>
{
    static std::string name()       { return "nautical mile"; }
    static std::string symbol()     { return "nmi"; }
};

typedef boost::units::make_system<length_base_unit>::type system;

/// unit typedefs
typedef unit<length_dimension,system>    length;

static const length mile,miles;

} // namespace nautical

// helper for conversions between nautical length and si length
BOOST_UNITS_DEFINE_CONVERSION_FACTOR(nautical::length_base_unit,
                                     boost::units::si::meter_base_unit,
                                     double, 1.852e3);
//]

//[radar_beam_height_class_snippet_2
namespace imperial {

struct length_base_unit :
    boost::units::base_unit<length_base_unit, length_dimension, 2>
{
    static std::string name()       { return "foot"; }
    static std::string symbol()     { return "ft"; }
};

typedef boost::units::make_system<length_base_unit>::type system;

/// unit typedefs
typedef unit<length_dimension,system>    length;

static const length foot,feet;

} // imperial

// helper for conversions between imperial length and si length
BOOST_UNITS_DEFINE_CONVERSION_FACTOR(imperial::length_base_unit,
                                     boost::units::si::meter_base_unit,
                                     double, 1.0/3.28083989501312);
//]

// radar beam height functions
//[radar_beam_height_function_snippet_1
template<class System,typename T>
constexpr
quantity<unit<boost::units::length_dimension,System>,T>
radar_beam_height(const quantity<unit<length_dimension,System>,T>& radar_range,
                  const quantity<unit<length_dimension,System>,T>& earth_radius,
                  T k = 4.0/3.0)
{
    return quantity<unit<length_dimension,System>,T>
        (pow<2>(radar_range)/(2.0*k*earth_radius));
}
//]

//[radar_beam_height_function_snippet_2
template<class return_type,class System1,class System2,typename T>
constexpr
return_type
radar_beam_height(const quantity<unit<length_dimension,System1>,T>& radar_range,
                  const quantity<unit<length_dimension,System2>,T>& earth_radius,
                  T k = 4.0/3.0)
{
    // need to decide which system to use for calculation
    return pow<2>(static_cast<return_type>(radar_range))
            / (2.0*k*static_cast<return_type>(earth_radius));
}
//]

//[radar_beam_height_function_snippet_3
constexpr
quantity<imperial::length>
radar_beam_height(const quantity<nautical::length>& range)
{
    return quantity<imperial::length>
        (pow<2>(range/(1.23*nautical::miles/root<2>(imperial::feet))));
}
//]

int main(void)
{
    using namespace boost::units;
    using namespace boost::units::si;
    using namespace nautical;

    //[radar_beam_height_snippet_1
    const quantity<nautical::length> radar_range(300.0*miles);
    const quantity<si::length>       earth_radius(6371.0087714*kilo*meters);

    const quantity<si::length>       beam_height_1(radar_beam_height(quantity<si::length>(radar_range),earth_radius));
    const quantity<nautical::length> beam_height_2(radar_beam_height(radar_range,quantity<nautical::length>(earth_radius)));
    const quantity<si::length>       beam_height_3(radar_beam_height< quantity<si::length> >(radar_range,earth_radius));
    const quantity<nautical::length> beam_height_4(radar_beam_height< quantity<nautical::length> >(radar_range,earth_radius));
    //]

    std::cout << "radar range        : " << radar_range << std::endl
              << "earth radius       : " << earth_radius << std::endl
              << "beam height 1      : " << beam_height_1 << std::endl
              << "beam height 2      : " << beam_height_2 << std::endl
              << "beam height 3      : " << beam_height_3 << std::endl
              << "beam height 4      : " << beam_height_4 << std::endl
              << "beam height approx : " << radar_beam_height(radar_range)
              << std::endl
              << "beam height approx : "
              << quantity<si::length>(radar_beam_height(radar_range))
              << std::endl << std::endl;

    return 0;
}


#include <cmath>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <sys/time.h>
#include <chrono>
#include <sys/resource.h>   // check the memory usage
#include <stdio.h>
#include <thread>
#include <fstream>
#include <sstream>

#include <NTL/RR.h>
#include <NTL/xdouble.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/BasicThreadPool.h>

#include "../src/CZZ.h"
#include "../src/Params.h"
#include "../src/PubKey.h"
#include "../src/Scheme.h"
#include "../src/SchemeAlgo.h"
#include "../src/SecKey.h"
#include "../src/TestScheme.h"
#include "../src/TimeUtils.h"
#include "../src/Ring2Utils.h"
#include "../src/StringUtils.h"
#include "../src/EvaluatorUtils.h"

#include "Database.h"
#include "LRtest.h"
#include "HELR.h"

using namespace NTL;
using namespace std;

int main(int Argc, char** Argv) {

    if(Argc != 2){
        cout << "-------------------------------------------------------------" << endl;
        cerr << "Enter the File and degree of approximation \t"  << "(e.g. $test edin.txt 3) \n ";
    }

    char* filename  =  Argv[1];

    dMat  zData;
    dMat* zTest = new dMat[5];
    dMat* zTrain = new dMat[5];

    int nLine= readData(zData, filename);

    cout << "Sample the learning and test data ..." << endl;
    cvRandomSamplingData(zTrain, zTest, zData, filename);

    //----------------------------------------------------------------
    // Parameters for Logistic regression
    //----------------------------------------------------------------

    long logN= 11;
    long logp= 28;
    long logl= 10;
    long logq, cBit1, cBit2;
    int max_iter;
    long dim ;

    dMat mtheta3_list;
    dMat mtheta7_list;
    dMat mtheta_sig_list;

    ofstream fout;
    fout.open("beta.txt");

    for(int k = 0; k < 5; ++k){

        cout << "-------------------------------------------------------------" << endl;
        cout << "HELR_3 " << endl;

        int polydeg = 3;  // degree of approximation polynomial

        struct LRpar LRparams;
        ReadLRparams(LRparams, max_iter, zTrain[0], polydeg, logp);

        dVec mtheta3(LRparams.dim1, 0.0);
        for(int i= 0; i< LRparams.max_iter; i++){
            LR_poly(mtheta3, zTrain[k], LRparams);
        }

        for(int i= 0; i< LRparams.dim1; i++)
            cout << "[" << mtheta3[i] << "] " ;
        cout  << endl;

        getAUC(mtheta3, zTest[k]);
        mtheta3_list.push_back(mtheta3);

        cout << "-------------------------------------------------------------" << endl;
        cout << "HELR_7 " << endl;

        polydeg = 7;  // degree of approximation polynomial

        struct LRpar LRparams7;
        ReadLRparams(LRparams7, max_iter, zTrain[0], polydeg, logp);

        dVec mtheta7(LRparams7.dim1, 0.0);
        for(int i= 0; i< LRparams7.max_iter; i++){
            LR_poly(mtheta7, zTrain[k], LRparams7);
        }

        for(int i= 0; i< LRparams7.dim1; i++)
            cout << "[" << mtheta7[i] << "] " ;
        cout  << endl;

        getAUC(mtheta7, zTest[k]);
        mtheta7_list.push_back(mtheta7);

        dim = LRparams7.dim1;

        cout << "-------------------------------------------------------------" << endl;
        cout << "sigmoid LR " << endl;
        dVec mtheta_sig(LRparams.dim1, 0.0);

        for(int i= 0; i< LRparams.max_iter; i++){
            LR_sigmoid(mtheta_sig, zTrain[k], LRparams);
        }

        for(int i= 0; i< LRparams.dim1; i++)
            cout << "[" << mtheta_sig[i] << "] " ;
        cout  << endl;

        getAUC(mtheta_sig, zTest[k]);
        mtheta_sig_list.push_back(mtheta_sig);

        cout << "-------------------------------------------------------------" << endl;
        cout << "MSE (HELR/non-HELR): " << getMSE(mtheta3, mtheta_sig) << endl;
        cout << "MSE (HELR/non-HELR): " << getMSE(mtheta7, mtheta_sig) << endl;

    }

    //! write the beta results in the text file
    //fout << "-------------------------------------------------------------" << endl;
    //fout << "HELR_3" << endl;
    for(int i = 0; i < dim; ++i){
        for(int k = 0; k < 4; ++k){
            fout << mtheta3_list[k][i] << "," ;
        }
        fout << mtheta3_list[4][i] << ";" << endl;
    }

    //fout << "-------------------------------------------------------------" << endl;
    //fout << "HELR_7" << endl;
    for(int i = 0; i < dim; ++i){
        for(int k = 0; k < 4; ++k){
            fout << mtheta7_list[k][i] << "," ;
        }
        fout << mtheta7_list[4][i] << ";" << endl;
    }

    //fout << "-------------------------------------------------------------" << endl;
    //fout << "LR" << endl;
    for(int i = 0; i < dim; ++i){
        for(int k = 0; k < 4; ++k){
            fout << mtheta_sig_list[k][i] << "," ;
        }
        fout << mtheta_sig_list[4][i] << ";" << endl;
    }

    fout.close();

    delete[] zTest;
    delete[] zTrain;

    return 0;
}


/*
 * StereoCalib.cpp
 *
 *  Created on: 3 Jan 2015
 *      Author: link
 */

#include <iostream>
#include <list>

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <StorageFunctions.hpp>
#include <boost/program_options.hpp>

#include "ConvenienceFunctions.hpp"

const int USER_TRIGGERED_EXIT = 0;

void parseCommandline(const int& argc,
                      char** argv,
                      std::string& input,
                      std::string& output)
{

  boost::program_options::options_description desc;

  desc.add_options()("help,h", "this help message")(
    "input,i",
    boost::program_options::value<std::string>(),
    "Input directory containing imageList.xml")(
    "output,o",
    boost::program_options::value<std::string>(),
    "Output file with calibration values for cameras")(
    "length,l",
    boost::program_options::value<double>(),
    "Legth of side of a single chessboard field in chosen units."
    "These units will be used later as measure unit for all calculations");
  boost::program_options::variables_map vm;
  boost::program_options::store(
    boost::program_options::parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help"))
  {
    std::cout << desc << std::endl;
    throw USER_TRIGGERED_EXIT;
  }

  if (vm.count("output"))
  {
    output = vm["output"].as<std::string>();
  }

  if (vm.count("input"))
  {
    input = vm["input"].as<std::string>();
  }
}

bool getCorners(const cv::Mat& image,
                const cv::Size& chessboardSize,
                std::vector<cv::Point2f>& corners)
{

  if (cv::findChessboardCorners(image, chessboardSize, corners, 0))
  {
    cv::cornerSubPix(
      image,
      corners,
      cv::Size(11, 11),
      cv::Size(-1, -1),
      cv::TermCriteria(
        cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
    return true;
  }
  else
  {
    return false;
  }
}

int main(int argc, char** argv)
{
  std::string input, output;
  double sideLength;

  parseCommandline(argc, argv, input, output);

  cv::Size imageSize, chessboardSize;
  std::list<std::pair<cv::Mat, cv::Mat>> imageList;

  loadImageList(input, imageSize, chessboardSize, sideLength, imageList);

  std::vector<std::vector<cv::Point3f>> modelPoints(1);

  for (int i = 0; i < chessboardSize.height; ++i)
  {
    for (int j = 0; j < chessboardSize.width; ++j)
    {
      modelPoints[0].push_back(sideLength * cv::Point3f(j, i, 0));
    }
  }

  std::vector<std::vector<cv::Point2f>> leftPoints, rightPoints;
  {
    int i = 0;
    for (auto it = imageList.begin(); imageList.end() != it; ++it, ++i)
    {
      std::pair<cv::Mat, cv::Mat> pair = *it;
      std::cerr << i << std::endl;
      std::vector<cv::Point2f> lP, rP;
      if (getCorners(pair.first, chessboardSize, lP) &&
          getCorners(pair.second, chessboardSize, rP))
      {
        leftPoints.push_back(lP);
        rightPoints.push_back(rP);
      }
      else
      {
        it = imageList.erase(it);
        continue;
      }
    }
  }

  modelPoints.resize(leftPoints.size(), modelPoints[0]);

  cv::Mat lCM, rCM, lDC, rDC, R, T, E, F;

  std::vector<cv::Mat> rvecs, tvecs;

  double leftReprojectionError = cv::calibrateCamera(
    modelPoints, leftPoints, imageSize, lCM, lDC, rvecs, tvecs);
  rvecs.clear();
  tvecs.clear();
  double rightReprojectionError = cv::calibrateCamera(
    modelPoints, rightPoints, imageSize, rCM, rDC, rvecs, tvecs);

  std::cerr << "left rms" << leftReprojectionError << std::endl;
  std::cerr << "right rms" << rightReprojectionError << std::endl;

  double reprojectionError = cv::stereoCalibrate(
    modelPoints,
    leftPoints,
    rightPoints,
    lCM,
    lDC,
    rCM,
    rDC,
    imageSize,
    R,
    T,
    E,
    F,
    cv::CALIB_FIX_INTRINSIC,
    cv::TermCriteria(
      cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, 1e-6));
  std::cerr << reprojectionError << std::endl;

  saveCalibParameters(output,
                      lCM,
                      rCM,
                      lDC,
                      rDC,
                      R,
                      T,
                      E,
                      F,
                      chessboardSize,
                      imageSize,
                      sideLength);
}


#pragma once

#include <boost/assert.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>

namespace polar {

namespace detail {

template<class Matrix>
bool invert(Matrix const& input, Matrix& inverse)
{
    using namespace boost::numeric::ublas;

    typedef permutation_matrix<std::size_t> pmatrix;

    BOOST_ASSERT(input.size1() == input.size2());

    Matrix A(input);
    pmatrix pm(A.size1());

    if (lu_factorize(A, pm) != 0)
        return false;

    inverse.assign(identity_matrix<typename Matrix::value_type>(A.size1()));
    lu_substitute(A, pm, inverse);

    return true;
}

} // namespace detail

template<typename Matrix_in, typename Matrix_out>
void polar_decomposition(Matrix_in const& A, Matrix_out& U, Matrix_out& H,
        double rtol = 1.0e-8, std::size_t max_iter = 0)
{
    typedef typename Matrix_out::value_type value_type;

    BOOST_ASSERT(A.size1() == A.size2());

    U.resize(A.size1(), A.size2());
    U.assign(A);
    H.resize(A.size1(), A.size2());
    Matrix_out X(A.size1(), A.size2());

    std::size_t count = 0;
    bool close_to_convergence = false;

    while (true)
    {
        X.assign(U);
        detail::invert(X, H);
        value_type gamma = 1.0;

        if (!close_to_convergence)
        {
            value_type alpha = std::sqrt(norm_1(X) * norm_inf(X));
            value_type beta  = std::sqrt(norm_1(H) * norm_inf(H));
            gamma = std::sqrt(beta / alpha);
        }

        U.assign(0.5 * (gamma * X + 1.0 / gamma * trans(H)));

        if (norm_1(X - U) < rtol * norm_1(X))
            break;

        if (max_iter != 0 && ++count == max_iter)
            break;

        if (!close_to_convergence && norm_frobenius(X - U) < 1.0e-2)
            close_to_convergence = true;
    }

    H.assign(0.5 * (prod(trans(U), A) + prod(trans(A), U)));
}

} // namespace polar


//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Core>
#include "_hypre_utilities.h"
#include "HYPRE_krylov.h"
#include "HYPRE.h"
#include "HYPRE_parcsr_ls.h"
#include "general_functions.hpp"
#include "class.hpp"

FRACTIONAL_STEP_1::FRACTIONAL_STEP_1(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, vector<bool> &u_dirichlet_flag1, vector<bool> &v_dirichlet_flag1, vector<bool> &p_dirichlet_flag1, int temporal_order1)
{
    temporal_order = temporal_order1;
    u_dirichlet_flag = u_dirichlet_flag1, v_dirichlet_flag = v_dirichlet_flag1, p_dirichlet_flag = p_dirichlet_flag1;
    check_bc(points, parameters);
    clock_t clock_t1 = clock(), clock_t2 = clock();
    solver_p.init(points, cloud, parameters, p_dirichlet_flag, 0.0, 0.0, 1.0, true);
    parameters.factoring_timer = ((double)(clock() - clock_t1)) / CLOCKS_PER_SEC;

    zero_vector = Eigen::VectorXd::Zero(points.nv);
    zero_vector_1 = Eigen::VectorXd::Zero(points.nv + 1);
    uh = zero_vector, vh = zero_vector;

    p_bc_full_neumann = true;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv] && p_dirichlet_flag[iv])
        { //boundary point found with dirichlet BC
            p_bc_full_neumann = false;
            break;
        }

    if (p_bc_full_neumann)
        p_source = zero_vector_1; //this is full Neumann for pressure
    else
        p_source = zero_vector;
    u_source_old = zero_vector, v_source_old = zero_vector;
    if (temporal_order == 2)
        u_source_old_old = zero_vector, v_source_old_old = zero_vector;
}

void FRACTIONAL_STEP_1::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, int it1)
{
    it = it1;
    if (p_source.size() != p_new.size())
        p_new = Eigen::VectorXd::Zero(p_source.size());
    if (p_source.size() != p_old.size())
        p_old = Eigen::VectorXd::Zero(p_source.size());
    calc_vel_hat(points, parameters, u_old, v_old, p_old);
    calc_pressure(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
    calc_vel_corr(points, cloud, parameters, u_new, v_new, p_new, u_old, v_old);
    extras(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
}

void FRACTIONAL_STEP_1::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, Eigen::VectorXd &body_force_x, Eigen::VectorXd &body_force_y, int it1)
{
    it = it1;
    if (p_source.size() != p_new.size())
        p_new = Eigen::VectorXd::Zero(p_source.size());
    if (p_source.size() != p_old.size())
        p_old = Eigen::VectorXd::Zero(p_source.size());
    calc_vel_hat(points, parameters, u_old, v_old, p_old, body_force_x, body_force_y);
    calc_pressure(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
    calc_vel_corr(points, cloud, parameters, u_new, v_new, p_new, u_old, v_old);
    extras(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
}

void FRACTIONAL_STEP_1::check_bc(POINTS &points, PARAMETERS &parameters)
{
    int u_dirichlet_flag_sum = accumulate(u_dirichlet_flag.begin(), u_dirichlet_flag.end(), 0);
    if (u_dirichlet_flag_sum == 0)
    {
        printf("\n\nERROR from FRACTIONAL_STEP_1::check_bc Setting u_dirichlet_flag to full Neumann BC is not permitted; sum of u_dirichlet_flag: %i\n\n", u_dirichlet_flag_sum);
        throw bad_exception();
    }
    int v_dirichlet_flag_sum = accumulate(v_dirichlet_flag.begin(), v_dirichlet_flag.end(), 0);
    if (v_dirichlet_flag_sum == 0)
    {
        printf("\n\nERROR from FRACTIONAL_STEP_1::check_bc Setting v_dirichlet_flag to full Neumann BC is not permitted; sum of v_dirichlet_flag: %i\n\n", v_dirichlet_flag_sum);
        throw bad_exception();
    }
    if (parameters.rho < 0 || parameters.mu < 0)
    {
        printf("\n\nERROR from FRACTIONAL_STEP_1::check_bc Some parameters are not set; parameters.rho: %g, parameters.mu: %g\n\n", parameters.rho, parameters.mu);
        throw bad_exception();
    }
    if (temporal_order != 1 && temporal_order != 2)
    {
        printf("\n\nERROR from FRACTIONAL_STEP_1::check_bc temporal_order should be either '1' or '2'; current value: %i\n\n", temporal_order);
        throw bad_exception();
    }
}

void FRACTIONAL_STEP_1::calc_vel_hat(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, Eigen::VectorXd &body_force_x, Eigen::VectorXd &body_force_y)
{
    u_source_old = (parameters.rho * (-u_old.cwiseProduct(points.grad_x_matrix_EIGEN * u_old) - v_old.cwiseProduct(points.grad_y_matrix_EIGEN * u_old))) + (parameters.mu * points.laplacian_matrix_EIGEN * u_old) + body_force_x;
    v_source_old = (parameters.rho * (-u_old.cwiseProduct(points.grad_x_matrix_EIGEN * v_old) - v_old.cwiseProduct(points.grad_y_matrix_EIGEN * v_old))) + (parameters.mu * points.laplacian_matrix_EIGEN * v_old) + body_force_y;
    if (temporal_order == 1 || it == 0)
    { //Euler method for first timestep of multistep method
        uh = u_old + ((parameters.dt / parameters.rho) * u_source_old);
        vh = v_old + ((parameters.dt / parameters.rho) * v_source_old);
    }
    else
    { //Second order Adam-Bashforth for subsequent timesteps
        uh = u_old + ((parameters.dt / parameters.rho) * (1.5 * u_source_old - 0.5 * u_source_old_old));
        vh = v_old + ((parameters.dt / parameters.rho) * (1.5 * v_source_old - 0.5 * v_source_old_old));
    }
}

void FRACTIONAL_STEP_1::calc_vel_hat(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    u_source_old = (parameters.rho * (-u_old.cwiseProduct(points.grad_x_matrix_EIGEN * u_old) - v_old.cwiseProduct(points.grad_y_matrix_EIGEN * u_old))) + (parameters.mu * points.laplacian_matrix_EIGEN * u_old);
    v_source_old = (parameters.rho * (-u_old.cwiseProduct(points.grad_x_matrix_EIGEN * v_old) - v_old.cwiseProduct(points.grad_y_matrix_EIGEN * v_old))) + (parameters.mu * points.laplacian_matrix_EIGEN * v_old);
    if (temporal_order == 1 || it == 0)
    { //Euler method for first timestep of multistep method
        uh = u_old + ((parameters.dt / parameters.rho) * u_source_old);
        vh = v_old + ((parameters.dt / parameters.rho) * v_source_old);
    }
    else
    { //Second order Adam-Bashforth
        uh = u_old + ((parameters.dt / parameters.rho) * (1.5 * u_source_old - 0.5 * u_source_old_old));
        vh = v_old + ((parameters.dt / parameters.rho) * (1.5 * v_source_old - 0.5 * v_source_old_old));
    }
}

void FRACTIONAL_STEP_1::calc_pressure(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    int dim = parameters.dimension;
    p_source.head(points.nv) = ((points.grad_x_matrix_EIGEN_internal * uh) + (points.grad_y_matrix_EIGEN_internal * vh)) * parameters.rho / parameters.dt;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        {
            if (p_dirichlet_flag[iv]) //dirichlet BC
                p_source[iv] = p_old[iv];
            else //normal momentum
                p_source[iv] = -parameters.rho * ((u_old[iv] - uh[iv]) * points.normal[dim * iv] + (v_old[iv] - vh[iv]) * points.normal[dim * iv + 1]) / parameters.dt;
        }
    if (p_source.rows() == points.nv + 1)
        p_source[points.nv] = 0.0;
    solver_p.general_solve(points, parameters, p_new, p_old, p_source);
}

void FRACTIONAL_STEP_1::calc_vel_corr(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old)
{
    u_new = uh - (parameters.dt * (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv)) / parameters.rho);
    v_new = vh - (parameters.dt * (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv)) / parameters.rho);
    double diag_coeff, rhs, off_diag_coeff;
    int ivnb, dim = parameters.dimension;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        {
            if (!p_dirichlet_flag[iv])
            { //for dirichlet pressure, u_new comes from velocity correction above
                if (u_dirichlet_flag[iv])
                    u_new[iv] = u_old[iv]; //dirichlet BC
                else
                { //neumann BC
                    rhs = 0.0;
                    for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                    {
                        ivnb = cloud.nb_points_col[i1];
                        if (iv == ivnb)
                            diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                        else
                        {
                            off_diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                            rhs = rhs - (off_diag_coeff * u_new[ivnb]);
                        }
                    }
                    u_new[iv] = (rhs / diag_coeff);
                }
            }
        }
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        {
            if (!p_dirichlet_flag[iv])
            { //for dirichlet pressure, v_new comes from velocity correction above
                if (v_dirichlet_flag[iv])
                    v_new[iv] = v_old[iv]; //dirichlet BC
                else
                { //neumann BC
                    rhs = 0.0;
                    for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                    {
                        ivnb = cloud.nb_points_col[i1];
                        if (iv == ivnb)
                            diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                        else
                        {
                            off_diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                            rhs = rhs - (off_diag_coeff * v_new[ivnb]);
                        }
                    }
                    v_new[iv] = (rhs / diag_coeff);
                }
            }
        }
}

void FRACTIONAL_STEP_1::extras(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    // double total_steady_err, max_err, l1_err;
    // calc_max_l1_error(u_old, u_new, max_err, l1_err);
    // total_steady_err = l1_err / parameters.dt;
    // calc_max_l1_error(v_old, v_new, max_err, l1_err);
    // total_steady_err += l1_err / parameters.dt;
    if (temporal_order == 2)
        u_source_old_old = u_source_old, v_source_old_old = v_source_old;
    // parameters.steady_error_log.push_back(total_steady_err);
    // return total_steady_err;
}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/**
 * @file fft.cpp - computing the canonical embedding and related norms
 **/
#include <complex>
#include <cmath>
#include <numeric> // std::accumulate
#include <NTL/BasicThreadPool.h>
#include "NumbTh.h"
#include "timing.h"
#include "norms.h"
#include "PAlgebra.h"
NTL_CLIENT

const double pi = 4 * std::atan(1);

#ifdef FFT_ARMA
#warning "canonicalEmbedding implemented via Armadillo"
#include <armadillo>
void convert(zzX& to, const arma::vec& from)
{
  to.SetLength(from.size());
  NTL_EXEC_RANGE(to.length(), first, last)
  for (long i=first; i<last; i++)
    to[i] = std::round(from[i]);
  NTL_EXEC_RANGE_END
}

void convert(arma::vec& to, const zzX& from)
{
  to.resize(from.length());
  NTL_EXEC_RANGE(from.length(), first, last)
  for (long i=first; i<last; i++)
    to[i] = from[i];
  NTL_EXEC_RANGE_END
}

void convert(arma::vec& to, const ZZX& from)
{
  to.resize(from.rep.length());
  NTL_EXEC_RANGE(from.rep.length(), first, last)
  for (long i=first; i<last; i++) {
    double x = conv<double>(from[i]);
    to[i] = x;
  }
  NTL_EXEC_RANGE_END
}

#if 0

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

namespace arma {

  template<>
  struct is_supported_elem_type<RR> {
    enum {value = 1};
  };

  template<>
  struct is_supported_elem_type<cx_RR> {
    enum {value = 1};
  };

  template<>
  struct is_real<RR> {
    enum {value = 1};
  };

}

void convert(zzX& to, const arma::Col<RR>& from)
{
  to.SetLength(from.size());
  for (long i: range(to.length()))
    to[i] = conv<long>(RoundToZZ(from[i]));
}

void convert(arma::Col<RR>& to, const zzX& from)
{
  to.resize(from.length());
  for (long i: range(from.length()))
    to[i] = from[i];
}

void convert(arma::Col<RR>& to, const ZZX& from)
{
  to.resize(from.rep.length());
  for (long i: range(from.rep.length())) {
    RR x = conv<RR>(from[i]);
    to[i] = x;
  }
}

#endif

// Computing the canonical embedding. This function returns in v only
// the first half of the entries, the others are v[phi(m)-i]=conj(v[i])
void canonicalEmbedding(std::vector<cx_double>& v,
                        const zzX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  arma::vec av; // convert to vector of doubles
  convert(av, f);
  arma::cx_vec avv = arma::fft(av,m); // compute the full FFT

  v.resize(phimBy2); // the first half of Zm*

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      v[phimBy2-i-1] = avv[palg.ith_rep(i)];
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) v[idx++] = avv[i];
}

void canonicalEmbedding(std::vector<cx_double>& v,
                        const ZZX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  arma::vec av; // convert to vector of doubles
  convert(av, f);
  arma::cx_vec avv = arma::fft(av,m); // compute the full FFT

  v.resize(phimBy2); // the first half of Zm*

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      v[phimBy2-i-1] = avv[palg.ith_rep(i)];
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) v[idx++] = avv[i];
}

#if 0
void canonicalEmbedding(std::vector<cx_RR>& v,
                        const ZZX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  arma::Col<RR> av; // convert to vector of doubles
  convert(av, f);
  arma::Col<cx_RR> avv = arma::fft(av,m); // compute the full FFT

  v.resize(phimBy2); // the first half of Zm*

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      v[phimBy2-i-1] = avv[palg.ith_rep(i)];
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) v[idx++] = avv[i];
}
#endif

// Roughly the inverse of canonicalEmbedding, except for scaling and
// rounding issues. Calling embedInSlots(f,v,palg,1.0,strictInverse=true)
// after setting canonicalEmbedding(v, f, palg), is sure to recover the
// same f, but embedInSlots(f,v,palg,1.0,strictInverse=false) may return
// a different "nearby" f.
void embedInSlots(zzX& f, const std::vector<cx_double>& v,
                  const PAlgebra& palg, double scaling, bool strictInverse)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  arma::cx_vec avv(m);
  for (auto& x: avv) x = 0.0;

  if (palg.getNSlots()==phimBy2) // roots ordered by the palg order
    for (long i=0; i<palg.getNSlots(); i++) {
      long j = palg.ith_rep(i);
      long ii = palg.getNSlots()-i-1;
      if (ii < lsize(v)) {
        avv[j] = scaling*v[ii];
        avv[m-j] = std::conj(avv[j]);
      }
    }
  else                           // roots ordered sequentially
    for (long i=1, idx=0; i<=m/2 && idx<lsize(v); i++) {
      if (palg.inZmStar(i)) {
        avv[i] = scaling*v[idx++];
        avv[m-i] = std::conj(avv[i]);
      }
    }
  arma::vec av = arma::real(arma::ifft(avv,m)); // compute the inverse FFT

  // If v was obtained by canonicalEmbedding(v,f,palg,1.0) then we have
  // the guarantee that m*av is an integral polynomial, and moreover
  // m*av mod Phi_m(x) is in m*Z[X].
  if (strictInverse) av *= m; // scale up by m
  convert(f, av);    // round to an integer polynomial
  reduceModPhimX(f, palg);
  if (strictInverse) f /= m;  // scale down by m
  normalize(f);
}
#else
#ifdef FFT_NATIVE
#warning "canonicalEmbedding implemented via slow DFT, expect very slow key-generation"
// An extremely lame implementation of the canonical embedding

// evaluate poly(x) using Horner's rule
cx_double complexEvalPoly(const zzX& poly, const cx_double& x)
{
  if (lsize(poly)<=0) return cx_double(0.0,0.0);
  cx_double res(double(poly[0]), 0.0);
  for (long i=1; i<lsize(poly); i++) {
    res *= x;
    res += cx_double(double(poly[i]));
  }
  return res;
}

void canonicalEmbedding(std::vector<cx_double>& v, const zzX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  vector<long> zmstar(phimBy2); // the first half of Zm*

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      zmstar[phimBy2-i-1] = palg.ith_rep(i);
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) zmstar[idx++] = i;

  v.resize(phimBy2);
  NTL_EXEC_RANGE(phimBy2, first, last)
  for (long i=first; i < last; ++i) {
    auto rou = std::polar<double>(1.0, -(2*pi*zmstar[i])/m); // root of unity
    v[i] = complexEvalPoly(f,rou);
  }
  NTL_EXEC_RANGE_END
  FHE_TIMER_STOP;
}

// evaluate poly(x) using Horner's rule
// FIXME: this is actually evaluating the reverse polynomial
cx_double complexEvalPoly(const Vec<double>& poly, const cx_double& x)
{
  if (poly.length()<=0) return cx_double(0.0,0.0);
  cx_double res(poly[0], 0.0);
  for (long i: range(1, poly.length())) {
    res *= x;
    res += cx_double(poly[i]);
  }
  return res;
}

void canonicalEmbedding(std::vector<cx_double>& v, const ZZX& f, const PAlgebra& palg)
{
  FHE_TIMER_START;
  long m = palg.getM();
  long phimBy2 = divc(palg.getPhiM(),2);
  vector<long> zmstar(phimBy2); // the first half of Zm*

  Vec<double> ff;
  conv(ff, f.rep);

  if (palg.getNSlots()==phimBy2) // order roots by the palg order
    for (long i=0; i<phimBy2; i++)
      zmstar[phimBy2-i-1] = palg.ith_rep(i);
  else                           // order roots sequentially
    for (long i=1, idx=0; i<=m/2; i++)
      if (palg.inZmStar(i)) zmstar[idx++] = i;

  v.resize(phimBy2);
  NTL_EXEC_RANGE(phimBy2, first, last)
  for (long i=first; i < last; ++i) {
    auto rou = std::polar<double>(1.0, -(2*pi*zmstar[i])/m); // root of unity
    v[i] = complexEvalPoly(ff,rou);
  }
  NTL_EXEC_RANGE_END
  FHE_TIMER_STOP;
}

void embedInSlots(zzX& f, const std::vector<cx_double>& v,
                  const PAlgebra& palg, double scaling, bool strictInverse)
{
  NTL::Error("embedInSlots not implemented\n");
}
#endif // ifdef FFT_NATIVE
#endif // ifdef FFT_ARMA


//
//  ShamirGenerator.cpp
//  edgeRuntime
//
//  Created by Abdelrahaman Aly on 20/11/13.
//  Copyright (c) 2013 Abdelrahaman Aly. All rights reserved.
//

//Generic Headers
#include <iostream>

//Library Headers
#include <NTL/ZZ_p.h>
#include <NTL/ZZ.h>

//Custom Headers
#include "List.h"
#include "Constants.h"
#include "StandardPlayer.h"
#include "StandardShare.h"
#include "ShamirGenerator.h"

using namespace NTL;
namespace ShareGenerators
{
    //Method implementation of ShamirGenerator

    //Constructor
    ShareGenerators::ShamirGenerator::ShamirGenerator(Players::StandardPlayer * player, long p)
    {
        //Initialize the Random number generator's seed using the system clocks
        srand((unsigned)time(0));
        //Initialize variables
        player_=player;
        this->p_=p;
    };

    //Destructor
    ShareGenerators::ShamirGenerator::~ShamirGenerator(){

    };

    //generate the shares
    int ShamirGenerator::generateShares(long secret, int players, Utils::List<Shares::StandardShare> *list)
    {
        //Gets the slope for the equation randomically
        ZZ_p slope= conv<ZZ_p>(rand()% this->p_-1);
        //TODO: The process to obtain the shares should be based on the T-1 paradigm.
        // hence the construction of the shares will depend on the number of players. there is a formula to do that to build polinomials of n deegree.
        for (ZZ i=conv<ZZ>(0); i< players;i++)
        {
            //creates the share TODO: take the process to the constructor
            Shares::StandardShare * aux =new Shares::StandardShare();

            aux->setPlayerId(player_->getPlayer());
            aux->setValue(conv<long>(slope*(conv<ZZ_p>(i)+ conv<ZZ_p>(1)) +conv<ZZ_p>(secret) ));

            list->add(aux);

        }

        return 1;

    };
    //generates the multiplication Regenerations
     int ShamirGenerator::multiplicationRegeneration(Shares::StandardShare * share, int players,Utils::List<Shares::StandardShare> * list)
    {
        //TODO: Add modulus operation that is why is here
        //TODO: modify this to make it authomatic in the number of players maybe slower though
        ZZ_p result=3*(conv<ZZ_p>(list->get(0)->getValue())) -3*conv<ZZ_p>((list->get(1)->getValue()))+conv<ZZ_p>((list->get(2)->getValue()));
        long value= conv<long>(result);
        share->setValue(value);
        return  1;
    };

}


#include <Eigen/Dense>
#include <math.h>
#include <optimization.h>

namespace al = alglib;

class LieMPC {

  public:
  Eigen::Matrix<double, 18, 1> x; // State vector
  Eigen::Matrix<double, 6, 1> y;  // Output vector
  Eigen::Vector4d u;              // Control input vector
  Eigen::Vector4d u_lin;          // Linearization control input
  Eigen::VectorXd Y;              // Target trajectory

  private:
  // MPC parameters
  int _nx = 12; // Number of states
  int _ny = 6;  // Number of outputs
  int _nu = 4;  // Number of control inputs
  int _np;      // Prediction steps
  double _ts;   // Time step (s)

  // Linear system Matrices
  Eigen::Matrix<double, 12, 12> _Ac = Eigen::Matrix<double, 12, 12>::Zero(); // Continuous state matrix
  Eigen::Matrix<double, 12, 4> _Bc = Eigen::Matrix<double, 12, 4>::Zero();   // Continuous input matrix
  Eigen::Matrix<double, 12, 4> _Bcg;                                         // Input matrix with direct allocation
  Eigen::Matrix<double, 12, 1> _f;                                           // Constant offset matrix
  Eigen::Matrix<double, 12, 1> _f_t;                                         // Time-variant offset matrix
  Eigen::Matrix<double, 12, 12> _A;                                          // Discrete state matrix
  Eigen::Matrix<double, 12, 4> _B;                                           // Discrete input matrix
  Eigen::Matrix<double, 6, 12> _C = Eigen::Matrix<double, 6, 12>::Zero();    // Discrete output matrix

  // Misc matrix definitions
  Eigen::Matrix<double, 4, 4> _Gamma;               // Control allocation matrix
  Eigen::Matrix3d _I = Eigen::Matrix3d::Identity(); // 3x3 identity matrix
  Eigen::Vector3d _1_3 = Eigen::Vector3d(0, 0, 1);  // Z-axis vector
  Eigen::Matrix3d _J = Eigen::Matrix3d::Zero();     // Inertia matrix (kg.m^2)
  Eigen::Matrix3d _J_inv;                           // Inverse of inertia matrix

  // Partial state matrices
  Eigen::Matrix3d _A_rv = _I;
  Eigen::Matrix3d _A_vp;
  Eigen::Matrix3d _A_pp;
  Eigen::Matrix3d _A_pw = _I;
  Eigen::Matrix3d _A_ww;

  // Partial input matrices
  Eigen::Vector3d _B_vt;
  Eigen::Matrix3d _B_wm;

  // Partial offset matrices
  Eigen::Vector3d _f_r;
  Eigen::Vector3d _f_v;
  Eigen::Vector3d _f_p;
  Eigen::Vector3d _f_p_t;
  Eigen::Vector3d _f_w;

  // MPC Matrices
  Eigen::MatrixXd _H;
  Eigen::VectorXd _F;
  Eigen::MatrixXd _H_f;
  Eigen::VectorXd _F_f;
  Eigen::MatrixXd _G;
  Eigen::MatrixXd _C_bar;
  Eigen::VectorXd _dY;
  Eigen::VectorXd _U_lin;

  // Optimization variables
  al::real_1d_array _U;             // Control input prediction
  al::real_1d_array _U0;            // Initial control input
  al::real_1d_array _Uc;            // Intermediate control input sequence
  al::real_2d_array _MPC_H;         // Quadratic program H matrix
  al::real_1d_array _MPC_F;         // Quadratic program F matrix
  Eigen::MatrixXd _MPC_H_Eig;       // Eigen Quadratic program H matrix
  Eigen::MatrixXd _MPC_H_Eig_Sym;   // Eigen Symmetric Quadratic program H matrix
  Eigen::VectorXd _MPC_F_Eig;       // Eigen Quadratic program F matrixi
  Eigen::Matrix<double, 6, 6> _q;   // Output weighting matrix
  Eigen::Matrix<double, 4, 4> _r;   // Control input weighting matrix
  Eigen::Matrix<double, 6, 6> _q_f; // Terminal output weighting matrix
  Eigen::MatrixXd _Q;               // General output weighting matrix
  Eigen::MatrixXd _R;               // General input weighting matrix
  al::minqpstate _qpstate;          // State for Quadratic Program
  al::minqpreport _qpreport;        // Report for QP solver

  // Constraint handling
  double _t_input;           // Time for thrust constraint
  al::real_1d_array _u_bndl; // Lower bound on thrusts
  al::real_1d_array _u_bndh; // Upper bound on thrusts
  double _T_scaled;
  double _time;

  // Constants
  double _m = 1.0;       // Mass (kg)
  double _g = 9.81;      // Gravity (m/s)
  double _j_xx = 0.0122; // x-axis principle moment
  double _j_yy = 0.0126; // y-axis principle moment
  double _j_zz = 0.0239; // z-axis principle moment
  double _k = 1.8;       // Propeller yaw moment ratio
  double _c = 0.166;     // Propeller thrust to moment
  double _T_max = 7.0;   // Maximum thrust (N)
  double _t_ramp = 0.2;  // Thrust ramp time (s)

  // Calculation Variables;
  Eigen::Matrix3d _Rotm;
  Eigen::Vector3d _gamma;
  Eigen::Vector4d _u_tm;
  double _phi;
  double _sphi;
  Eigen::Matrix<double, 12, 12> _E;
  Eigen::Matrix<double, 12, 12> _Ek;
  Eigen::Matrix<double, 12, 12> _Ak;
  Eigen::Matrix<double, 12, 4> _ABk;
  Eigen::Matrix<double, 12, 1> _Afk;
  Eigen::Matrix<double, 6, 6> _A_dare;
  Eigen::Matrix<double, 6, 4> _B_dare;
  Eigen::Matrix<double, 6, 6> _Ad;
  Eigen::Matrix<double, 6, 6> _Adk;
  Eigen::Matrix<double, 6, 6> _Gd;
  Eigen::Matrix<double, 6, 6> _Gdk;
  Eigen::Matrix<double, 6, 6> _Hd;
  Eigen::Matrix<double, 6, 6> _Hdk = Eigen::Matrix<double, 6, 6>::Identity();
  Eigen::Matrix<double, 6, 6> _W;
  Eigen::Matrix<double, 6, 6> _V1;
  Eigen::Matrix<double, 6, 6> _V2;

  public:
  // Default constructor
  LieMPC()
  {
    // Set default parameter values
    _np = 8;
    _ts = 0.04;
    _J(0, 0) = _j_xx;
    _J(1, 1) = _j_yy;
    _J(2, 2) = _j_zz;
    _J_inv = _J.inverse();
    _B_wm = _J_inv;

    //Build control allocation matrix;
    _Gamma << 1, 1, 1, 1,
        -_c, _c, _c, -_c,
        _c, -_c, _c, -_c,
        _k, _k, -_k, -_k;

    // Set default cost weights
    Eigen::Matrix<double, 6, 1> y_weight;
    y_weight << 1.e-2, 1.e-2, 1.e-2, 100., 100., 100.;
    _q = y_weight.asDiagonal();

    Eigen::Matrix<double, 4, 1> u_weight;
    u_weight << 1.e-7, 1.e-7, 1.e-7, 1.e-7;
    _r = u_weight.asDiagonal();

    // Assign constant state matrix parts
    _Ac.block(0, 3, 3, 3) = _A_rv;
    _Ac.block(6, 9, 3, 3) = _A_pw;
    _Bc.block(9, 1, 3, 3) = _B_wm;

    // Assign output matrix parts;
    _C.block(0, 0, 3, 3) = _I;
    _C.block(3, 6, 3, 3) = _I;

    // Set matrix sizes
    Y.resize(12 * _np);
    _H.resize(_nx * _np, _nu * _np);
    _F.resize(_nx * _np);
    _G.resize(_nx * _np, _nx);
    _C_bar.resize(_ny * _np, _nx * _np);
    _Q.resize(_ny * _np, _ny * _np);
    _R.resize(_nu * _np, _nu * _np);
    _H_f.resize(_nx, _nu * _np);
    _F_f.resize(_nx);
    _dY.resize(_ny * _np);
    _U_lin.resize(_nu * _np);
    _MPC_H_Eig.resize(_np * _nu, _np * _nu);
    _MPC_H_Eig_Sym.resize(_np * _nu, _np * _nu);
    _MPC_F_Eig.resize(_np * _nu);
    _U.setlength(_np * _nu);
    _U0.setlength(_np * _nu);
    _Uc.setlength(_np * _nu);
    _u_bndl.setlength(_np * _nu);
    _u_bndh.setlength(_np * _nu);

    // Initialize matrix values
    _H = Eigen::MatrixXd::Zero(_nx * _np, _nu * _np);
    _H_f = Eigen::MatrixXd::Zero(_nx, _nu * _np);
    _G = Eigen::MatrixXd::Zero(_nx * _np, _nx);
    _C_bar = Eigen::MatrixXd::Zero(_ny * _np, _nx * _np);
    _Q = Eigen::MatrixXd::Zero(_ny * _np, _ny * _np);
    _R = Eigen::MatrixXd::Zero(_nu * _np, _nu * _np);

    // Initialize QP Solver
    al::minqpcreate(_nu * _np, _qpstate);
    for (int i = 0; i < _np * _nu; i++) {
      _U0(i) = _m * _g / 4;
    }
    al::minqpsetscale(_qpstate, _U0);
    al::minqpsetalgoquickqp(_qpstate, 1e-10, 1e-10, 1e-10, 100, true);
  };

  // Constructor used to specify prediction horizon, timestep, and weights
  LieMPC(int n_p, double timestep, Eigen::MatrixXd y_weight, Eigen::MatrixXd u_weight)
  {
    // Fetch paramter values
    _np = n_p;
    _ts = timestep;
    _q = y_weight;
    _r = u_weight;
    _J(0, 0) = _j_xx;
    _J(1, 1) = _j_yy;
    _J(2, 2) = _j_zz;
    _J_inv = _J.inverse();
    _B_wm = _J_inv;

    //Build control allocation matrix;
    _Gamma << 1, 1, 1, 1,
        -_c, _c, _c, -_c,
        _c, -_c, _c, -_c,
        _k, _k, -_k, -_k;

    // Assign constant state matrix parts
    _Ac.block(0, 3, 3, 3) = _A_rv;
    _Ac.block(6, 9, 3, 3) = _A_pw;
    _Bc.block(9, 1, 3, 3) = _B_wm;

    // Assign output matrix parts;
    _C.block(0, 0, 3, 3) = _I;
    _C.block(3, 6, 3, 3) = _I;

    // Set matrix sizeis
    Y.resize(12 * _np);
    _H.resize(_nx * _np, _nu * _np);
    _F.resize(_nx * _np);
    _G.resize(_nx * _np, _nx);
    _C_bar.resize(_ny * _np, _nx * _np);
    _Q.resize(_ny * _np, _ny * _np);
    _R.resize(_nu * _np, _nu * _np);
    _H_f.resize(_nx, _nu * _np);
    _F_f.resize(_nx);
    _dY.resize(_ny * _np);
    _U_lin.resize(_nu * _np);
    _MPC_H_Eig.resize(_np * _nu, _np * _nu);
    _MPC_H_Eig.resize(_np * _nu, _np * _nu);
    _MPC_F_Eig.resize(_np * _nu);
    _U.setlength(_np * _nu);
    _u_bndl.setlength(_np * _nu);
    _u_bndh.setlength(_np * _nu);

    // Initialize matrix values
    _H = Eigen::MatrixXd::Zero(_nx * _np, _nu * _np);
    _H_f = Eigen::MatrixXd::Zero(_nx, _nu * _np);
    _G = Eigen::MatrixXd::Zero(_nx * _np, _nx);
    _C_bar = Eigen::MatrixXd::Zero(_ny * _np, _nx * _np);
    _Q = Eigen::MatrixXd::Zero(_ny * _np, _ny * _np);
    _R = Eigen::MatrixXd::Zero(_nu * _np, _nu * _np);
  };

  void linearize()
  {
    // Build rotation matrix
    _Rotm = Eigen::Map<Eigen::Matrix3d>((x.segment(6, 9)).data()).transpose();

    // Calculate forces and moments;
    _u_tm = _Gamma * u_lin;

    // Build state matrix components
    _A_vp.noalias() = (1. / _m) * _Rotm * _cross_op(_1_3 * _u_tm(0));
    _A_pp.noalias() = -_cross_op(x.segment(15, 3));
    _A_ww.noalias() = _J_inv * (_cross_op(_J * x.segment(15, 3)) - (_cross_op(x.segment(15, 3)) * _J));

    // Build input matrix components
    _B_vt.noalias() = -(1. / _m) * _Rotm * _1_3;

    // Build offset matrix componenets
    _f_r = -x.segment(3, 3);
    _f_v.noalias() = (1. / _m) * _Rotm * _1_3 * _u_tm(0) - _1_3 * _g;
    _f_p = -x.segment(15, 3);
    _gamma.noalias() = (-_cross_op(x.segment(15, 3)) * _J * x.segment(15, 3) + _u_tm.segment(1, 3));
    _f_p_t.noalias() = -_J_inv * _gamma;
    _f_w = _f_p_t;

    // Build matrices
    _Ac.block(3, 6, 3, 3) = _A_vp;
    _Ac.block(6, 6, 3, 3) = _A_pp;
    _Ac.block(9, 9, 3, 3) = _A_ww;

    _Bc.block(3, 0, 3, 1) = _B_vt;
    _Bcg = _Bc * _Gamma;

    _f.segment(0, 3) = _f_r;
    _f.segment(3, 3) = _f_v;
    _f.segment(6, 3) = _f_p;
    _f.segment(9, 3) = _f_w;

    _f_t.segment(6, 3) = _f_p_t;
  };

  void discretize()
  {
    // Obtain discrete A and B matrices using a ZOH discretization
    _Ek = Eigen::Matrix<double, 12, 12>::Identity();
    _E = _Ek;

    for (int i = 1; i < 16; i++) {
      _Ek = _Ek * (_Ac * _ts) / (i + 1);
      _E += _Ek;
    }

    _A = Eigen::Matrix<double, 12, 12>::Identity() + _E * _Ac * _ts;
    _B = _E * _Bcg * _ts;

    // Solve for terminal cost from discrete-time algebraic Ricatti equation
    _q_f = _solve_dare(_C * _A * _C.transpose(), _C * _B, _q, _r);
  };

  void build_mpc()
  {
    // Build prediction matrices for MPC
    _Ak = Eigen::MatrixXd::Identity(_nx, _nx);
    _ABk = _B;
    _Afk = _ts * (_f + (_ts / 2.) * _f_t);
    _F = Eigen::VectorXd::Zero(_nx * _np);
    _F_f = Eigen::VectorXd::Zero(_nx);

    for (int i = 0; i < _np; i++) {

      if (i != 0) {
        _Ak *= _A;
        _ABk.noalias() = _Ak * _B;
        _Afk.noalias() = _Ak * _ts * (_f + i * (_ts / 2) * _f_t);
      }

      _C_bar.block(i * _ny, i * _nx, _ny, _nx) = _C;
      _Q.block(i * _ny, i * _ny, _ny, _ny) = _q;
      _R.block(i * _nu, i * _nu, _nu, _nu) = _r;
      // _G.block(i * _nx, 0, _nx, _nx) = _Ak;

      _dY.segment(i * _ny, 3) = x.segment(0, 3) - Y.segment(i * 12, 3);
      _dY.segment(i * _ny + 3, 3) = _vee_SO3(_Rotm.transpose() * Eigen::Map<Eigen::Matrix3d>((Y.segment(i * 12 + 3, 9)).data()));

      _U_lin.segment(i * _nu, _nu) = u_lin;

      for (int j = 0; j + i < _np - 1; j++) {
        _F.segment(_nx * (_np - 1 - j), _nx) += _Afk;
        _H.block((j + i + 1) * _nx, j * _nu, _nx, _nu) = _ABk;
      }

      _H_f.block(0, (_np - i - 1) * _nu, _nx, _nu) = _ABk;
      _F_f += _Afk;
    }

    _MPC_H_Eig.noalias() = (_C_bar * _H).transpose() * _Q * (_C_bar * _H) + (_C * _H_f).transpose() * _q_f * (_C * _H_f) + _R;
    _MPC_H_Eig_Sym = _MPC_H_Eig + _MPC_H_Eig.transpose();
    _MPC_F_Eig.noalias() = 2. * (_C_bar * _F - _dY).transpose() * _Q * (_C_bar * _H) + 2. * (_C * _F_f - 2. * _dY.segment((_np - 1) * _ny, _ny)).transpose() * _q_f * (_C * _H_f) - _U_lin.transpose() * _R;
  };

  void solve()
  {
    // Solve QP to obtain optimal control inputs
    for (int i = 0; i < _np; i++) {
      for (int j = 0; j < _nu; j++) {
        _U0(i * _nu + j) = u_lin(j);
      }
    }

    _eigen_to_al(_MPC_H_Eig_Sym, &_MPC_H);
    _eigen_to_al(_MPC_F_Eig, &_MPC_F);
    al::minqpsetquadraticterm(_qpstate, _MPC_H);
    al::minqpsetlinearterm(_qpstate, _MPC_F);
    _init_constraints();
    _U = _U0;

    for (int i = 0; i < 5; i++) {
      al::minqpsetbc(_qpstate, _u_bndl, _u_bndh);
      al::minqpsetstartingpoint(_qpstate, _U);
      al::minqpoptimize(_qpstate);
      al::minqpresults(_qpstate, _U, _qpreport);
      for (int j = 0; j < (_np * _nu); j++) {
        _Uc(j) = _U0(j) - _U(j);
      }
      _build_constraints();
      if (_check_bounds(_U, _u_bndl, _u_bndh)) {
        break;
      }
    }

    u(0) = u_lin(0) - _U(0);
    u(1) = u_lin(1) - _U(1);
    u(2) = u_lin(2) - _U(2);
    u(3) = u_lin(3) - _U(3);
  };

  private:
  Eigen::Matrix3d _cross_op(Eigen::Vector3d _v)
  {
    // Calculate cross operator matrix from vector
    Eigen::MatrixXd _M = Eigen::MatrixXd::Zero(3, 3);
    _M(0, 1) = -_v(2);
    _M(0, 2) = _v(1);
    _M(1, 2) = -_v(0);
    _M(1, 0) = _v(2);
    _M(2, 0) = -_v(1);
    _M(2, 1) = _v(0);
    return _M;
  };

  Eigen::Vector3d _vee_SO3(Eigen::Matrix3d _rotMatrix)
  {
    _phi = acos((_rotMatrix.trace() - 1) / 2);
    if (_phi == 0.0) {
      return (Eigen::VectorXd(3) << 0,0,0).finished();
    } else {
    _sphi = _phi / sin(_phi);
    return (Eigen::VectorXd(3) << _sphi * (_rotMatrix(1, 2) - _rotMatrix(2, 1)) / 2., _sphi * (-_rotMatrix(0, 2) + _rotMatrix(2, 0)) / 2., _sphi * (_rotMatrix(0, 1) - _rotMatrix(1, 0)) / 2.).finished();
    }
  };

  double _constraint(double _init_time, int _timesteps)
  {
    // Calculate thrust limits after number of timesteps
    _time = _init_time + _timesteps * _ts;
    if (_time > 0.) {
      return _T_max / (1 + std::exp(-12. / _t_ramp * (_time - (_t_ramp / 2.))));
    } else if (_time < 0.) {
      return -_T_max / (1 + std::exp(12. / _t_ramp * (_time + (_t_ramp / 2.))));
    } else {
      return 0.;
    }
  };

  double _inv_constraint(double _thrust)
  {
    // Calculate time associated with thrust value on constraint curve
    _T_scaled = _thrust / _T_max;

    if (std::abs(_T_scaled) > 0.98) {
      _T_scaled = ((_thrust > 0.) - (_thrust < 0.)) * 0.98;
    }

    if (_thrust > 0.) {
      return (std::log(_T_scaled / (1. - _T_scaled)) * _t_ramp / 12.) + _t_ramp / 2.;
    } else if (_thrust < 0.) {
      return -(std::log(-_T_scaled / (1. - _T_scaled)) * _t_ramp / 12.) - _t_ramp / 2.;
    } else {
      return 0.;
    }
  };

  Eigen::Matrix<double, 6, 6> _solve_dare(Eigen::Matrix<double, 6, 6> _Adare, Eigen::Matrix<double, 6, 4> _Bdare, Eigen::Matrix<double, 6, 6> _Qdare, Eigen::Matrix<double, 4, 4> _Rdare)
  {
    // Solve discrete-time algebraic Ricatti equation
    _Ad = _Adare;
    _Gd.noalias() = _Bdare * (_Rdare.householderQr().solve(_Bdare.transpose()));
    _Hd = _Qdare;

    while ((_Hd - _Hdk).squaredNorm() > 1e-10 * _Hd.squaredNorm()) {
      _Adk = _Ad;
      _Gdk = _Gd;
      _Hdk = _Hd;

      _W = Eigen::Matrix<double, 6, 6>::Identity() + _Gdk * _Hdk;
      _V1 = _W.householderQr().solve(_Adk);
      _V2 = (_W.householderQr().solve(_Gdk.transpose())).transpose();

      _Ad.noalias() = _Adk * _V1;
      _Gd.noalias() = _Gdk + _Adk * _V2 * _Adk.transpose();
      _Hd.noalias() = _Hdk + _V1.transpose() * _Hdk * _Adk;
    }
    return _Hd;
  };

  void _eigen_to_al(Eigen::MatrixXd emat, al::real_2d_array* almat)
  {
    almat->setlength(emat.rows(), emat.cols());
    for (int i = 0; i < emat.rows(); i++) {
      for (int j = 0; j < emat.cols(); j++) {
        almat->operator()(i, j) = emat(i, j);
      }
    }
  };

  void _eigen_to_al(Eigen::VectorXd emat, al::real_1d_array* almat)
  {
    almat->setlength(emat.rows());
    for (int i = 0; i < emat.rows(); i++) {
      almat->operator()(i) = emat(i);
    }
  };

  bool _check_bounds(al::real_1d_array _U_ctrl, al::real_1d_array _U_low, al::real_1d_array _U_upp)
  {
    for (int i = 0; i < _np; i++) {
      if (_U_ctrl(i) < _U_low(i) || _U_ctrl(i) > _U_upp(i)) {
        return 0;
      }
    }
    return 1;
  };

  void _init_constraints()
  {
    // Build constraint matrices for MPC
    for (int i = 0; i < 4; i++) {
      _t_input = _inv_constraint(_U0(i));
     for (int j = 1; j <= _np; j++) {
        _u_bndh(i + _nu * (j - 1)) = _U0(i + _nu * (j - 1)) - _constraint(_t_input, -j);
        _u_bndl(i + _nu * (j - 1)) = _U0(i + _nu * (j - 1)) - _constraint(_t_input, j);
      }
    }
  };

  void _build_constraints()
  {
    // Build constraint matrices for MPC
    for (int i = 0; i < 4; i++) {
      _t_input = _inv_constraint(_U0(i));
      _u_bndh(i) = _U0(i) - _constraint(_t_input, -1);
      _u_bndl(i) = _U0(i) - _constraint(_t_input, 1);
      for (int j = 2; j <= _np; j++) {
        _t_input = _inv_constraint(_Uc(i + _nu * (j - 2)));
        _u_bndh(i + _nu * (j - 1)) = _U0(i + _nu * (j - 1)) - _constraint(_t_input, -1);
        _u_bndl(i + _nu * (j - 1)) = _U0(i + _nu * (j - 1)) - _constraint(_t_input, 1);
      }
    }
  };
};


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <fstream>
#include <exception>
#include <cstdlib>
#include <execinfo.h>

#include <boost/exception/info.hpp>

#include <gsl/gsl_multiroots.h>
#include <gsl/gsl_vector.h>

#include <libsgp4/DateTime.h>
#include <libsgp4/Eci.h>
#include <libsgp4/Globals.h>
#include <libsgp4/SGP4.h>
#include <libsgp4/Tle.h>

#include <Astro/astro.hpp>
#include <SML/sml.hpp>
#include <SML/constants.hpp>
#include <SML/basicFunctions.hpp>

#include <Atom/printFunctions.hpp>

#include <Astro/orbitalElementConversions.hpp>
#include <Atom/convertCartesianStateToTwoLineElements.hpp>

#include "/media/abhishek/work/TU delft/INTERNSHIP at DINAMICA/work/github/pykep/src/core_functions/par2ic.h"

#include "CppProject/TleGen.hpp"

namespace TleGen{

	typedef double Real;
	typedef std::vector< Real > Vector6;
	typedef std::vector< Real > Vector3;
	typedef std::vector< Real > Vector2;
	typedef std::vector < std::vector < Real > > Vector2D;

	void TleGen( Vector6 randKepElem, std::string& SolverStatus, int& IterationCount )
	{
		// grav. parameter 'mu' of earth
    	const double muEarth = kMU*( pow( 10, 9 ) ); // unit m^3/s^2
	    // generate sets of cartesian elements corresponding to each of the psuedo random orbital element set.
	    // The conversion from keplerian elements to the cartesian elements is done using the PyKep library from ESA.
	    Vector3 CartPos( 3 ); // empty vector to store position coordinates
	    Vector3 CartVel( 3 ); // empty vector to store velocity components
	    // Real rangeMag = 0; // magnitude range
	    // Real velocityMag = 0; // velocity magnitude
	    kep_toolbox::par2ic( randKepElem, muEarth, CartPos, CartVel );

	    // verification of the conversion process using Ron Noomen's lecture notes from TUDelft
	    // Document ID ae4878.basics.v4-16.pdf
	    // Vector6 testKep = { 6787746.891, 0.000731104, sml::convertDegreesToRadians( 51.68714486 ),
	    //     sml::convertDegreesToRadians( 127.5486706 ), sml::convertDegreesToRadians( 74.21987137 ), sml::convertDegreesToRadians( 24.08317766 ) };
	    // Vector3 testPos( 3 );
	    // Vector3 testVel( 3 );
	    // kep_toolbox::par2ic( testKep, muEarth, testPos, testVel );
	    // std::cout << testPos[ 0 ] << std::endl;
	    // std::cout << testPos[ 1 ] << std::endl;
	    // std::cout << testPos[ 2 ] << std::endl;
	    // std::cout << testVel[ 0 ] << std::endl;
	    // std::cout << testVel[ 1 ] << std::endl;
	    // std::cout << testVel[ 2 ] << std::endl;

	    // convert the cartesian elements to the corresponding TLE format using the ATOM toolbox
	    Vector6 cartesianState( 6 );
	    // cartesianState[ 0 ] = -7.1e3;
	    // cartesianState[ 1 ] = 2.7e3;
	    // cartesianState[ 2 ] = 1.3e3;
	    // cartesianState[ 3 ] = -2.5;
	    // cartesianState[ 4 ] = -5.5;
	    // cartesianState[ 5 ] = 5.5;
	    Tle convertedTle;
	    Tle referenceTle = Tle(); // empty TLE for reference
	    // std::cout << referenceTle << std::endl << std::endl;
	    const Real absTol = 1.0e-10; // absolute tolerance
	    const Real relTol = 1.0e-5; // relative tolerance
	    const int maxItr = 100; // maximum allowed iterations per conversion run

	    // important note, the atom function converting cartesian to TLEs takes in values in km and km/s.
	    cartesianState[ 0 ] = CartPos[ 0 ]/1000;
	    cartesianState[ 1 ] = CartPos[ 1 ]/1000;
	    cartesianState[ 2 ] = CartPos[ 2 ]/1000;
	    cartesianState[ 3 ] = CartVel[ 0 ]/1000;
	    cartesianState[ 4 ] = CartVel[ 1 ]/1000;
	    cartesianState[ 5 ] = CartVel[ 2 ]/1000;
		convertedTle = atom::convertCartesianStateToTwoLineElements< Real, Vector6 >( cartesianState, DateTime( ), SolverStatus,
	    	IterationCount, referenceTle, kMU, kXKMPER, absTol, relTol, maxItr );
	}
}


#include <algorithm>
#include <iostream>
#include <iterator>
#include <string>
#include <vector>
#include <map>
#include <valarray>
#include <boost/algorithm/string.hpp>

using namespace std;

map<string, valarray<int>> DIRECTIONS = {
    // x, y, z
    // Hex cubic labeling
    {"e", {1, -1, 0}},
    {"w", {-1, 1, 0}},
    {"se", {1, 0, -1}},
    {"ne", {0, -1, 1}},
    {"sw", {0, 1, -1}},
    {"nw", {-1, 0, 1}},
};

string hash_loc(valarray<int> loc){
  return (to_string(loc[0]) + ","+
          to_string(loc[1]) + ","+
          to_string(loc[2]));
}

valarray<int> unhash_loc(string hloc) {
  vector<string> split_str;
  boost::split(split_str, hloc, boost::is_any_of(","));
  valarray<int> out = {
      stoi(split_str[0]),
      stoi(split_str[1]),
      stoi(split_str[2]),
  };
  return out;
}

int count_black(map<string, int> tile_flips){
  int num_black = 0;
  for (auto [sloc, flips] : tile_flips) {
    num_black += flips % 2;
  }
  return num_black;
}

int main() {

  map<string, int> tile_flips; // 1 if black

  for (string line; getline(cin, line);){
    // cout << line << endl;
    vector<string> directions;
    int curr_loc=0;
    while (curr_loc < line.size()){
      if ((line[curr_loc] == 'n') ||
          (line[curr_loc] == 's')) {
        directions.push_back(line.substr(curr_loc, 2));
        curr_loc += 2;
      } else{
        directions.push_back(line.substr(curr_loc, 1));
        curr_loc += 1;
      }
    }
    valarray<int> loc = {0,0,0}; // Reference tile
    for (string dir : directions){
      loc += DIRECTIONS[dir];
    }
    tile_flips[hash_loc(loc)] += 1;
  }

  cout << "Part 1: " << count_black(tile_flips) << endl;
  cout << endl;

  // Part 2
  int num_days = 100;
  for (int i=1; i<=num_days; i++){
    auto start_flips = tile_flips;
    // Map of white tiles to num black adjacent
    // Only gets evaluated for tiles with atleast 1 black neighbour
    map<string,int> blackn_count;
    // Look over all known tiles;
    for (auto [hloc, flips] : start_flips){
      bool black = flips % 2;
      valarray<int> loc = unhash_loc(hloc);
      if (black){
        // If a black tile, count adjacent black tiles
        int blackn = 0;
        for (auto [dir, mdir] : DIRECTIONS){
          string nhash = hash_loc(loc + mdir);
          bool is_blackn = (start_flips[nhash] % 2);
          blackn += is_blackn;
          // If neighbour is white, increment
          // the number of black adjacents that
          // that white tile has.
          if (not is_blackn){
            blackn_count[nhash] += 1;
          }
        }
        // Flip if matches rule for black tile
        if ((blackn == 0) || (blackn > 2)){
          tile_flips[hloc]++;
        }
      }
    }

    // Find white tiles with 2 black adjacent.
    for (auto [hloc, nblackn] : blackn_count){
      if (nblackn == 2){
        tile_flips[hloc]++;
      }
    }
    cout << "Day " << i << ": " << count_black(tile_flips) << endl;
  }
  cout << endl;
  cout << "Part 2: " << count_black(tile_flips) << endl;
}


//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Core>
#include "_hypre_utilities.h"
#include "HYPRE_krylov.h"
#include "HYPRE.h"
#include "HYPRE_parcsr_ls.h"
#include "general_functions.hpp"
#include "class.hpp"

IMPLICIT_SCALAR_TRANSPORT_SOLVER::IMPLICIT_SCALAR_TRANSPORT_SOLVER(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, vector<bool> &dirichlet_flag1, int precond_freq_it1, double unsteady_coeff1, double conv_coeff1, double diff_coeff1, bool solver_log_flag1)
{
    dirichlet_flag = dirichlet_flag1, solver_log_flag = solver_log_flag1;
    unsteady_coeff = unsteady_coeff1, conv_coeff = conv_coeff1, diff_coeff = diff_coeff1;
    precond_freq_it = precond_freq_it1;

    zero_vector = Eigen::VectorXd::Zero(points.nv);
    bc_full_neumann = true;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv] && dirichlet_flag[iv])
        { //boundary point found with dirichlet BC
            bc_full_neumann = false;
            break;
        }
    if (bc_full_neumann)
    {
        printf("\n\nERROR from IMPLICIT_SCALAR_TRANSPORT_SOLVER::IMPLICIT_SCALAR_TRANSPORT_SOLVER Setting  full Neumann BC is not permitted\n\n");
        throw bad_exception();
    }
    source = zero_vector, phi_old_old = zero_vector;
}

void IMPLICIT_SCALAR_TRANSPORT_SOLVER::set_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    matrix.resize(0, 0);
    vector<Eigen::Triplet<double>> triplet;
    int ivnb, dim = parameters.dimension;
    double value, unsteady_factor;
    if (it == 0)
        unsteady_factor = 1.0; //BDF1: implicit Euler
    else
        unsteady_factor = bdf2_alpha_1;
    for (int iv = 0; iv < points.nv; iv++)
    {
        if (points.boundary_flag[iv])
        {
            if (dirichlet_flag[iv])
                triplet.push_back(Eigen::Triplet<double>(iv, iv, 1.0));
            else
            {
                for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                {
                    ivnb = cloud.nb_points_col[i1];
                    value = points.normal[dim * iv] * cloud.grad_x_coeff[i1] + points.normal[dim * iv + 1] * cloud.grad_y_coeff[i1];
                    triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
                }
            }
        }
        else
        {
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = diff_coeff * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * conv_coeff * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * conv_coeff * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (unsteady_factor * unsteady_coeff); //diagonal term
                triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
            }
        }
    }
    matrix.resize(points.nv, points.nv);
    matrix.setFromTriplets(triplet.begin(), triplet.end());
    matrix.makeCompressed();
    triplet.clear();
}

void IMPLICIT_SCALAR_TRANSPORT_SOLVER::modify_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    int ivnb, dim = parameters.dimension, index;
    double value, unsteady_factor;
    if (it == 0)
        unsteady_factor = 1.0; //BDF1: implicit Euler
    else
        unsteady_factor = bdf2_alpha_1;
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = diff_coeff * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * conv_coeff * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * conv_coeff * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (unsteady_factor * unsteady_coeff); //diagonal term
                index = nb_points_col_matrix[i1];
                matrix.valuePtr()[index] = value;
            }
        }
}

void IMPLICIT_SCALAR_TRANSPORT_SOLVER::calc_nb_points_col_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters)
{
    int ivnb, index;
    nb_points_col_matrix.clear();
    for (int i1 = 0; i1 < cloud.nb_points_col.size(); i1++) //initialize to -1
        nb_points_col_matrix.push_back(-1);
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities; nb_points_col_matrix ahas value of [-1] at bounday points
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                index = -1;
                ivnb = cloud.nb_points_col[i1];
                for (int i2 = matrix.outerIndexPtr()[iv]; i2 < matrix.outerIndexPtr()[iv + 1]; i2++)
                    if (matrix.innerIndexPtr()[i2] == ivnb)
                    {
                        index = i2;
                        break;
                    }
                if (index < 0)
                {
                    cout << "\n\nError from SEMI_IMPLICIT_SPLIT_SOLVER::calc_nb_points_col_matrix in matrix_u, unable to find ivnb: " << ivnb << " for iv: " << iv << ", points.boundary_flag[iv]: " << points.boundary_flag[iv] << "\n\n";
                    throw bad_exception();
                }
                nb_points_col_matrix[i1] = index;
            }
        }
}

void IMPLICIT_SCALAR_TRANSPORT_SOLVER::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &phi_new, Eigen::VectorXd &phi_old, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, int it1)
{
    it = it1;
    if (it % precond_freq_it == 0 || it == 0 || it == 1)
    {
        set_matrix(points, cloud, parameters, u_new, v_new);
        solver_eigen.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen.compute(matrix);
        if (it == 0)
            calc_nb_points_col_matrix(points, cloud, parameters);
    }
    modify_matrix(points, cloud, parameters, u_new, v_new);

    if (it == 0) //BDF1: implicit Euler
        source = (unsteady_coeff * phi_old);
    else //BDF2
        source = -((bdf2_alpha_2 * unsteady_coeff) * phi_old) - ((bdf2_alpha_3 * unsteady_coeff) * phi_old_old);

    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        { //retain boundary condition from "_old"
            if (dirichlet_flag[iv])
                source[iv] = phi_old[iv];
            else
                source[iv] = 0.0;
        }
    phi_new = solver_eigen.solveWithGuess(source, phi_old);
    if (solver_log_flag)
    {
        double absolute_residual = (matrix * phi_new - source).norm();
        double relative_residual = absolute_residual / source.norm();
        parameters.rel_res_log.push_back(relative_residual);
        parameters.abs_res_log.push_back(absolute_residual);
        parameters.n_iter_actual.push_back(solver_eigen.iterations());
    }
    phi_old_old = phi_old;
}


/*
 * baseframe.cc
 * Copyright (C) 2017 roman <roman.podolski@tum.de>
 *
 * Distributed under terms of the MIT license.
 */

#include "planner/baseframe.h"
#include <gflags/gflags.h>
#include <boost/math/interpolators/cubic_b_spline.hpp>
#include <boost/math/quadrature/trapezoidal.hpp>
#include <boost/math/tools/minima.hpp>
#include <boost/math/tools/roots.hpp>
#include <fstream>
#include <iostream>
#include <map>
#include <utility>
#include <vector>
#include "planner/map.h"

namespace planner {

using boost::math::quadrature::trapezoidal;
using boost::math::cubic_b_spline;
using boost::math::tools::bisect;
using boost::math::tools::eps_tolerance;
using boost::math::tools::newton_raphson_iterate;

Baseframe::Baseframe(const Map &map) {
  const auto vx = map.get_waypoints_x();
  const auto vy = map.get_waypoints_y();
  init(vx, vy);
}

Baseframe::Baseframe(const std::vector<std::pair<double, double>> &wp) {
  std::vector<double> vx, vy;
  for (const auto &p : wp) {
    vx.push_back(p.first);
    vy.push_back(p.second);
  }
  init(vx, vy);
}

Baseframe::Baseframe(const std::vector<point> &wp) {
  std::vector<double> vx, vy;
  for (const auto &p : wp) {
    vx.push_back(p.x());
    vy.push_back(p.y());
  }
  init(vx, vy);
}

void Baseframe::init(const std::vector<double> vx,
                     const std::vector<double> vy) {
  CHECK_EQ(vx.size(), vy.size());
  _N = vx.size() - 1;
  _x = cubic_b_spline<double>(vx.begin(), vx.end(), 0, 1.0);
  _y = cubic_b_spline<double>(vy.begin(), vy.end(), 0, 1.0);
}

point Baseframe::Q(const double t) const {
  if (t < 0 || t > _N)
    throw std::domain_error("t = " + std::to_string(t) +
                            " not within the domain [0,N]");
  return point(_x(t), _y(t));
}

point Baseframe::Q_prime(const double t) const {
  if (t < 0 || t > _N)
    throw std::domain_error("t = " + std::to_string(t) +
                            " not within the domain [0,N]");
  return point(_x.prime(t), _y.prime(t));
}

point Baseframe::Q_pprime(const double t) const {
  const double ddt_x =
      derivative(t, 0.001, [this](const double t) { return _x.prime(t); });
  const double ddt_y =
      derivative(t, 0.001, [this](const double t) { return _y.prime(t); });
  return point(ddt_x, ddt_y);
}

double Baseframe::theta_Q(const double t) const {
  auto p = Q_prime(t);
  // return atan2(p.y(), p.x());
  return atan(p.y() / p.x());
}

static std::map<double, double> _A_lut{{0, 0}};
static std::vector<double> _segment_lut;
double Baseframe::A(const double t) const {
  if (t < 0 || t > _N)
    throw std::domain_error("t = " + std::to_string(t) +
                            " not within the domain [0,N]");
  if (_A_lut.find(t) != _A_lut.end()) {
    return _A_lut[t];
  }

  double result = 0.0;
  if (t == 0) return result;

  auto f = [this](double t) {
    return sqrt(pow(_x.prime(t), 2) + pow(_y.prime(t), 2));
  };

  // The 8 and 1e-5 are a little borderline, but help performance.
  size_t max_refinements = 8;
  result = trapezoidal(f, 0.0, t, 1e-5, max_refinements);

  _A_lut[t] = result;

  return result;
}

static std::map<double, double> _inv_A_lut{{0, 0}};
double Baseframe::inv_A(const double s) const {
  if (s < 0 || s > length())
    throw std::domain_error("s = " + std::to_string(s) +
                            " not within the domain [0,L]");

  if (_segment_lut.empty()) {
    for (int t = 0; t < _N; ++t) {
      _segment_lut.push_back(A(t));
    }
  }

  if (_inv_A_lut.find(s) != _inv_A_lut.end()) {
    return _inv_A_lut[s];
  }

  std::uintmax_t iterations = 1000;
  auto low = std::lower_bound(_segment_lut.begin(), _segment_lut.end(), s);
  const double segment = low - _segment_lut.begin();
  double result = 0.0;
  result = bisect([this, s](double t) { return A(t) - s; }, segment - 1,
                  segment, eps_tolerance<float>(), iterations)
               .first;

  _inv_A_lut[s] = result;

  return result;
}

static std::map<double, double> _curvature_lut;
double Baseframe::curvature(const double s) {
  if (_curvature_lut.find(s) != _curvature_lut.end()) {
    return _curvature_lut[s];
  }

  const auto d_P = P_prime(s);
  double d_x = d_P.x();
  double d_y = d_P.y();

  if (d_x == 0.0 && d_y == 0.0) {  // avoid division by 0.0
    d_x = std::numeric_limits<double>::denorm_min();
    d_y = std::numeric_limits<double>::denorm_min();
  }

  const auto dd_P = P_pprime(s);
  const double dd_x = dd_P.x();
  const double dd_y = dd_P.y();

  const double kappa =
      (d_x * dd_y - dd_x * d_y) / pow((pow(d_x, 2) + pow(d_y, 2)), 3.0 / 2.0);

  _curvature_lut[s] = kappa;

  return kappa;
}

// TODO(roman): check for numerical problems
std::pair<double, double> Baseframe::localize(const point position,
                                              const double v,
                                              const double heading,
                                              const double dt,
                                              const double last_known_s) const {
  auto D_prime = [ this, x_0 = position.x(), y_0 = position.y() ](double s) {
    auto p = P(s);
    double x = p.x();
    double y = p.y();

    auto p_prime = P_prime(s);
    double x_prime = p_prime.x();
    double y_prime = p_prime.y();

    auto p_pprime = P_pprime(s);
    double x_pprime = p_pprime.x();
    double y_pprime = p_pprime.y();

    double dist_prime = 2 * (x - x_0) * x_prime + 2 * (y - y_0) * y_prime;
    double dist_pprime = 2 * ((x - x_0) * x_pprime + pow(x_prime, 2) +
                              (y - y_0) * y_pprime + pow(y_prime, 2));

    return std::pair<double, double>(dist_prime, dist_pprime);
  };

  std::uintmax_t iterations = 1000;
  double minimum_extra_offset = 10;
  double lower_barrier = last_known_s - minimum_extra_offset / 4; // added /4
  if (lower_barrier <= 0.0) lower_barrier = 0.0;

  double initial_guess = last_known_s + v * dt;
  if (initial_guess > length()) initial_guess = length();
  double upper_barrier = initial_guess + minimum_extra_offset;
  if (upper_barrier > length()) upper_barrier = length();

  //std::cout << lower_barrier << ' ' << initial_guess << ' ' << upper_barrier << '\n';

  auto result = newton_raphson_iterate(
      D_prime, lower_barrier, initial_guess, upper_barrier,
      std::numeric_limits<double>::digits / 4, iterations); // was /2

  //std::cout <<  result << '\n';

  auto p = P(result);

  return std::pair<double, double>(result, bg::distance(p, position));
}

}  // namespace planner


#include <vector>
#include <iostream>
#include <functional>
#include <solver.hpp>
#include <math.h>
#include <complex>

#include <Eigen/Dense>

#include <boost/multiprecision/eigen.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/mpc.hpp>

typedef boost::multiprecision::mpc_complex_500 C;
typedef boost::multiprecision::mpfr_float_500 R;

int main() {
    std::function<C(Eigen::Matrix<C,2,1>&)> f, g;

    f = [] (Eigen::Matrix<C,2,1> &x) -> C {
        return x(0)*x(1) - C(6)/10;
    };

    g = [] (Eigen::Matrix<C,2,1> &x) -> C {
        return x(0)*x(0) + x(1)*x(1) - C(2);
    };

    std::vector<decltype(f)> vf;

    vf.push_back(f);
    vf.push_back(g);

    Eigen::Matrix<C,2,1> x0, x;

    x0(0) = 1;
    x0(1) = 1;

    auto s = solver::Solver2<C,R,2>(vf);
    s.set_h(R("1e-200"));
    s.set_tol(R("1e-100"));
    x = s.solve(x0);

    return 0;
}


// Copyright John Maddock 2014.

// 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)

// Caution: this file contains Quickbook markup as well as code
// and comments, don't change any of the special comment markups!

#ifdef _MSC_VER
#  pragma warning (disable : 4996) // disable -D_SCL_SECURE_NO_WARNINGS C++ 'Checked Iterators'
#endif

#include <boost/math/distributions/hyperexponential.hpp>
#include <iostream>

#ifndef BOOST_NO_CXX11_HDR_ARRAY
#include <array>
#endif

int main()
{
   {
//[hyperexponential_snip1
//=#include <boost/math/distributions/hyperexponential.hpp>
//=#include <iostream>
//=int main()
//={
   const double rates[] = { 1.0 / 10.0, 1.0 / 12.0 };

   boost::math::hyperexponential he(rates);

   std::cout << "Average lifetime: "
      << boost::math::mean(he)
      << " years" << std::endl;
   std::cout << "Probability that the appliance will work for more than 15 years: "
      << boost::math::cdf(boost::math::complement(he, 15.0))
      << std::endl;
//=}
//]
   }
   using namespace boost::math;
#ifndef BOOST_NO_CXX11_HDR_ARRAY
   {
   //[hyperexponential_snip2
   std::array<double, 2> phase_prob = { 0.5, 0.5 };
   std::array<double, 2> rates = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he(phase_prob.begin(), phase_prob.end(), rates.begin(), rates.end());
   //]
   }

   {
   //[hyperexponential_snip3
   // We could be using any standard library container here... vector, deque, array, list etc:
   std::array<double, 2> phase_prob = { 0.5, 0.5 };
   std::array<double, 2> rates      = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he1(phase_prob, rates);    // Construct from standard library container.

   double phase_probs2[] = { 0.5, 0.5 };
   double rates2[]       = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he2(phase_probs2, rates2);  // Construct from native C++ array.
   //]
   }
   {
   //[hyperexponential_snip4
   // We could be using any standard library container here... vector, deque, array, list etc:
   std::array<double, 2> rates = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he(rates.begin(), rates.end());

   assert(he.probabilities()[0] == 0.5); // Phase probabilities will be equal and normalised to unity.
   //]
   }
   {
   //[hyperexponential_snip5
   std::array<double, 2> rates = { 1.0 / 10, 1.0 / 12 };

   hyperexponential he(rates);

   assert(he.probabilities()[0] == 0.5); // Phase probabilities will be equal and normalised to unity.
   //]
   }
#endif
#if !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST) && !(defined(BOOST_GCC_VERSION) && (BOOST_GCC_VERSION < 40500))
   {
   //[hyperexponential_snip6
   hyperexponential he = { { 0.5, 0.5 }, { 1.0 / 10, 1.0 / 12 } };
   //]
   }
   {
   //[hyperexponential_snip7
   hyperexponential he = { 1.0 / 10, 1.0 / 12 };

   assert(he.probabilities()[0] == 0.5);
   //]
   }
#endif
   return 0;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
#define CROCODDYL_CORE_UTILS_MATH_HPP_

#include <boost/type_traits.hpp>

#include <Eigen/Dense>
#include <algorithm>
#include <limits>

// fwd

template <typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
struct pseudoInverseAlgo {
  typedef typename MatrixLike::Scalar Scalar;
  typedef typename MatrixLike::RealScalar RealScalar;

  static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a, const RealScalar& epsilon) {
    using std::max;
    Eigen::JacobiSVD<MatrixLike> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
    RealScalar tolerance =
        epsilon * static_cast<Scalar>(max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
    return svd.matrixV() *
           (svd.singularValues().array().abs() > tolerance)
               .select(svd.singularValues().array().inverse(), 0)
               .matrix()
               .asDiagonal() *
           svd.matrixU().adjoint();
  }
};

template <typename MatrixLike>
struct pseudoInverseAlgo<MatrixLike, false> {
  typedef typename MatrixLike::Scalar Scalar;
  typedef typename MatrixLike::RealScalar RealScalar;

  static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a, const RealScalar&) {
    return Eigen::MatrixBase<MatrixLike>::Zero(a.rows(), a.cols());
  }
};

template <typename MatrixLike>
MatrixLike pseudoInverse(const Eigen::MatrixBase<MatrixLike>& a,
                         const typename MatrixLike::RealScalar& epsilon =
                             Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) {
  return pseudoInverseAlgo<MatrixLike>::run(a, epsilon);
}

#endif  // CROCODDYL_CORE_UTILS_MATH_HPP_


// [[Rcpp::depends(RcppArmadillo)]]
#define ARMA_DONT_PRINT_ERRORS
#include <iostream>
#include <fstream>
#include <cmath>
#include <armadillo>
#include <errno.h>
#include <RcppArmadillo.h>

//' Compute hazard of detection (Hayes-Buckland isotropic hazard)
//'
//' @param x x distance
//' @param y y distance
//' @param dt time step
//' @param observer_speed observer speed
//' @param parameter detection parameters
//' @param w truncation width
//' @param type transect type 0 = point, 1 = line
//' @return hazard of detection
double CalcHazard2(double x, double y, double dt, double observer_speed,
                   arma::vec parameter, double w = 0, int type = 0) {
  double hazard = 0;
  if (type == 1) {
    double r = x * x + y * y;
    hazard = dt * parameter(0) / pow(r, 0.5 * parameter(1));
  }
  if (type == 0) {
    if (y < 0) return 0;
    double abeta = 0.5 * (parameter(1) - 1.0);
    double r0 = x * x  + y * y;
    double y1 = y - observer_speed * dt;
    if (y1 < 0) y1 = 0;
    double r1 = x * x  + y1 * y1;
    if (r1 < 1e-10) return arma::datum::inf;
    if (fabs(x) < 1e-10) {
      if (fabs(parameter(1) - 1) < 1e-10) {
        hazard = log(sqrt(r1)) - log(sqrt(r0));
        hazard *= parameter(0);
      } else {
        hazard = 1.0 / pow(r1, abeta) - 1.0 / pow(r0, abeta);
        hazard *= parameter(0) / (parameter(1) - 1.0);
      }
    } else {
      hazard = R::pbeta(x * x / r1, abeta, 0.5, 1, 0) - R::pbeta(x * x / r0,
                        abeta, 0.5, 1, 0);
      hazard *= R::beta(abeta, 0.5) * parameter(0) / (2.0 * pow(fabs(x),
                                                parameter(1) - 1.0));
    }
  }
  return hazard;
}

//' Get Recorded forward distance once detection occurs
//'
//' @param x recorded x location
//' @param y recorded y location
//' @param accu_hazard hazard accumulated up to that time
//' @param u log random deviate
//' @param parameter detection parameters
//'
//' @return recorded forward distance
double GetRecordedYPosition(double x, double y, double accu_hazard,
                            double u, arma::vec parameter) {
  double r = x * x + y * y;
  double y1;
  double abeta =  0.5 * (parameter(1) - 1.0);
  if (fabs(x) < 1e-10) {
    if (fabs(parameter(1) - 1) < 1e-10) {
      y1 = sqrt(exp(2.0 * (u - accu_hazard) / parameter(0) + log(r)) - x * x);
    } else {
      y1 = sqrt(pow((u - accu_hazard) * (parameter(1) - 1.0) / parameter(0) +
        1 / pow(r, abeta), -1.0 / abeta) - x * x);
    }
  } else {
    y1 = R::pbeta(x * x / r, abeta, 0.5, 1, 0) + 2 * pow(fabs(x),
                  parameter(1) - 1.0) * (u - accu_hazard) / (parameter(0) * R::beta(abeta, 0.5));
    y1 = R::qbeta(y1, abeta, 0.5, 1, 0);
    y1 = fabs(x) * sqrt(1.0 / y1 - 1.0);
  }
  return y1;
}

//' Get Recorded time once detection occurs
//'
//' @param x recorded x location
//' @param y recorded y location
//' @param accu_hazard hazard accumulated up to that time
//' @param u log random deviate
//' @param parameter detection parameters
//'
//' @return recorded forward distance
double GetRecordedT(double x, double y, double accu_hazard,
                    double u, arma::vec parameter) {
  double r = x * x + y * y;
  double t_add = (u - accu_hazard) * pow(r, 0.5 * parameter(1)) / parameter(0);
  return t_add;
}

//' Simulate distance sampling survey
//'
//' @param true_parameter (detection shape, scale, diffusion sd)
//' @param N number of animals
//' @param auxiliary_info (region x-extent ,region y-extent, survey time, dt,
//' transect type (0 = point, 1 = line), observer_speed, number of transects, half width of transects)
//' @param dt time step
//' @param move = 0 no mvoement, 1 Brownian motion
//' @return Outputs csv data file
// [[Rcpp::export]]
int SimulateDsData(arma::vec true_parameter,
                   int N,
                   arma::vec auxiliary_info,
                   double dt,
                   int move = 0) {
  arma::vec area = auxiliary_info.rows(0, 1);
  double half_width = auxiliary_info(2) * 0.5;
  double survey_time = auxiliary_info(4);
  double transect_type = auxiliary_info(7);
  double observer_speed = auxiliary_info(5);
  int num_transects = auxiliary_info(6);
  arma::vec parameter(true_parameter);
  if (transect_type == 0) {
    parameter(0) = 2.0 * pow(parameter(0), parameter(1)) / R::beta(0.5 * parameter(1), 0.5);
  } else {
    parameter(0) = pow(parameter(0), parameter(1));
  }
  if (transect_type == 0) ++parameter(1);
  arma::vec observer_position(2);
  observer_position(0) = area(0) * 0.5;
  observer_position(1) = area(1) * 0.5;
  arma::vec detection_parameter(parameter.rows(0,1));
  double sd = parameter(2);
  // open data file
  std::ofstream data("./simulated_dsdata.csv");
  arma::vec x(N);
  arma::vec y(N);
  arma::vec u(N);
  arma::vec detected(N);
  arma::vec accu_hazard(N);
  double recorded_y;
  double recorded_t;
  double recorded_r;
  double hazard;
  bool include;
  for (int transect = 0; transect < num_transects; ++transect) {
    if (transect_type == 0) observer_position(1) = 0.0;
    x = arma::randu(N) * area(0);
    y = arma::randu(N) * area(1);
    u = -log(arma::randu(N));
    detected.zeros();
    accu_hazard.zeros();
    for (int t = 0; t < floor(survey_time / dt); ++t) {
      for (int i = 0; i < N; ++i) {
        hazard = CalcHazard2(x(i) - observer_position(0), y(i) -
          observer_position(1), dt, observer_speed, parameter, half_width, transect_type);
        if ((accu_hazard(i) + hazard >= u(i)) & (accu_hazard(i) < u(i))) {
          include = false;
          if (transect_type == 0) {
            recorded_y = GetRecordedYPosition(x(i) - observer_position(0),
                                              y(i) - observer_position(1), accu_hazard(i), u(i),
                                              parameter);
            recorded_t = t * dt + (y(i) - observer_position(1) - recorded_y) /
              observer_speed;
            if ((recorded_t < survey_time) & (fabs(x(i) - observer_position(0)) < half_width)) include = true;
          } else {
            recorded_y = y(i) - observer_position(1);
            recorded_t = t * dt + GetRecordedT(x(i) - observer_position(0),
                                               y(i) - observer_position(1),
                                               accu_hazard(i),
                                               u(i),
                                               parameter);
            recorded_r = pow(x(i) - observer_position(0), 2) + recorded_y * recorded_y;
            if (recorded_r <= half_width * half_width) include = true;
          }
          if (include) {
            data << transect + 1 << "," << x(i) - observer_position(0) << "," << recorded_y << "," <<
              recorded_t << "\n";
          }
        }
        accu_hazard(i) += hazard;
        // assume uniform initial behaviour distribution
        if (move == 1) {
          x(i) += sd * sqrt(dt) * R::rnorm(0, 1);
          y(i) += sd * sqrt(dt) * R::rnorm(0, 1);
        }
      }
      observer_position(1) += observer_speed * dt;
      arma::uvec outside = find(x < 0 || x > area(0) || y < 0 || y > area(1));
      arma::vec u2(arma::randu(N));
      u(outside) = u2(outside);
      accu_hazard(outside).fill(0.0);
      x(find(x < 0)) += area(0);
      x(find(x > area(0))) -= area(0);
      y(find(y < 0)) += area(1);
      y(find(y > area(1))) -= area(1);
    }
  }
  data.close();
  return 0;
}


/**
 * @file 	MyDijkstra.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 10, 2017
 */

#include <boost/heap/fibonacci_heap.hpp>
#include "MyDijkstra.h"

using std::pair;

using Pair = pair<int, double>;
typedef graph_traits<Graph>::vertex_descriptor Vertex;

typename graph_traits<Graph>::out_edge_iterator out_i, out_end;

/**
 * Data that is stored in one node of a heap. Contains an integer and a double.
 * Comparisons are made by the double, smaller has higher priority
 */
struct heap_data
{
    heap::fibonacci_heap<heap_data>::handle_type handle;
    Pair pair;

    heap_data(Pair p):
        pair(p)
    {}

    bool operator<(heap_data const & rhs) const {
        return pair.second > rhs.pair.second;
    }
};

using Heap = heap::fibonacci_heap<heap_data>;

void MyDijkstra::computeShortestPaths(Graph& g, int numVertices, int source,
		double distances[], int predecessors[]) {

	predecessors[source] = source;

	Heap heap;

	Heap::handle_type *handles = new Heap::handle_type[numVertices];

	//initialization of the heap
	for (int i = 0; i < numVertices; ++i) {
		if (i == source) {
			handles[i] = heap.push(std::make_pair(i,0.));
			distances[i] = 0.;
		} else {
			handles[i] = heap.push(std::make_pair(i, std::numeric_limits<double>::infinity()));
			distances[i] = std::numeric_limits<double>::infinity();
		}
	}

	property_map<Graph, edge_weight_t>::type weights = get(edge_weight, g);
	property_map<Graph, vertex_index_t>::type index = get(vertex_index, g);

	//the actual algorithm
	while (!heap.empty()) {
		Pair min = heap.top().pair;
		heap.pop();
		for (tie(out_i, out_end) = out_edges(*(vertices(g).first+min.first), g);
				out_i != out_end; ++out_i) {
			double tmp = min.second + weights[*out_i];
			int targetIndex = index[target(*out_i, g)];
			if (tmp < distances[targetIndex]) {
				distances[targetIndex] = tmp;
				predecessors[targetIndex] = min.first;
				(*handles[targetIndex]).pair.second = tmp;
				heap.increase(handles[targetIndex]);
			}
		}
	}

	delete[] handles;
}


/* +---------------------------------------------------------------------------+
|                     Mobile Robot Programming Toolkit (MRPT)               |
|                          http://www.mrpt.org/                             |
|                                                                           |
| Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
| See: http://www.mrpt.org/Authors - All rights reserved.                   |
| Released under BSD License. See details in http://www.mrpt.org/License    |
+---------------------------------------------------------------------------+ */

#include "vision-precomp.h"   // Precompiled headers

#include <iostream>
#include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
#include <Eigen/Dense>
#include <Eigen/SVD>

#include "ppnp.h"

mrpt::vision::pnp::ppnp::ppnp(const Eigen::MatrixXd& obj_pts, const Eigen::MatrixXd& img_pts, const Eigen::MatrixXd& cam_intrinsic)
{
	P = img_pts;
	S = obj_pts;
	C = cam_intrinsic;
}

bool mrpt::vision::pnp::ppnp::compute_pose(Eigen::Matrix3d& R, Eigen::Vector3d& t, int n)
{
	double tol=0.00001;

	Eigen::MatrixXd I=Eigen::MatrixXd::Identity(n, n), A(n,n), Y(n,3), E(n,3), E_old(n,3), U,V, I3 =Eigen::MatrixXd::Identity(3, 3), PR, Z=Eigen::MatrixXd::Zero(n, n);
	Eigen::VectorXd e(n), II(n), c(3), Zmindiag(n);

	e.fill(1);
	II.fill(1.0/((double)n));

	A=I-e*e.transpose()/n;

	double err=std::numeric_limits<double>::infinity();

	E_old.fill(1000);

    int cnt =0;

	while(err>tol)
	{

		Eigen::JacobiSVD<Eigen::MatrixXd> svd(P.transpose()*Z*A*S, Eigen::ComputeThinU | Eigen::ComputeThinV);
		U=svd.matrixU();
		V=svd.matrixV();

		I3(2,2) = (U*V.transpose()).determinant();
		R=U*I3*V.transpose();
		PR = P*R;

		c=(S-Z*PR).transpose()*II;

		Y=S-e*c.transpose();

		Zmindiag=((PR*Y.transpose()).diagonal()).array() / ((P.array()*P.array()).rowwise().sum()).array();

		for (int i = 0; i < n; i++)
			if (Zmindiag(i) < 0)
				Zmindiag(i) = 0;

		Z=Zmindiag.asDiagonal();

		E=Y-Z*PR;

		err=(E-E_old).norm();

		E_old=E;

        cnt ++;

	}

	t=-R*c;

	return 1;

}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic sine: \f$(e^{x}-e^{-x})/2\f$.

    @par Header <boost/simd/function/sinh.hpp>

    @see tanh, cosh, sech, csch, sinhcosh

    @par Example:

      @snippet sinh.cpp sinh

    @par Possible output:

      @snippet sinh.txt sinh
  **/
  IEEEValue sinh(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sinh.hpp>
#include <boost/simd/function/simd/sinh.hpp>

#endif