#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>

#include <algorithm>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <iostream>
#include <thread>
#include <unistd.h>
#include <string.h>

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

#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>

#include <Eigen/Eigen>
#include <AlignTrajectory.h>
#include <Viewer.h>
#include <pangolin/pangolin.h>

using namespace cv;
using namespace std;

enum GTformat  { TUM, VISIM, ICLNUIM };

bool readFiles(const std::string& strGroundTruth,
               const std::string& strEstimate,
               std::vector<std::pair<double, Eigen::Matrix4d>>& vGroundTruthTimed,
               std::vector<std::pair<double, Eigen::Matrix4d>>& vEstimateTimed,
               const std::string& strGTformat);

Eigen::Matrix3d quat2mat(float qx, float qy, float qz, float qw);

int main(int argc, char* argv[])
{
    for(int i = 0; i < argc; ++i)
        printf("Argument %d : %s\n", i, argv[i]);

    if(argc < 4)
    {
        std::cout << "USAGE: ./align [ground_truth_trajectory] [estime_trajectory] [dataset format: TUM, VISIM, ICLNUIM]" << std::endl;
        return 1;
    }

    std::string strGroundTruth = std::string(argv[1]);
    std::string strEstimate    = std::string(argv[2]);
    std::string strGTformat    = std::string(argv[3]);

    int iDriftRange  = 1;   //for RPE evaluation, number of frames to evaluate the drift over

    std::vector<std::pair<double, Eigen::Matrix4d>> vGroundTruthTimed;
    std::vector<std::pair<double, Eigen::Matrix4d>> vEstimateTimed;
    std::vector<std::pair<double, Eigen::Matrix4d>> vTransformedTimed;

    std::vector<Eigen::Matrix4d> vGroundTruth;
    std::vector<Eigen::Matrix4d> vEstimate;
    std::vector<Eigen::Matrix4d> vTransformed;

    readFiles(strGroundTruth, strEstimate, vGroundTruthTimed, vEstimateTimed, strGTformat);

    // calculte ATE
    float ate=0;
    AlignTrajectory align;

    bool bDoAssociation;
    if(vGroundTruthTimed.size() == vEstimateTimed.size())
        bDoAssociation = false;
    else
        bDoAssociation = true;

    Eigen::Matrix4d Mat = align.calculateATE(vGroundTruthTimed, vEstimateTimed, ate, bDoAssociation);

    std::cout << "M is : " << std::endl << Mat << std::endl;
    std::cout << "ATE is: " << std::endl << ate << std::endl;

    // generate the aligned trajectory
    Eigen::Matrix3d scaledRotation = Mat.block<3,3>(0,0);
    Eigen::Vector3d translation    = Mat.block<3,1>(0,3);
    vTransformedTimed.clear();

    for(int i = 0 ; i < vEstimateTimed.size(); i++)
    {
        std::pair<double, Eigen::Matrix4d> poseTimed;
        poseTimed = std::make_pair(vEstimateTimed[i].first, (Mat.inverse())*vEstimateTimed[i].second);
        vTransformedTimed.push_back(poseTimed);
    }

    // calculate RPE
    int iDeltaFrames;
    double  rpe_rmse;

    std::vector<std::pair<double, Eigen::Matrix4d>>  rpe = align.calculateRPE(vGroundTruthTimed, vEstimateTimed, iDriftRange, rpe_rmse, bDoAssociation);

    if(!(rpe.size() == 0))
    {
        std::cout << "RPE is: " << std::endl << rpe_rmse << std::endl;

	ofstream f;
	string filename;;
	filename = "RGBDRPE.txt";
	f.open(filename.c_str());
	f << fixed;

	for (int i = 0; i < rpe.size(); i++)
	{
		Eigen::Matrix4d poseEr = rpe[i].second;
		f << setprecision(6) <<  rpe[i].first
	           << setprecision(7)
        	   << " "
        	   << poseEr(0,3) << " "
        	   << poseEr(1,3) << " "
        	   << poseEr(2,3) << " "
        	   //<< q.x() << " " << q.y() << " " << q.z() << " " << q.w()
		   << endl;
	}
	f.close();
    }
    else
        std::cout << "RPE is: " << std::endl << "N.A." << std::endl;

    // plot results
    Viewer* mpViewer;
    std::thread* mptViewer;

    mpViewer = new Viewer(&vGroundTruthTimed, &vEstimateTimed, &vTransformedTimed);
    mptViewer = new thread(&Viewer::Run, mpViewer);

    getchar();
    cout << "Done!" << endl;

    if(mpViewer)
    {
        mpViewer->RequestStop();
        while(!mpViewer->isStopped())
            sleep(1);
    }

/*    if(mpViewer)
        pangolin::BindToContext("Trajectory");*/

    return 0;
}

Eigen::Matrix3d quat2mat(float qx, float qy, float qz, float qw)
{
    Eigen::Matrix3d rot;

    rot(0,0) = 1.0;
    rot(0,1) = 0.0;
    rot(0,2) = 0.0;

    rot(1,0) = 0.0;
    rot(1,1) = 1.0;
    rot(1,2) = 0.0;

    rot(2,0) = 0.0;
    rot(2,1) = 0.0;
    rot(2,2) = 1.0;

    return rot;
}

bool readFiles(const std::string& strGroundTruth,
               const std::string& strEstimate,
               std::vector<std::pair<double, Eigen::Matrix4d>>& vGroundTruthTimed,
               std::vector<std::pair<double, Eigen::Matrix4d>>& vEstimateTimed,
               const std::string& strGTformat)
{
    std::ifstream gtFile(strGroundTruth);
    std::ifstream estimateFile(strEstimate);

    std::string line;
    boost::smatch match;

    if(strGTformat.compare("VISIM") == 0)
    {
	    while (std::getline(gtFile, line))
	    {
		if (line.size() == 0) {
		continue;
		}
		else if(boost::regex_match(line,match,boost::regex("^\\s*#.*$")))
		{
		continue;
		}
		else if (boost::regex_match(line,match,boost::regex("^([0-9]+),+([-0-9.e]+),+([-0-9.e]+),+([-0-9.e]+),+([-0-9.e]+),+([-0-9.e]+),+([-0-9.e]+),+([-0-9.e]+).*")))
		{
                    double time =  std::stod(match[1]);

                    float tx =  std::stof(match[2]);
		    float ty =  std::stof(match[4]);
		    float tz =  std::stof(match[3]);

		    float qx =  std::stof(match[6]);
		    float qy =  std::stof(match[7]);
		    float qz =  std::stof(match[8]);
		    float qw =  std::stof(match[5]);

		    Eigen::Matrix3d rot = quat2mat(qx, qy, qz, qw);

		    Eigen::Matrix4d pose6d;
		    pose6d.block<3,3>(0,0) = rot;
		    pose6d.block<3,1>(0,3) << tx, ty, tz;
		    pose6d.block<1,4>(3,0) << 0.0, 0.0, 0.0, 1.0;

                    std::pair<double, Eigen::Matrix4d> pose6dTimed (time, pose6d);

		    vGroundTruthTimed.push_back(pose6dTimed);
		 }
		 else
		 {
		     std::cerr << "Unknown line:" << line << std::endl;
		     return false;
		 }
	    }
	    std::cout << "Number of ground truth poses: " << vGroundTruthTimed.size() << std::endl;
    }

    if(strGTformat.compare("TUM") == 0)
    {
	    while (std::getline(gtFile, line))
	    {
		if (line.size() == 0) {
		continue;
		}
		else if(boost::regex_match(line,match,boost::regex("^\\s*#.*$")))
		{
		continue;
		}
                else if (boost::regex_match(line,match,boost::regex("^([0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+).*")))
		{

                    double time =  std::stod(match[1]);

		    float tx =  std::stof(match[2]);
		    float ty =  std::stof(match[3]);
		    float tz =  std::stof(match[4]);

		    float qx =  std::stof(match[5]);
		    float qy =  std::stof(match[6]);
		    float qz =  std::stof(match[7]);
		    float qw =  std::stof(match[8]);

		    Eigen::Matrix3d rot = quat2mat(qx, qy, qz, qw);

		    Eigen::Matrix4d pose6d;
		    pose6d.block<3,3>(0,0) = rot;
		    pose6d.block<3,1>(0,3) << tx, ty, tz;
		    pose6d.block<1,4>(3,0) << 0.0, 0.0, 0.0, 1.0;

		    //vGroundTruth.push_back(pose6d);

                    std::pair<double, Eigen::Matrix4d> pose6dTimed (time, pose6d);

		    vGroundTruthTimed.push_back(pose6dTimed);

		 }
		 else
		 {
		     std::cerr << "Unknown line in reading groundtruth poses: " << line << std::endl;
		     return false;
		 }
	    }
	    std::cout << "Number of ground truth poses: " << vGroundTruthTimed.size() << std::endl;
    }

    while (std::getline(estimateFile, line))
    {
        if (line.size() == 0) {
        continue;
        }
        else if(boost::regex_match(line,match,boost::regex("^\\s*#.*$")))
        {
        continue;
        }
        else if (boost::regex_match(line,match,boost::regex("^([0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+)\\s+([-0-9.e]+).*")))
        {

            double time =  std::stod(match[1]);

            float tx =  std::stof(match[2]);
            float ty =  std::stof(match[4]);
            float tz =  std::stof(match[3]);

            float qx =  std::stof(match[5]);
            float qy =  std::stof(match[6]);
            float qz =  std::stof(match[7]);
            float qw =  std::stof(match[8]);

            Eigen::Matrix3d rot = quat2mat(qx, qy, qz, qw);

            Eigen::Matrix4d pose6d;
            pose6d.block<3,3>(0,0) = rot;
            pose6d.block<3,1>(0,3) << tx, ty, tz;
            pose6d.block<1,4>(3,0) << 0.0, 0.0, 0.0, 1.0;

//            vEstimate.push_back(pose6d);

                    std::pair<double, Eigen::Matrix4d> pose6dTimed (time, pose6d);

		    vEstimateTimed.push_back(pose6dTimed);
         }
         else
         {
             std::cerr << "Unknown line in reading estimated poses: " << line << std::endl;
             return false;
         }
    }

    std::cout << "Number of estimate poses: " << vEstimateTimed.size() << std::endl;

    return true;
}


// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka/rate_limiting.h>

#include <Eigen/Dense>

#include <franka/control_tools.h>

namespace franka {

namespace {

Eigen::Vector3d limitRate(double max_velocity,
                          double max_acceleration,
                          double max_jerk,
                          const Eigen::Vector3d& commanded_velocity,
                          const Eigen::Vector3d& last_commanded_velocity,
                          const Eigen::Vector3d& last_commanded_acceleration) {
  // Differentiate to get jerk
  Eigen::Vector3d commanded_jerk =
      (((commanded_velocity - last_commanded_velocity) / kDeltaT) - last_commanded_acceleration) /
      kDeltaT;

  // Limit jerk and integrate to get desired acceleration
  Eigen::Vector3d commanded_acceleration = last_commanded_acceleration;
  if (commanded_jerk.norm() > kNormEps) {
    commanded_acceleration += (commanded_jerk / commanded_jerk.norm()) *
                              std::max(std::min(commanded_jerk.norm(), max_jerk), -max_jerk) *
                              kDeltaT;
  }

  // Compute Euclidean distance to the max velocity vector that would be reached starting from
  // last_commanded_velocity with the direction of the desired acceleration
  Eigen::Vector3d unit_commanded_acceleration =
      commanded_acceleration / commanded_acceleration.norm();
  double dot_product = unit_commanded_acceleration.transpose() * last_commanded_velocity;
  double distance_to_max_velocity =
      -dot_product + std::sqrt(pow(dot_product, 2.0) - last_commanded_velocity.squaredNorm() +
                               pow(max_velocity, 2.0));

  // Compute safe acceleration limits
  double safe_max_acceleration =
      std::min((max_jerk / max_acceleration) * distance_to_max_velocity, max_acceleration);

  // Limit acceleration and integrate to get desired velocities
  Eigen::Vector3d limited_commanded_velocity = last_commanded_velocity;

  if (commanded_acceleration.norm() > kNormEps) {
    limited_commanded_velocity += unit_commanded_acceleration *
                                  std::min(commanded_acceleration.norm(), safe_max_acceleration) *
                                  kDeltaT;
  }

  return limited_commanded_velocity;
}

}  // anonymous namespace

std::array<double, 7> limitRate(const std::array<double, 7>& max_derivatives,
                                const std::array<double, 7>& commanded_values,
                                const std::array<double, 7>& last_commanded_values) {
  if (!std::all_of(commanded_values.begin(), commanded_values.end(),
                   [](double d) { return std::isfinite(d); })) {
    throw std::invalid_argument("Commanding value is infinite or NaN.");
  }
  std::array<double, 7> limited_values{};
  for (size_t i = 0; i < 7; i++) {
    double commanded_derivative = (commanded_values[i] - last_commanded_values[i]) / kDeltaT;
    limited_values[i] =
        last_commanded_values[i] +
        std::max(std::min(commanded_derivative, max_derivatives[i]), -max_derivatives[i]) * kDeltaT;
  }
  return limited_values;
}

double limitRate(double max_velocity,
                 double max_acceleration,
                 double max_jerk,
                 double commanded_velocity,
                 double last_commanded_velocity,
                 double last_commanded_acceleration) {
  if (!std::isfinite(commanded_velocity)) {
    throw std::invalid_argument("commanded_velocity is infinite or NaN.");
  }
  // Differentiate to get jerk
  double commanded_jerk =
      (((commanded_velocity - last_commanded_velocity) / kDeltaT) - last_commanded_acceleration) /
      kDeltaT;

  // Limit jerk and integrate to get acceleration
  double commanded_acceleration = last_commanded_acceleration +
                                  std::max(std::min(commanded_jerk, max_jerk), -max_jerk) * kDeltaT;

  // Compute acceleration limits
  double safe_max_acceleration = std::min(
      (max_jerk / max_acceleration) * (max_velocity - last_commanded_velocity), max_acceleration);
  double safe_min_acceleration = std::max(
      (max_jerk / max_acceleration) * (-max_velocity - last_commanded_velocity), -max_acceleration);

  // Limit acceleration and integrate to get desired velocities
  return last_commanded_velocity +
         std::max(std::min(commanded_acceleration, safe_max_acceleration), safe_min_acceleration) *
             kDeltaT;
}

double limitRate(double max_velocity,
                 double max_acceleration,
                 double max_jerk,
                 double commanded_position,
                 double last_commanded_position,
                 double last_commanded_velocity,
                 double last_commanded_acceleration) {
  if (!std::isfinite(commanded_position)) {
    throw std::invalid_argument("commanded_position is infinite or NaN.");
  }
  return last_commanded_position +
         limitRate(max_velocity, max_acceleration, max_jerk,
                   (commanded_position - last_commanded_position) / kDeltaT,
                   last_commanded_velocity, last_commanded_acceleration) *
             kDeltaT;
}

std::array<double, 7> limitRate(const std::array<double, 7>& max_velocity,
                                const std::array<double, 7>& max_acceleration,
                                const std::array<double, 7>& max_jerk,
                                const std::array<double, 7>& commanded_velocities,
                                const std::array<double, 7>& last_commanded_velocities,
                                const std::array<double, 7>& last_commanded_accelerations) {
  if (!std::all_of(commanded_velocities.begin(), commanded_velocities.end(),
                   [](double d) { return std::isfinite(d); })) {
    throw std::invalid_argument("commanded_velocities is infinite or NaN.");
  }
  std::array<double, 7> limited_commanded_velocities{};

  for (size_t i = 0; i < 7; i++) {
    limited_commanded_velocities[i] =
        limitRate(max_velocity[i], max_acceleration[i], max_jerk[i], commanded_velocities[i],
                  last_commanded_velocities[i], last_commanded_accelerations[i]);
  }
  return limited_commanded_velocities;
}

std::array<double, 7> limitRate(const std::array<double, 7>& max_velocity,
                                const std::array<double, 7>& max_acceleration,
                                const std::array<double, 7>& max_jerk,
                                const std::array<double, 7>& commanded_positions,
                                const std::array<double, 7>& last_commanded_positions,
                                const std::array<double, 7>& last_commanded_velocities,
                                const std::array<double, 7>& last_commanded_accelerations) {
  if (!std::all_of(commanded_positions.begin(), commanded_positions.end(),
                   [](double d) { return std::isfinite(d); })) {
    throw std::invalid_argument("commanded_positions is infinite or NaN.");
  }
  std::array<double, 7> limited_commanded_positions{};
  for (size_t i = 0; i < 7; i++) {
    limited_commanded_positions[i] = limitRate(
        max_velocity[i], max_acceleration[i], max_jerk[i], commanded_positions[i],
        last_commanded_positions[i], last_commanded_velocities[i], last_commanded_accelerations[i]);
  }
  return limited_commanded_positions;
}

std::array<double, 6> limitRate(
    double max_translational_velocity,
    double max_translational_acceleration,
    double max_translational_jerk,
    double max_rotational_velocity,
    double max_rotational_acceleration,
    double max_rotational_jerk,
    const std::array<double, 6>& O_dP_EE_c,          // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_dP_EE_c,     // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_ddP_EE_c) {  // NOLINT(readability-identifier-naming)
  if (!std::all_of(O_dP_EE_c.begin(), O_dP_EE_c.end(), [](double d) { return std::isfinite(d); })) {
    throw std::invalid_argument("O_dP_EE_c is infinite or NaN.");
  }
  Eigen::Matrix<double, 6, 1> dx(O_dP_EE_c.data());
  Eigen::Matrix<double, 6, 1> last_dx(last_O_dP_EE_c.data());
  Eigen::Matrix<double, 6, 1> last_ddx(last_O_ddP_EE_c.data());

  dx.head(3) << limitRate(max_translational_velocity, max_translational_acceleration,
                          max_translational_jerk, dx.head(3), last_dx.head(3), last_ddx.head(3));
  dx.tail(3) << limitRate(max_rotational_velocity, max_rotational_acceleration, max_rotational_jerk,
                          dx.tail(3), last_dx.tail(3), last_ddx.tail(3));

  std::array<double, 6> limited_values{};
  Eigen::Map<Eigen::Matrix<double, 6, 1>>(&limited_values[0], 6, 1) = dx;
  return limited_values;
}

std::array<double, 16> limitRate(
    double max_translational_velocity,
    double max_translational_acceleration,
    double max_translational_jerk,
    double max_rotational_velocity,
    double max_rotational_acceleration,
    double max_rotational_jerk,
    const std::array<double, 16>& O_T_EE_c,          // NOLINT(readability-identifier-naming)
    const std::array<double, 16>& last_O_T_EE_c,     // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_dP_EE_c,     // NOLINT(readability-identifier-naming)
    const std::array<double, 6>& last_O_ddP_EE_c) {  // NOLINT(readability-identifier-naming)
  if (!std::all_of(O_T_EE_c.begin(), O_T_EE_c.end(), [](double d) { return std::isfinite(d); })) {
    throw std::invalid_argument("O_T_EE_c is infinite or NaN.");
  }
  if (!isHomogeneousTransformation(O_T_EE_c)) {
    throw std::invalid_argument(
        "O_T_EE_c is invalid transformation matrix. Has to be column major!");
  }
  Eigen::Matrix<double, 6, 1> dx;
  Eigen::Affine3d commanded_pose(Eigen::Matrix4d::Map(O_T_EE_c.data()));
  Eigen::Affine3d limited_commanded_pose = Eigen::Affine3d::Identity();
  Eigen::Affine3d last_commanded_pose(Eigen::Matrix4d::Map(last_O_T_EE_c.data()));

  // Compute translational velocity
  dx.head(3) << (commanded_pose.translation() - last_commanded_pose.translation()) / kDeltaT;

  // Compute rotational velocity
  Eigen::AngleAxisd rot_difference(commanded_pose.linear() *
                                   last_commanded_pose.linear().transpose());
  dx.tail(3) << rot_difference.axis() * rot_difference.angle() / kDeltaT;

  // Limit the rate of the twist
  std::array<double, 6> commanded_O_dP_EE_c{};  // NOLINT(readability-identifier-naming)
  Eigen::Map<Eigen::Matrix<double, 6, 1>>(&commanded_O_dP_EE_c[0], 6, 1) = dx;
  commanded_O_dP_EE_c =
      limitRate(max_translational_velocity, max_translational_acceleration, max_translational_jerk,
                kFactorCartesianRotationPoseInterface * max_rotational_velocity,
                kFactorCartesianRotationPoseInterface * max_rotational_acceleration,
                kFactorCartesianRotationPoseInterface * max_rotational_jerk, commanded_O_dP_EE_c,
                last_O_dP_EE_c, last_O_ddP_EE_c);
  dx = Eigen::Matrix<double, 6, 1>(commanded_O_dP_EE_c.data());

  // Integrate limited twist
  limited_commanded_pose.translation() << last_commanded_pose.translation() + dx.head(3) * kDeltaT;
  limited_commanded_pose.linear() << last_commanded_pose.linear();
  if (dx.tail(3).norm() > kNormEps) {
    Eigen::Matrix3d omega_skew;
    Eigen::Vector3d w_norm(dx.tail(3) / dx.tail(3).norm());
    double theta = kDeltaT * dx.tail(3).norm();
    omega_skew << 0, -w_norm(2), w_norm(1), w_norm(2), 0, -w_norm(0), -w_norm(1), w_norm(0), 0;
    // NOLINTNEXTLINE(readability-identifier-naming)
    Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + sin(theta) * omega_skew +
                        (1.0 - cos(theta)) * (omega_skew * omega_skew);
    limited_commanded_pose.linear() << R * last_commanded_pose.linear();
  }

  std::array<double, 16> limited_values{};
  Eigen::Map<Eigen::Matrix4d>(&limited_values[0], 4, 4) = limited_commanded_pose.matrix();
  return limited_values;
}

}  // namespace franka


#include <bits/stdc++.h>
#include <boost/optional.hpp>

using namespace std;

boost::optional<long long> dijkstra(
    const vector<vector<tuple<long long, long long>>> &graph,
    long long from_i,
    long long to_i) {
  const long long INF = 1e18;

  vector<long long> dist(graph.size(), INF);
  dist[from_i] = 0;

  priority_queue<tuple<long long, long long>, vector<tuple<long long, long long>>, greater<>> que;
  que.push(make_tuple(0, from_i));
  while (!que.empty()) {
    long long d, v;
    tie(d, v) = que.top();
    que.pop();
    if (d > dist[v]) continue;
    if (v == to_i) return d;

    for (const auto &edge : graph[v]) {
      long long w, u;
      tie(u, w) = edge;
      if (d + w < dist[u]) {
        dist[u] = d + w;
        que.push(make_tuple(d + w, u));
      }
    }
  }

  if (dist[to_i] < INF) {
    return dist[to_i];
  } else {
    return boost::none;
  }
}

vector<long long> topological_sort(const vector<vector<long long>> &graph) {
  long long size = graph.size();

  // 入次数
  vector<long long> ins(size, 0);

  for (auto &&vs : graph) {
    for (auto &&v : vs) {
      ins[v]++;
    }
  }

  // 入次数がゼロのやつを集める
  vector<long long> zeros;
  for (long long v = 0; v < size; ++v) {
    if (ins[v] == 0) {
      zeros.push_back(v);
    }
  }

  // ゼロのやつから追加してく
  vector<long long> ret;
  while (!zeros.empty()) {
    long long v = zeros.back();
    zeros.pop_back();
    ret.push_back(v);
    for (auto &&u:  graph[v]) {
      ins[u]--;
      if (ins[u] == 0) {
        zeros.push_back(u);
      }
    }
  }
  // 閉路があると入次数が絶対ゼロにならない
  if (ret.size() != size) {
    throw invalid_argument("閉路があります");
  }

  return ret;
}


#include "CEGO/CEGO.hpp"
#include "CEGO/minimizers.hpp"
#include "CEGO/utilities.hpp"
#include <Eigen/Dense>

// autodiff include
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>

#if defined(PYBIND11)
#include <pybind11/embed.h>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
namespace py = pybind11;
#endif

std::atomic_size_t Ncalls(0);

template <typename T> using EArray = Eigen::Array<T, Eigen::Dynamic, 1>;

class Bumps {
public:
    std::size_t Nbumps;
    Eigen::ArrayXd c0, xp, yp, zp;
    double gamma = 10;
    const std::vector<CEGO::Bound> m_bounds;

    Bumps(std::size_t Nbumps, std::size_t Npoints, const std::vector<CEGO::Bound> &bounds) : Nbumps(Nbumps), m_bounds(bounds)
    {
        // Initialize the random number generator
        std::random_device rd;  // Will be used to obtain a seed for the random number engine
        std::mt19937 gen(rd()); // Standard mersenne_twister_engine seeded with rd()

        // Calculate the initial set of coefficients for the bump characteristics
        c0.resize(Nbumps * 6);
        for (auto i = 0; i < bounds.size(); ++i) {
            double dbl; int integer;
            bounds[i].gen_uniform(gen, dbl, integer);
            if (bounds[i].m_lower.type == bounds[i].m_lower.DOUBLE){
                c0[i] = dbl;
            }
            else {
                c0[i] = static_cast<double>(integer);
            }
        }

        // Generate some random points in the domain [0.1,1.0] for both variables
        xp = (1-0.1)*(Eigen::ArrayXd::Random(Npoints)+1)/2 + 0.1;
        yp = (1-0.1)*(Eigen::ArrayXd::Random(Npoints)+1)/2 + 0.1;
        zp = f_givenxy(c0, xp, yp);

        double checkval = objective(to_scaled(c0));
        std::cout << "c0: " << c0 << std::endl;

        assert(std::abs(checkval) < 1e-16);
        if (std::abs(checkval) > 1e-16) {
            throw std::invalid_argument("Did not start out with zero objective function!");
        }
    }
    /**
     * @brief Calculate the functional value for a set of vectors of points
     * @brief xb The x coordinate of the center of the bump
     * @brief x The x coordinate of the points to be evaluated
     * @brief y The y coordinate of the points to be evaluated
     */
    template <typename T>
    EArray<T> f_givenxy(const EArray<T> &c, const EArray<T> &x, const EArray<T> &y)
    {
        Ncalls++;
        EArray<T> s = EArray<T>::Zero(x.size());
        auto chunksize = 6;
        assert(c.size()%chunksize==0);
        for (long i = 0; i < c.size(); i += chunksize) {
            s += x.pow(c[i+0])*y.pow(c[i+1])*(c[i+2]*(x-c[i+3]).square() +c[i+4]*(y-c[i+5]).square()).exp();
        }
        return s.eval();
    }
    double objective(const CEGO::AbstractIndividual *pind) {
        const EArray<CEGO::numberish> &c = static_cast<const CEGO::NumericalIndividual<CEGO::numberish>*>(pind)->get_coefficients();
        Eigen::ArrayXd cc(c.size());
        for (auto i = 0; i < cc.size(); ++i) {
            cc[i] = c[i];
        }
        return objective(cc);
    }
    double objective(const EArray<double>& cscaled) {
        return (f_givenxy<double>(to_realworld<double>(cscaled), xp, yp) - zp).square().sum();
    }
    std::complex<double> objective(const EArray<std::complex<double>>& cscaled) {
        return (f_givenxy<std::complex<double>>(to_realworld<std::complex<double>>(cscaled), xp, yp) - zp).square().sum();
    }

    autodiff::dual objective(const EArray<autodiff::dual>& cscaled) {
        EArray<autodiff::dual> creal = to_realworld(cscaled);
        EArray<autodiff::dual> zmodel = f_givenxy<autodiff::dual>(creal, xp, yp);
        EArray<autodiff::dual> err = zmodel - zp.cast<autodiff::dual>();
        return err.square().sum();
    }

    // Inspired by scipy, keep all variables scaled in 0,1
    template <typename T>
    EArray<T> to_realworld(const EArray<T>&x){
        EArray<T> o(x.size());
        for (auto i = 0; i < o.size(); ++i){
            if constexpr (std::is_same<T, std::complex<double>>::value) {
                // If complex<double> type, first cast the numberish bounds to double, then to complex
                // Otherwise compiler gets confused
                T lower = static_cast<T>(static_cast<double>(m_bounds[i].m_lower));
                T upper = static_cast<T>(static_cast<double>(m_bounds[i].m_upper));
                o[i] = lower * (static_cast<T>(1.0) - x[i]) + upper * x[i];
            }
            else {
                T lower = static_cast<T>(m_bounds[i].m_lower);
                T upper = static_cast<T>(m_bounds[i].m_upper);
                o[i] = lower * (static_cast<T>(1.0) - x[i]) + upper * x[i];
            }
        }
        return o.eval();
    }
    Eigen::ArrayXd to_scaled(const Eigen::ArrayXd &x) {
        Eigen::ArrayXd o(x.size());
        for (auto i = 0; i < o.size(); ++i) {
            double lower = static_cast<double>(m_bounds[i].m_lower);
            double upper = static_cast<double>(m_bounds[i].m_upper);
            o[i] = (x[i]-lower)/(upper-lower);
        }
        return o;
    }

    void plot_surface() {
        #if defined(PYBIND11)
        using namespace pybind11::literals;
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        std::size_t Nx = 100, Ny = 100;
        Eigen::MatrixXd X = Eigen::RowVectorXd::LinSpaced(Nx, 0.1, 1).replicate(Ny, 1);
        Eigen::MatrixXd Y = Eigen::VectorXd::LinSpaced(Ny, 0.1, 1).replicate(Nx, 1);
        X.resize(Nx*Ny,1); Y.resize(Nx*Ny,1);
        Eigen::MatrixXd Z = f_givenxy(c0, X.array(), Y.array()).matrix();
        X.resize(Nx, Ny); Y.resize(Nx, Ny); Z.resize(Nx, Ny);

        Eigen::ArrayXd levels = Eigen::ArrayXd::LinSpaced(300, Z.minCoeff(), Z.maxCoeff());
        try{
            plt.attr("contourf")(X, Y, Z, levels);
        }
        catch (std::exception &e) {
            std::cout << e.what()  << std::endl;
        }
        plt.attr("colorbar")();
        plt.attr("scatter")(xp, yp);
        plt.attr("show")();
        #else
        std::cout << "No support for pybind11, so no plots\n";
        #endif
    }
    void plot_trace(const std::vector<double> &best_costs) {
        #if defined(PYBIND11)
        py::module plt = py::module::import("matplotlib.pyplot"); // Import matplotlib
        plt.attr("plot")(best_costs);
        plt.attr("show")();
        #else
        std::cout << "No support for pybind11, so no plots\n";
        #endif
    }
};

struct BumpsInputs{
    std::string root = "";
    std::size_t parallel_threads = 1;
    std::size_t Nbumps = 4;
    std::vector<std::size_t> Nlayersvec = { 1 };
    std::size_t i = 0;
    std::size_t gradmin_mod = 5;
    std::size_t Nmax_gradient = 5;
};

inline void to_json(nlohmann::json& j, const BumpsInputs& f) {
    j = nlohmann::json{ { "root", f.root },{ "parallel_threads", f.parallel_threads },{ "Nbumps", f.Nbumps },{"i",f.i},{"gradmin_mod",f.gradmin_mod},{"Nmax_gradient",f.Nmax_gradient} };
}

inline void from_json(const nlohmann::json& j, BumpsInputs& f) {
    f.root = j.at("root").get<std::string>();
    f.parallel_threads = j.at("parallel_threads").get<int>();
    f.Nbumps = j.at("Nbumps").get<int>();
    f.i = j.at("i").get < std::size_t > ();
    f.gradmin_mod = j.at("gradmin_mod").get < std::size_t >();
    f.Nmax_gradient = j.at("Nmax_gradient").get < std::size_t >();
}

bool do_one(BumpsInputs &inputs)
{
    std::srand((unsigned int)time(0));

    // Construct the bounds
    std::size_t Npoints = inputs.Nbumps*6*10;
    std::vector<CEGO::Bound> bounds;
    for (auto i = 0; i < inputs.Nbumps; ++i) {
        bounds.push_back(CEGO::Bound(std::pair<double, double>(0.1, 1))); // ex
        bounds.push_back(CEGO::Bound(std::pair<double, double>(1, 3))); // ey
        bounds.push_back(CEGO::Bound(std::pair<double, double>(-50, -10))); // gx
        bounds.push_back(CEGO::Bound(std::pair<double, double>(0.1, 1))); // xb
        bounds.push_back(CEGO::Bound(std::pair<double, double>(-50, -10))); // gy
        bounds.push_back(CEGO::Bound(std::pair<double, double>(0.1, 1))); // yb
    }

    // Normalized bounds in [0,1]
    std::vector<CEGO::Bound> nbounds;
    for (auto i = 0; i < inputs.Nbumps; ++i) {
        nbounds.push_back(CEGO::Bound(std::pair<double, double>(0, 1))); // ex
        nbounds.push_back(CEGO::Bound(std::pair<double, double>(0, 1))); // ey
        nbounds.push_back(CEGO::Bound(std::pair<double, double>(0, 1))); // gx
        nbounds.push_back(CEGO::Bound(std::pair<double, double>(0, 1))); // xb
        nbounds.push_back(CEGO::Bound(std::pair<double, double>(0, 1))); // gy
        nbounds.push_back(CEGO::Bound(std::pair<double, double>(0, 1))); // yb
    }

    Bumps bumps(inputs.Nbumps, Npoints, bounds);
    //bumps.plot_surface();

    for (auto Nlayers : inputs.Nlayersvec){
        Ncalls = 0;
        CEGO::CostFunction<CEGO::numberish> cost_wrapper = std::bind((double (Bumps::*)(const CEGO::AbstractIndividual *)) &Bumps::objective, bumps, std::placeholders::_1);
        auto Npop_size = 15*bounds.size();
        auto layers = CEGO::Layers<CEGO::numberish>(cost_wrapper, bounds.size(), Npop_size, Nlayers, 5);
        layers.parallel = (inputs.parallel_threads > 1);
        layers.parallel_threads = inputs.parallel_threads;
        layers.set_builtin_evolver(CEGO::BuiltinEvolvers::differential_evolution);
        layers.set_bounds(nbounds);
        auto f = [&bumps](const CEGO::EArray<double>& c)->double { return bumps.objective(c); };
        auto f2 = [&bumps](const CEGO::EArray<std::complex<double>>& c)->std::complex<double> { return bumps.objective(c); };
        layers.add_gradient(f, f2);

        auto flags = layers.get_evolver_flags();
        flags["Nelite"] = 1;
        flags["Fmin"] = 0.1;
        flags["Fmax"] = 1.0;
        flags["CR"] = 1;
        layers.set_evolver_flags(flags);

        std::vector<double> best_costs;
        std::vector<std::vector<double> > objs;
        double VTR = 1e-16, best_cost = 999999.0;
        auto startTime = std::chrono::system_clock::now();
        for (auto counter = 0; counter < 50000; ++counter) {
            layers.do_generation();

            if (counter % inputs.gradmin_mod == 0 && counter > 0) {
                layers.gradient_minimizer();
            }

            // Store the best objective function in each layer
            std::vector<double> oo;
            for (auto &&cost_coefficients : layers.get_best_per_layer()) {
                oo.push_back(std::get<0>(cost_coefficients));
            }
            objs.push_back(oo);
            auto stats = layers.cost_stats_each_layer();

            // For the overall best result, print it, and write JSON to file
            auto [best_cost, best_coeffs] = layers.get_best();
            if (counter % 50 == 0) {
                std::cout << counter << ": best: " << best_cost << std::endl;
                //std::cout << bumps.to_realworld(best_coeffs//)-bumps.c0 << "\n ";// << CEGO::vec2string(bumps.c0) << "\n";
            }
            if (best_cost < VTR){ return true; }
        }
        auto endTime = std::chrono::system_clock::now();
        double elap = std::chrono::duration<double>(endTime - startTime).count();
        std::cout << "run:" << elap << " s" << std::endl;

        //bumps.plot_trace(best_costs);
        std::string fname = inputs.root + "Nbumps"+std::to_string(inputs.Nbumps)+"-Nlayers"+std::to_string(Nlayers) + "-run" + std::to_string(inputs.i) + ".txt";
        FILE* fp = fopen(fname.c_str(), "w");
        for (auto j = 0; j < best_costs.size(); ++j){
            fprintf(fp, "%12.8e", best_costs[j]);
            if (j < best_costs.size() - 1) {
                fprintf(fp, ", ");
            }
        }
        fclose(fp);
        /*std::cout << bumps.xb0 << std::endl;
        std::cout << bumps.yb0 << std::endl;*/
        std::cout << "NFE:" << Ncalls << std::endl;
    }
    return 0;
}

int main() {
    #if defined(PYBIND11)
    py::scoped_interpreter interp{};
    #endif
    BumpsInputs in;
    in.root = "shaped-";
    in.Nlayersvec = {3};
    using CEGO::get_env_int;
    auto Nrepeats = get_env_int("NREPEATS", 10);
    in.Nbumps = get_env_int("NBUMPS", 1);
    in.gradmin_mod = get_env_int("GRADMOD", 100);
    in.parallel_threads = get_env_int("NTHREADS", 6);
    in.Nmax_gradient = get_env_int("NMAX_gradient", 5);
    nlohmann::json j = in;
    std::cout << j << std::endl;
    int good_counter = 0;
    for (in.i = 0; in.i < Nrepeats; ++in.i) {
        good_counter += do_one(in);
    }
    std::cout << "Success: " << good_counter << "/" << Nrepeats << std::endl;
}


//
//  Rational.hpp
//
//
//  https://gist.github.com/sklaw/10473569
//

#ifndef RATIONAL_NUM
#define RATIONAL_NUM

#include <iostream>
#include <Eigen/Dense>
#include "../basic.h"

class RationalNum {
    friend RationalNum operator+(const RationalNum& left, const RationalNum& right);
    friend RationalNum operator-(const RationalNum& left, const RationalNum& right);
    friend RationalNum operator*(const RationalNum& left, const RationalNum& right);
    friend RationalNum operator/(const RationalNum& left, const RationalNum& right);
    friend bool operator==(const RationalNum& left, const RationalNum& right);
    friend bool operator!=(const RationalNum& left, const RationalNum& right);
    friend bool operator<(const RationalNum& left, const RationalNum& right);
    friend bool operator>(const RationalNum& left, const RationalNum& right);
    friend bool operator<=(const RationalNum& left, const RationalNum& right);
    friend bool operator>=(const RationalNum& left, const RationalNum& right);
    friend std::ostream& operator<<(std::ostream& out, const RationalNum& obj);
    friend std::istream& operator>>(std::istream& in, RationalNum& obj);

public:
    RationalNum(): numerator(0), denominator(1) {}
    RationalNum(rational_t x): numerator(x.p), denominator(x.q) {}
    RationalNum(int_t numerator_, int_t denominator_ = 1): numerator(numerator_), denominator(denominator_) {}

    RationalNum& operator=(const RationalNum& obj);
    RationalNum& operator+=(const RationalNum& obj);
    RationalNum& operator-=(const RationalNum& obj);
    RationalNum& operator*=(const RationalNum& obj);
    RationalNum& operator/=(const RationalNum& obj);
    RationalNum& operator++();
    RationalNum operator++(int);
    RationalNum& operator--();
    RationalNum operator--(int);
    RationalNum operator+() const;
    RationalNum operator-() const;

    explicit operator int_t() const;

    int_t getNumerator() const { return numerator; }
    int_t getDenominator() const { return denominator; }

private:
    int_t numerator;
    int_t denominator;
    RationalNum& simplify();
};

namespace Eigen {
template<> struct NumTraits<RationalNum>
 : NumTraits<int_t> // permits to get the epsilon, dummy_precision, lowest, highest functions
{
  typedef RationalNum Real;
  typedef RationalNum NonInteger;
  typedef RationalNum Nested;
  enum {
    IsComplex = 0,
    IsInteger = 0,
    IsSigned = 1,
    RequireInitialization = 1,
    ReadCost = 1,
    AddCost = 3,
    MulCost = 3
  };
    static inline Real epsilon() { return 0; }
    static inline Real dummy_precision() { return 0; }
//    static inline int digits10() { return 0; }

};
}

inline const RationalNum& conj(const RationalNum& x)  { return x; }
inline const RationalNum& real(const RationalNum& x)  { return x; }
inline RationalNum imag(const RationalNum&)    { return RationalNum(); }
inline RationalNum abs(const RationalNum&  x)  { return RationalNum(absInt(x.getNumerator()), absInt(x.getDenominator())); }
inline RationalNum abs2(const RationalNum& x)  { return x*x; }

inline rational_t to_rational_t(const RationalNum& x) { return {x.getNumerator(), x.getDenominator()}; }

#endif


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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integer_list.hpp>
#include <boost/hana/integral.hpp>
using namespace boost::hana;
using namespace literals;

int main() {
    //! [main]
    BOOST_HANA_CONSTANT_ASSERT(
        take_while(_ < 2_c, integer_list<int, 0, 1, 2, 3>) == integer_list<int, 0, 1>
    );
    //! [main]
}


/*

 [begin_description]
 Test case for issue 189:
 Controlled Rosenbrock stepper fails to increase step size
 [end_description]

 Copyright 2016 Karsten Ahnert
 Copyright 2016 Mario Mulansky

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

#define BOOST_TEST_MODULE odeint_regression_189

#include <boost/numeric/odeint.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/operator.hpp>

using namespace boost::numeric::odeint;
namespace phoenix = boost::phoenix;

typedef boost::numeric::ublas::vector< double > vector_type;
typedef boost::numeric::ublas::matrix< double > matrix_type;

struct stiff_system
{
    void operator()( const vector_type &x , vector_type &dxdt , double /* t */ )
    {
        dxdt[ 0 ] = -101.0 * x[ 0 ] - 100.0 * x[ 1 ];
        dxdt[ 1 ] = x[ 0 ];
    }
};

struct stiff_system_jacobi
{
    void operator()( const vector_type & /* x */ , matrix_type &J , const double & /* t */ , vector_type &dfdt )
    {
        J( 0 , 0 ) = -101.0;
        J( 0 , 1 ) = -100.0;
        J( 1 , 0 ) = 1.0;
        J( 1 , 1 ) = 0.0;
        dfdt[0] = 0.0;
        dfdt[1] = 0.0;
    }
};

BOOST_AUTO_TEST_CASE( regression_189 )
{
    vector_type x( 2 , 1.0 );

    size_t num_of_steps = integrate_const( make_dense_output< rosenbrock4< double > >( 1.0e-6 , 1.0e-6 ) ,
            std::make_pair( stiff_system() , stiff_system_jacobi() ) ,
            x , 0.0 , 50.0 , 0.01 ,
            std::cout << phoenix::arg_names::arg2 << " " << phoenix::arg_names::arg1[0] << "\n" );
    // regression: number of steps should be 74
    BOOST_CHECK_EQUAL( num_of_steps , 74 );

    vector_type x2( 2 , 1.0 );

    size_t num_of_steps2 = integrate_const( make_dense_output< runge_kutta_dopri5< vector_type > >( 1.0e-6 , 1.0e-6 ) ,
            stiff_system() , x2 , 0.0 , 50.0 , 0.01 ,
            std::cout << phoenix::arg_names::arg2 << " " << phoenix::arg_names::arg1[0] << "\n" );

    BOOST_CHECK_EQUAL( num_of_steps2 , 1531 );
}


#include "CppUnitTest.h"
#include "UnitTestAux.h"
#include "../BVP/Utils/AuxUtils.h"
#include "..\BVP\Problems\TroeschProblem.h"
#include "..\BVP\FunctionApproximation\PointSimple.h"
#include "..\BVP\MultipleShooting\HybridMultipleShootingComponent.h"
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "../BVP/Utils/AuxUtils.h"
#include "../BVP/FunctionApproximation/InitialCondition.h"

using namespace auxutils;

using namespace UnitTestAux;
using namespace Microsoft::VisualStudio::CppUnitTestFramework;

typedef float_50_noet numTypeMp;
typedef double numType;

namespace GeneralTest
{
	TEST_CLASS(MultipleShootingHybridTestUnit)
	{
	public:

		TEST_METHOD(MultipleShootingHybridTest)
		{
			TroeschProblem<numType> tp(20);
			try
			{
				PointSimple<numType> ptLeft;
				ptLeft.Argument  = 0;
				ptLeft.Value  = 0;

				PointSimple<numType> ptRight;
				ptRight.Argument  = 1;
				ptRight.Value  = 1;

				HybridMultipleShootingComponent<numType> HMSComp(tp);

				bool succeeded;
				std::vector<InitCondition<numType>> solution = HMSComp.Run(ptLeft, ptRight, 0.0001, succeeded);

			    Assert::IsTrue(succeeded, Message("Algorithm has not succeeded"));
				auto startDerivativeDiff = abs(solution[0].Derivative - 1.64877350732915e-008);
				Assert::IsTrue(startDerivativeDiff <= 5e-20, Message("du(0) is different" + auxutils::ToString(startDerivativeDiff)));
				auto finalDerivativeDiff = abs(solution[solution.size() - 1].Derivative - 22026.4657494062);
				Assert::IsTrue(finalDerivativeDiff <= 1e-10, Message("du(1) is different" + auxutils::ToString(finalDerivativeDiff)));
			}
			catch (exception e)
			{
				Assert::IsTrue(false, Message(e.what()));

			}
			// TODO: Your test code here
		}

		TEST_METHOD(MultipleShootingHybridTestMultiPrec)
		{
			TroeschProblem<numTypeMp> tp(20);
			try
			{
				PointSimple<numTypeMp> ptLeft;
				ptLeft.Argument  = 0;
				ptLeft.Value  = 0;

				PointSimple<numTypeMp> ptRight;
				ptRight.Argument  = 1;
				ptRight.Value  = 1;

				HybridMultipleShootingComponent<numTypeMp> HMSComp(tp);

				bool succeeded;
				std::vector<InitCondition<numTypeMp>> solution = HMSComp.Run(ptLeft, ptRight, (numTypeMp)1/50, succeeded);

			    Assert::IsTrue(succeeded, Message("Algorithm has not succeeded"));

				auto startDerivativeDiff = abs(solution[0].Derivative - (numTypeMp)"1.65428160979175801124447457829e-08");
				Assert::IsTrue(startDerivativeDiff <= std::numeric_limits<numTypeMp>::epsilon(),
					Message("du(0) is different " + auxutils::ToString(solution[0].Derivative)+ " " + auxutils::ToString(startDerivativeDiff)));

				auto finalDerivativeDiff = abs(solution[solution.size() - 1].Derivative - (numTypeMp)"22026.4657402382174165421786901");
				Assert::IsTrue( finalDerivativeDiff <= 1e-24,
					Message("du(1) is different " + auxutils::ToString(solution[solution.size() - 1].Derivative) + " " + auxutils::ToString(finalDerivativeDiff)));
			}
			catch (exception e)
			{
				Assert::IsTrue(false, Message(e.what()));

			}
			// TODO: Your test code here
		}

	};
}


#ifndef STAN_MATH_PRIM_FUN_CHOOSE_HPP
#define STAN_MATH_PRIM_FUN_CHOOSE_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/functor/apply_scalar_binary.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the binomial coefficient for the specified integer
 * arguments.
 *
 * The binomial coefficient, \f${n \choose k}\f$, read "n choose k", is
 * defined for \f$0 \leq k \leq n\f$ (otherwise return 0) by
 *
 * \f${n \choose k} = \frac{n!}{k! (n-k)!}\f$.
 *
 * @param n total number of objects
 * @param k number of objects chosen
 * @return n choose k or 0 iff k > n
 * @throw std::domain_error if either argument is negative or the
 * result will not fit in an int type
 */
inline int choose(int n, int k) {
  check_nonnegative("choose", "n", n);
  check_nonnegative("choose", "k", k);
  if (k > n) {
    return 0;
  }
  const double choices = boost::math::binomial_coefficient<double>(n, k);
  check_less_or_equal("choose", "n choose k", choices,
                      std::numeric_limits<int>::max());
  return static_cast<int>(std::round(choices));
}

/**
 * Enables the vectorised application of the binomial coefficient function,
 * when the first and/or second arguments are containers.
 *
 * @tparam T1 type of first input
 * @tparam T2 type of second input
 * @param a First input
 * @param b Second input
 * @return Binomial coefficient function applied to the two inputs.
 */
template <typename T1, typename T2, require_any_container_t<T1, T2>* = nullptr>
inline auto choose(const T1& a, const T2& b) {
  return apply_scalar_binary(
      a, b, [&](const auto& c, const auto& d) { return choose(c, d); });
}

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


#include <bits/stdc++.h>
//#include <boost/multiprecision/cpp_int.hpp>
//using namespace boost::multiprecision;
using namespace std;
typedef long long ll;
typedef vector <int> vi;

int n, a[10010];
int b[10010];

int main(){
    ios_base::sync_with_stdio(false);
    cin.tie(nullptr);
    freopen("in.txt", "r", stdin);
    freopen("out.txt", "w", stdout);
    while(cin >> n, n){
        ll cnt = 0;
        a[0] = INT_MAX;
        for(int i=1; i<=n; i++){
            cin >> a[i], cnt += a[i];
        }
        sort(a+1, a+n+1, greater<int> ());
        b[n] = a[n];
        for(int i=n-1; i>=1; i--){
            b[i] = b[i+1] + a[i];
        }
        b[n+1] = 0;
        //for(int i=1; i<=n; i++) cout << a[i] << " "; cout << endl;
        //for(int i=1; i<=n; i++) cout << b[i] << " "; cout << endl;
        bool flag = true;
        if(a[n] < 0 || cnt % 2)
            flag = false;
        int last = 0, sum;
        for(int i=1; i<=n && flag; i++){
            last += a[i];
            int index = lower_bound(a+i+1, a+n+1, i, greater<int> ()) - a;
            sum = b[index] + (index - i - 1) * (i);
            if(last > (i)*(i-1) + sum)
                flag = false;
        }
        if(flag)
            cout << "Possible\n";
        else cout << "Not possible\n";

    }
    return 0;
}

Rare Topics/Rare Algorithms/Formulas or Theorems/Graph Construction.cpp


/*  _______________________________________________________________________

    PECOS: Parallel Environment for Creation Of Stochastics
    Copyright (c) 2011, Sandia National Laboratories.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Pecos directory.
    _______________________________________________________________________ */

#ifndef PECOS_GLOBAL_DEFS_H
#define PECOS_GLOBAL_DEFS_H

#include <iostream>
#include <cfloat>  // for DBL_MIN, DBL_MAX
#include <cmath>
#include <cstdlib>

#include <boost/math/constants/constants.hpp>

namespace Pecos {

// --------------
// Special values
// --------------
/// the value for PI used in various numerical routines
const double PI = boost::math::constants::pi<double>();

/// special value returned by index() when entry not found
const size_t _NPOS = ~(size_t)0; // one's complement

/// used in ostream data output functions
const int WRITE_PRECISION = 10;

/// small value used for protecting division by zero, etc.; an alternative
/// to DBL_MIN that is less likely to cause underflow/overflow when numbers
/// larger than it are used in calculations
const double SMALL_NUMBER = 1.e-25;
/// large value used as a surrogate for infinity in error traps; an alternative
/// to DBL_MAX or inf that is less likely to cause underflow/overflow when used
/// in subsequent calculations
const double LARGE_NUMBER = 1.e+50;

// define special values for vector/matrix data copying modes
enum { DEFAULT_COPY=0, SHALLOW_COPY, DEEP_COPY };

// define special values for ExpansionConfigOptions::outputLevel
enum { SILENT_OUTPUT, QUIET_OUTPUT, NORMAL_OUTPUT, VERBOSE_OUTPUT,
       DEBUG_OUTPUT };

// define special values for ranVarTypesX/U
enum { NO_TYPE=0,
       // continuous random variable types:
       CONTINUOUS_DESIGN, STD_NORMAL, NORMAL, BOUNDED_NORMAL,
       LOGNORMAL, BOUNDED_LOGNORMAL, STD_UNIFORM, UNIFORM, LOGUNIFORM,
       TRIANGULAR, STD_EXPONENTIAL, EXPONENTIAL, STD_BETA, BETA, STD_GAMMA,
       GAMMA, GUMBEL, FRECHET, WEIBULL, HISTOGRAM_BIN, INV_GAMMA,
       CONTINUOUS_INTERVAL, CONTINUOUS_STATE, STOCHASTIC_EXPANSION,
       // discrete random variable types:
       POISSON, BINOMIAL, NEGATIVE_BINOMIAL, GEOMETRIC, HYPERGEOMETRIC,
       HISTOGRAM_PT_INT, HISTOGRAM_PT_STRING, HISTOGRAM_PT_REAL };

// define special values for secondaryACVarMapTargets/secondaryADVarMapTargets
enum { NO_TARGET=0, CDV_LWR_BND, CDV_UPR_BND, DDRIV_LWR_BND, DDRIV_UPR_BND,
       N_MEAN, N_STD_DEV, N_LWR_BND, N_UPR_BND, N_LOCATION, N_SCALE, LN_MEAN,
       LN_STD_DEV, LN_LAMBDA, LN_ZETA, LN_ERR_FACT, LN_LWR_BND, LN_UPR_BND,
       U_LWR_BND, U_UPR_BND, U_LOCATION, U_SCALE, LU_LWR_BND, LU_UPR_BND,
       T_MODE, T_LWR_BND, T_UPR_BND, T_LOCATION, T_SCALE, E_BETA,
       BE_ALPHA, BE_BETA, BE_LWR_BND, BE_UPR_BND, GA_ALPHA, GA_BETA,
       GU_ALPHA, GU_BETA, F_ALPHA, F_BETA, W_ALPHA, W_BETA, IGA_ALPHA, IGA_BETA,
       P_LAMBDA, BI_P_PER_TRIAL, BI_TRIALS, NBI_P_PER_TRIAL, NBI_TRIALS,
       GE_P_PER_TRIAL, HGE_TOT_POP, HGE_SEL_POP, HGE_FAILED,
       CSV_LWR_BND, CSV_UPR_BND, DSRIV_LWR_BND, DSRIV_UPR_BND };

/// derived basis approximation types
enum { NO_BASIS=0, //FOURIER_BASIS, EIGEN_BASIS,
       GLOBAL_NODAL_INTERPOLATION_POLYNOMIAL,
       PIECEWISE_NODAL_INTERPOLATION_POLYNOMIAL,
       GLOBAL_HIERARCHICAL_INTERPOLATION_POLYNOMIAL,
       PIECEWISE_HIERARCHICAL_INTERPOLATION_POLYNOMIAL,
       GLOBAL_REGRESSION_ORTHOGONAL_POLYNOMIAL,
       GLOBAL_PROJECTION_ORTHOGONAL_POLYNOMIAL,
       GLOBAL_ORTHOGONAL_POLYNOMIAL };
       //PIECEWISE_REGRESSION_ORTHOGONAL_POLYNOMIAL,
       //PIECEWISE_PROJECTION_ORTHOGONAL_POLYNOMIAL,
       //PIECEWISE_ORTHOGONAL_POLYNOMIAL };

/// derived basis polynomial types (orthogonal polynomial order follows
/// uncertain variable spec order of normal, uniform, exponential, beta, gamma)
enum { NO_POLY=0, HERMITE_ORTHOG, LEGENDRE_ORTHOG, LAGUERRE_ORTHOG,
       JACOBI_ORTHOG, GEN_LAGUERRE_ORTHOG, CHEBYSHEV_ORTHOG, NUM_GEN_ORTHOG,
       LAGRANGE_INTERP, HERMITE_INTERP, PIECEWISE_LINEAR_INTERP,
       PIECEWISE_QUADRATIC_INTERP, PIECEWISE_CUBIC_INTERP };

/// integration rules within VPISparseGrid (1-12: CC through User-closed)
/// and beyond (GOLUB_WELSCH, NEWTON_COTES)
enum { NO_RULE=0, CLENSHAW_CURTIS, FEJER2, GAUSS_PATTERSON, GAUSS_LEGENDRE,
       GAUSS_HERMITE, GEN_GAUSS_HERMITE, GAUSS_LAGUERRE, GEN_GAUSS_LAGUERRE,
       GAUSS_JACOBI, GENZ_KEISTER, /*USER_OPEN, USER_CLOSED,*/ GOLUB_WELSCH,
       NEWTON_COTES };

// growth rules within VPISparseGrid
//enum { DEFAULT_GROWTH=0, SLOW_LINEAR, SLOW_LINEAR_ODD, MODERATE_LINEAR,
//       SLOW_EXPONENTIAL, MODERATE_EXPONENTIAL, FULL_EXPONENTIAL };

/// options for synchronizing linear and exponential growth rule settings
/// (consistent with slow/moderate/full growth for new level_to_growth_*
/// functions in sandia_rules.cpp)
enum { SLOW_RESTRICTED_GROWTH, MODERATE_RESTRICTED_GROWTH,
       UNRESTRICTED_GROWTH };

/// solution approaches for calculating the polynomial basis coefficients
/// (options for ExpansionConfigOptions::expCoeffsSolnApproach)
enum { QUADRATURE, CUBATURE, LIGHTWEIGHT_SPARSE_GRID, COMBINED_SPARSE_GRID,
       HIERARCHICAL_SPARSE_GRID, SAMPLING, DEFAULT_REGRESSION,
       DEFAULT_LEAST_SQ_REGRESSION, SVD_LEAST_SQ_REGRESSION,
       EQ_CON_LEAST_SQ_REGRESSION, BASIS_PURSUIT, BASIS_PURSUIT_DENOISING,
       ORTHOG_MATCH_PURSUIT, LASSO_REGRESSION, LEAST_ANGLE_REGRESSION,
       ORTHOG_LEAST_INTERPOLATION };
/// options for BasisConfigOptions::nestingOverride (inactive)
enum { NO_NESTING_OVERRIDE=0, NESTED, NON_NESTED };
/// options for overriding the default growth restriction policy
enum { NO_GROWTH_OVERRIDE=0, RESTRICTED, UNRESTRICTED };
/// options for ExpansionConfigOptions::refinementType (inactive)
enum { NO_REFINEMENT=0, P_REFINEMENT, H_REFINEMENT };
/// options for ExpansionConfigOptions::refinementControl
enum { NO_CONTROL=0, UNIFORM_CONTROL, LOCAL_ADAPTIVE_CONTROL,
       DIMENSION_ADAPTIVE_CONTROL_SOBOL, DIMENSION_ADAPTIVE_CONTROL_DECAY,
       DIMENSION_ADAPTIVE_CONTROL_GENERALIZED };

/// options for expansion basis type
enum { DEFAULT_BASIS=0, TENSOR_PRODUCT_BASIS, TOTAL_ORDER_BASIS,
       ADAPTED_BASIS_GENERALIZED, ADAPTED_BASIS_EXPANDING_FRONT,
       NODAL_INTERPOLANT, HIERARCHICAL_INTERPOLANT };

/// mode of integration driver: integration versus interpolation
enum { DEFAULT_MODE=0, INTEGRATION_MODE, INTERPOLATION_MODE };

/// options for local basis functions within PiecewiseInterpPolynomial
enum { LINEAR_EQUIDISTANT, LINEAR, QUADRATIC_EQUIDISTANT, QUADRATIC,
       CUBIC_EQUIDISTANT, CUBIC };

/// special values for nodal interpolation of variance and variance gradient
enum { INTERPOLATION_OF_PRODUCTS, REINTERPOLATION_OF_PRODUCTS,
       PRODUCT_OF_INTERPOLANTS_FAST, PRODUCT_OF_INTERPOLANTS_FULL };

/// special values for polynomial expansion combination
enum { NO_COMBINE=0,  ADD_COMBINE, MULT_COMBINE, ADD_MULT_COMBINE };

// ----------------
// Standard streams
// ----------------
#define PCout std::cout
#define PCerr std::cerr

// --------------
// Global objects
// --------------
/// Dummy struct for overloading letter-envelope constructors.
/** BaseConstructor is used to overload the constructor for the base class
    portion of letter objects.  It avoids infinite recursion (Coplien p.139)
    in the letter-envelope idiom by preventing the letter from instantiating
    another envelope.  Putting this struct here avoids circular dependencies. */
struct BaseConstructor {
  BaseConstructor(int = 0) {} ///< C++ structs can have constructors
};

// ----------------
// Global functions
// ----------------

/// global function which handles serial or parallel aborts
void abort_handler(int code);

inline void abort_handler(int code)
{ std::exit(code); } // for now, prior to use of MPI

/** Templatized abort_handler_t method that allows for convenient return from
    methods that otherwise have no sensible return from error clauses.  Usage:
    MyType& method() { return abort_handler<MyType&>(-1); } */
template <typename T>
T abort_handler_t(int code)
{
  abort_handler(code);
  throw code;
}

} // namespace Pecos

#endif // PECOS_GLOBAL_DEFS_H


#include "QMathUtil.h"
#include <cmath>
//#include <Eigen/Dense>
#include <iostream>
namespace GCL {

QVector3D QMathUtil::getRayPlaneIntersect(const QVector3D &rayPos, const QVector3D &rayDir,
                              const QVector3D &planePoint, const QVector3D &planeNormal, bool *has_intersection)
{
    float t0 = QVector3D::dotProduct(rayDir,planeNormal);
    float t1 = QVector3D::dotProduct(rayPos - planePoint , planeNormal);
    if(fabs(t0) < 1e-7)
    {
        if(has_intersection)
        {
            *has_intersection = false;
        }
        return QVector3D(0,0,0);
    }

    float k = -t1 / t0;
    QVector3D intersect = rayPos + k * rayDir;
    if(has_intersection)
    {
        *has_intersection = true;
    }
    return intersect;
}
float clamp(float x, float l, float r)
{
    if(x < l) return l;
    if(x > r) return r;
    return x;
}
QVector3D QMathUtil::fromMatrixToEuler(const QMatrix4x4 &m, const QString &order)
{
    QMatrix3x3 tm = m.normalMatrix();

   float m11 = tm(0,0), m12 = tm(1,0), m13 = tm(2,0);
   float m21 = tm(0,1), m22 = tm(1,1), m23 = tm(2,1);
   float m31 = tm(0,2), m32 = tm(1,2), m33 = tm(2,2);
    float _x,_y,_z;
    if(order == "XYZ")
    {
        _y = asin(clamp(m13,-1,1));
        if(fabs(m13) < 0.99999)
        {
            _x = atan2(-m23,m33);
            _z = atan2(-m12,m11);
        }else
        {
            _x = atan2(m32,m22);
            _z = 0;
        }
    }else if ( order == "YXZ" ) {

        _x = asin( - clamp( m23, - 1, 1 ) );

        if ( fabs( m23 ) < 0.99999 ) {

            _y = atan2( m13, m33 );
            _z = atan2( m21, m22 );

        } else {

            _y = atan2( - m31, m11 );
            _z = 0;

        }

    } else if ( order == "ZXY" ) {

        _x = asin( clamp( m32, - 1, 1 ) );

        if ( fabs( m32 ) < 0.99999 ) {

            _y = atan2( - m31, m33 );
            _z = atan2( - m12, m22 );

        } else {

            _y = 0;
            _z = atan2( m21, m11 );

        }

    } else if ( order == "ZYX" ) {

        _y = asin( - clamp( m31, - 1, 1 ) );

        if ( fabs( m31 ) < 0.99999 ) {

            _x = atan2( m32, m33 );
            _z = atan2( m21, m11 );

        } else {

            _x = 0;
            _z = atan2( - m12, m22 );

        }

    } else if ( order == "YZX" ) {

        _z = asin( clamp( m21, - 1, 1 ) );

        if ( fabs( m21 ) < 0.99999 ) {

            _x = atan2( - m23, m22 );
            _y = atan2( - m31, m11 );

        } else {

            _x = 0;
            _y = atan2( m13, m33 );

        }

    } else if ( order == "XZY" ) {

        _z = asin( - clamp( m12, - 1, 1 ) );

        if ( fabs( m12 ) < 0.99999 ) {

            _x = atan2( m32, m22 );
            _y = atan2( m13, m11 );

        } else {

            _x = atan2( - m23, m33 );
            _y = 0;

        }

    } else {

         qDebug()<<( "THREE.Euler: .setFromRotationMatrix() given unsupported order: " + order );
    }

    QVector3D v(_x,_y,_z);
    v = v / 3.1415926535898 * 180;
    return -v;

}

QVector3D QMathUtil::mulEuler(const QVector3D &x, const QVector3D &y)
{
    QMatrix4x4 mat;
    mat.rotate(x[0],QVector3D(1,0,0));
    mat.rotate(x[1],QVector3D(0,1,0));
    mat.rotate(x[2],QVector3D(0,0,1));

    mat.rotate(y[0],QVector3D(1,0,0));
    mat.rotate(y[1],QVector3D(0,1,0));
    mat.rotate(y[2],QVector3D(0,0,1));

    return QMathUtil::fromMatrixToEuler(mat);

}

float QMathUtil::getDistanceSumToPlane(const QList<QVector3D> &vlist, const QVector3D &point, const QVector3D &normal)
{
    float sum = 0;
    for(const auto & v : vlist)
    {
        sum += fabs(QVector3D::dotProduct(v-point,normal));
    }
    return sum;
}

float QMathUtil::getDistanceSquareSumToPlane(const QList<QVector3D> &vlist, const QVector3D &point, const QVector3D &normal)
{
    float sum = 0;
    for(const auto & v : vlist)
    {
        float val = fabs(QVector3D::dotProduct(v-point,normal));
        sum += val * val;
    }
    return sum;
}

void QMathUtil::computePCA(const QList<QVector3D> &vlist, QVector3D &axis_0, QVector3D &axis_1, QVector3D &axis_2)
{
//    QVector3D center;
//    for(const auto &v : vlist)
//    {
//        center += v;
//    }
//    center/= vlist.size();
//    Eigen::Matrix3d mat;
//    for(const auto &v : vlist)
//    {
//        Eigen::Vector3d ev;
//        QVector3D tv = v - center;
//        for(int j=0; j < 3; j++)
//        {
//            ev(j) = tv[j];
//        }
//        Eigen::Matrix3d tm = ev * ev.transpose();
//        mat += tm;
//    }
//    mat /= vlist.size();
//    Eigen::EigenSolver<Eigen::Matrix3d> solver;
//    solver.compute(mat);
//    auto eigen_v = solver.eigenvectors();
//    for(int j=0; j < 3; j++)
//    {
//        axis_0[j] = eigen_v.coeff(j,0).real();
//        axis_1[j] = eigen_v.coeff(j,1).real();
//        axis_2[j] = eigen_v.coeff(j,2).real();
//    }

}

QVector3D QMathUtil::getRayTriangleIntesect(const QVector3D &rayPos, const QVector3D &rayDir, const QVector3D &v0, const QVector3D &v1, const QVector3D &v2, bool *has_intersection)
{
    Vec3 vray(rayPos.x(),rayPos.y(),rayPos.z());
    Vec3 vdir(rayDir.x(),rayDir.y(),rayDir.z());
    Vec3 vv0(v0.x(),v0.y(),v0.z());
    Vec3 vv1(v1.x(),v1.y(),v1.z());
    Vec3 vv2(v2.x(),v2.y(),v2.z());
    Vec3 ans;
    bool t = Vec3::getIntersectionRayToTriangle(vray,vdir,vv0,vv1,vv2,ans);

    if(has_intersection)
    {
        *has_intersection = t;
    }
    return QVector3D(ans[0],ans[1],ans[2]);
}

QVector3D QMathUtil::fromVectorTransformToEuler(const QVector3D &v0, const QVector3D &v1,const QString &order)
{
    Vec3 vv0(v0.x(),v0.y(),v0.z());
    Vec3 vv1(v1.x(),v1.y(),v1.z());

    Quat quat =  Quat::quatFromVectorTransform(vv0,vv1);
    HomoMatrix4 hmat =  quat.convertToMatrix();

    QMatrix4x4 qmat(hmat.data(),4,4);

    return fromMatrixToEuler(qmat,order);

}

}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *        Wakker, K.F., "Astrodynamics I, AE4-874", Delft University of Technology, 2007.
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/Gravitation/jacobiEnergy.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
namespace tudat
{
namespace unit_tests
{

using namespace gravitation;
using namespace orbital_element_conversions;

//! Test if Jacobi energy is computed correctly.
BOOST_AUTO_TEST_CASE( testJacobiEnergy )
{
    // Test 1: test Jacobi energy at L1.
    {

        // Set mass parameter for Earth-moon system. Value from Table 3.1 (Wakker, 2007).
        double massParameter = 0.01215;

        // Initialize position L1, from Table 3.4 (Wakker, 2007).
        Eigen::VectorXd stateAtL1 = Eigen::VectorXd::Zero( 6 );
        stateAtL1( xCartesianPositionIndex ) = 0.836914;

        // Set expected value of Jacobi energy at L1.
        double expectedJacobiEnergy = 3.1883;

        // Compute Jacobi energy.
        double computedJacobiEnergy = computeJacobiEnergy( massParameter, stateAtL1 );

        // Check if expected Jacobi energy matches computed.
        BOOST_CHECK_CLOSE_FRACTION( expectedJacobiEnergy,  computedJacobiEnergy, 1.0e-4 );
    }

    // Test 2: test Jacobi energy at L4.
    {

        // Set mass parameter for Earth-moon system. Value from Table 3.1 (Wakker, 2007).
        double massParameter = 0.01215;

        // Initialize position L4, from Table 3.4 (Wakker, 2007).
        Eigen::VectorXd stateAtL4 = Eigen::VectorXd::Zero( 6 );
        stateAtL4( xCartesianPositionIndex ) = 0.487849;
        stateAtL4( yCartesianPositionIndex ) = 0.866025;

        // Set expected value of Jacobi energy at L4.
        double expectedJacobiEnergy = 2.9880;

        // Compute Jacobi energy.
        double computedJacobiEnergy = computeJacobiEnergy( massParameter, stateAtL4 );

        // Check if expected Jacobi energy matches computed.
        BOOST_CHECK_CLOSE_FRACTION( expectedJacobiEnergy,  computedJacobiEnergy, 1.0e-6 );
    }
}

} // namespace unit_tests
} // namespace tudat


/** @file */

#include <boost/program_options.hpp>
#include <boost/test/unit_test.hpp>
#include <sstream>

#include <YukariMaths/Transform.h>
#include <YukariMaths/Units.h>

namespace po = boost::program_options;

namespace Yukari
{
namespace Maths
{
  namespace Test
  {
    BOOST_AUTO_TEST_SUITE(TransformTest)

    BOOST_AUTO_TEST_CASE(Transform_FromMatrix)
    {
      Eigen::Matrix4f mat;
      // clang-format off
      mat << 0.75f       , -0.216506362f, 0.62499994f  , 5.5f,
             0.433012664f, 0.875f       , -0.216506317f, 7.8f,
             -0.5f       , 0.433012664f , 0.75f        , 2.1f,
             0.0f        , 0.0f         , 0.0f         , 1.0f;
      // clang-format on

      Transform t(mat);

      Eigen::Quaternionf expected;
      expected = Eigen::AngleAxisf(30.0f * DEG_TO_RAD, Eigen::Vector3f::UnitZ()) *
                 Eigen::AngleAxisf(30.0f * DEG_TO_RAD, Eigen::Vector3f::UnitY()) *
                 Eigen::AngleAxisf(30.0f * DEG_TO_RAD, Eigen::Vector3f::UnitX());

      BOOST_CHECK_CLOSE(t.orientation().w(), expected.w(), 0.1f);
      BOOST_CHECK_CLOSE(t.orientation().x(), expected.x(), 0.1f);
      BOOST_CHECK_CLOSE(t.orientation().y(), expected.y(), 0.1f);
      BOOST_CHECK_CLOSE(t.orientation().z(), expected.z(), 0.1f);

      BOOST_CHECK_CLOSE(t.position().x(), 5.5f, 0.1f);
      BOOST_CHECK_CLOSE(t.position().y(), 7.8f, 0.1f);
      BOOST_CHECK_CLOSE(t.position().z(), 2.1f, 0.1f);
    }

    BOOST_AUTO_TEST_CASE(Transform_ToMatrix)
    {
      Transform t;
      t.orientation() = Eigen::AngleAxisf(30.0f * DEG_TO_RAD, Eigen::Vector3f::UnitZ()) *
                        Eigen::AngleAxisf(30.0f * DEG_TO_RAD, Eigen::Vector3f::UnitY()) *
                        Eigen::AngleAxisf(30.0f * DEG_TO_RAD, Eigen::Vector3f::UnitX());
      t.position() = Eigen::Vector3f(5.5f, 7.8f, 2.1f);

      Eigen::Matrix4f mat = t.toEigen();

      BOOST_CHECK_CLOSE(mat(0, 0), 0.75f, 0.1f);
      BOOST_CHECK_CLOSE(mat(0, 1), -0.216506362f, 0.1f);
      BOOST_CHECK_CLOSE(mat(0, 2), 0.62499994f, 0.1f);
      BOOST_CHECK_CLOSE(mat(0, 3), 5.5f, 0.1f);

      BOOST_CHECK_CLOSE(mat(1, 0), 0.433012664f, 0.1f);
      BOOST_CHECK_CLOSE(mat(1, 1), 0.875f, 0.1f);
      BOOST_CHECK_CLOSE(mat(1, 2), -0.216506317f, 0.1f);
      BOOST_CHECK_CLOSE(mat(1, 3), 7.8f, 0.1f);

      BOOST_CHECK_CLOSE(mat(2, 0), -0.5f, 0.1f);
      BOOST_CHECK_CLOSE(mat(2, 1), 0.433012664f, 0.1f);
      BOOST_CHECK_CLOSE(mat(2, 2), 0.75f, 0.1f);
      BOOST_CHECK_CLOSE(mat(2, 3), 2.1f, 0.1f);

      BOOST_CHECK_CLOSE(mat(3, 0), 0.0f, 0.1f);
      BOOST_CHECK_CLOSE(mat(3, 1), 0.0f, 0.1f);
      BOOST_CHECK_CLOSE(mat(3, 2), 0.0f, 0.1f);
      BOOST_CHECK_CLOSE(mat(3, 3), 1.0f, 0.1f);
    }

    BOOST_AUTO_TEST_CASE(Transform_Stream_Out_Empty)
    {
      Transform t;

      std::stringstream str;
      str << t;

      BOOST_CHECK_EQUAL(str.str(), "(o=[0, 0, 0, 1], p=[0, 0, 0])");
    }

    BOOST_AUTO_TEST_CASE(Transform_Stream_Out)
    {
      Transform t;
      t.orientation() = Eigen::AngleAxisf(30.0f * DEG_TO_RAD, Eigen::Vector3f::UnitZ()) *
                        Eigen::AngleAxisf(30.0f * DEG_TO_RAD, Eigen::Vector3f::UnitY()) *
                        Eigen::AngleAxisf(30.0f * DEG_TO_RAD, Eigen::Vector3f::UnitX());
      t.position() = Eigen::Vector3f(5.5f, 7.8f, 2.1f);

      std::stringstream str;
      str << t;

      BOOST_CHECK_EQUAL(str.str(),
                        "(o=[0.176777, 0.306186, 0.176777, 0.918559], p=[5.5, 7.8, 2.1])");
    }

    BOOST_AUTO_TEST_CASE(Transform_Stream_In)
    {
      Transform t;

      std::stringstream in("(o=[0.176777, 0.306186, 0.176777, 0.918559], p=[5.5, 7.8, 2.1])");
      in >> t;

      std::stringstream out;
      out << t;

      BOOST_CHECK_EQUAL(out.str(),
                        "(o=[0.176777, 0.306186, 0.176777, 0.918559], p=[5.5, 7.8, 2.1])");
    }

    BOOST_AUTO_TEST_CASE(Transform_From_String)
    {
      Transform t("(o=[0.176777, 0.306186, 0.176777, 0.918559], p=[5.5, 7.8, 2.1])");

      std::stringstream out;
      out << t;

      BOOST_CHECK_EQUAL(out.str(),
                        "(o=[0.176777, 0.306186, 0.176777, 0.918559], p=[5.5, 7.8, 2.1])");
    }

    BOOST_AUTO_TEST_CASE(Transform_From_Boost_Args_Orientation_Only)
    {
      po::variables_map args;
      args.insert(std::make_pair(
          "orientation", po::variable_value(boost::any(std::string("[0, 0, 1] -90.5")), false)));

      Transform t(args);

      Eigen::Quaternionf expectedQuat;
      expectedQuat = Eigen::AngleAxisf(-90.5f * DEG_TO_RAD, Eigen::Vector3f::UnitZ());
      BOOST_CHECK_EQUAL(expectedQuat.w(), t.orientation().w());
      BOOST_CHECK_EQUAL(expectedQuat.x(), t.orientation().x());
      BOOST_CHECK_EQUAL(expectedQuat.y(), t.orientation().y());
      BOOST_CHECK_EQUAL(expectedQuat.z(), t.orientation().z());

      BOOST_CHECK_EQUAL(t.position().x(), 0.0f);
      BOOST_CHECK_EQUAL(t.position().y(), 0.0f);
      BOOST_CHECK_EQUAL(t.position().z(), 0.0f);
    }

    BOOST_AUTO_TEST_CASE(Transform_From_Boost_Args_Position_Only)
    {
      po::variables_map args;
      args.insert(std::make_pair(
          "position", po::variable_value(boost::any(std::string("[1, 4.5, 7.1]")), false)));

      Transform t(args);

      Eigen::Quaternionf expectedQuat = Eigen::Quaternionf::Identity();
      BOOST_CHECK_EQUAL(expectedQuat.w(), t.orientation().w());
      BOOST_CHECK_EQUAL(expectedQuat.x(), t.orientation().x());
      BOOST_CHECK_EQUAL(expectedQuat.y(), t.orientation().y());
      BOOST_CHECK_EQUAL(expectedQuat.z(), t.orientation().z());

      BOOST_CHECK_EQUAL(t.position().x(), 1.0f);
      BOOST_CHECK_EQUAL(t.position().y(), 4.5f);
      BOOST_CHECK_EQUAL(t.position().z(), 7.1f);
    }

    BOOST_AUTO_TEST_CASE(Transform_From_Boost_Args_Both)
    {
      po::variables_map args;
      args.insert(std::make_pair(
          "orientation", po::variable_value(boost::any(std::string("[0, 0, 1] 90.0")), false)));
      args.insert(std::make_pair(
          "position", po::variable_value(boost::any(std::string("[1, 4.5, 7.1]")), false)));

      Transform t(args);

      Eigen::Quaternionf expectedQuat;
      expectedQuat = Eigen::AngleAxisf(90.0f * DEG_TO_RAD, Eigen::Vector3f::UnitZ());
      BOOST_CHECK_EQUAL(expectedQuat.w(), t.orientation().w());
      BOOST_CHECK_EQUAL(expectedQuat.x(), t.orientation().x());
      BOOST_CHECK_EQUAL(expectedQuat.y(), t.orientation().y());
      BOOST_CHECK_EQUAL(expectedQuat.z(), t.orientation().z());

      BOOST_CHECK_EQUAL(t.position().x(), 1.0f);
      BOOST_CHECK_EQUAL(t.position().y(), 4.5f);
      BOOST_CHECK_EQUAL(t.position().z(), 7.1f);
    }

    BOOST_AUTO_TEST_CASE(Transform_From_Boost_Args_Both_Empty)
    {
      po::variables_map args;
      args.insert(
          std::make_pair("orientation", po::variable_value(boost::any(std::string("")), false)));
      args.insert(
          std::make_pair("position", po::variable_value(boost::any(std::string("")), false)));

      Transform t(args);

      Eigen::Quaternionf expectedQuat = Eigen::Quaternionf::Identity();
      BOOST_CHECK_EQUAL(expectedQuat.w(), t.orientation().w());
      BOOST_CHECK_EQUAL(expectedQuat.x(), t.orientation().x());
      BOOST_CHECK_EQUAL(expectedQuat.y(), t.orientation().y());
      BOOST_CHECK_EQUAL(expectedQuat.z(), t.orientation().z());

      BOOST_CHECK_EQUAL(t.position().x(), 0.0f);
      BOOST_CHECK_EQUAL(t.position().y(), 0.0f);
      BOOST_CHECK_EQUAL(t.position().z(), 0.0f);
    }

    BOOST_AUTO_TEST_SUITE_END()
  }
}
}


//==================================================================================================
/*!
  @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_ACOSH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOSH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic cosine argument: \f$\log(x+\sqrt{x^2-1})\f$.

    @see cosh, sinh,  acsch, asinh, atanh, asech, acoth, acsch

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

    @par Example:

      @snippet acosh.cpp acosh

    @par Possible output:

      @snippet acosh.txt acosh

  **/
  IEEEValue acosh(IEEEValue const & x);
} }
#endif

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

#endif


#ifndef ROVER_LINEAR_REGRESSION_HPP
#define ROVER_LINEAR_REGRESSION_HPP
#include <tuple>
#include <vector>
#include <dlib/matrix.h>

namespace Rover {

  //! Models a trial using linear regression.
  /*!
    \tparam T The arithmetic type used for calculations.
  */
  template<typename T = double>
  class LinearRegression {
    public:

      //! The arithmetic type used for calculations.
      using Type = T;

      //! Learns a trial represented by a ScalarView.
      template<typename ScalarView>
      void learn(const ScalarView& view);

      //! Predicts the dependent variable for a set of arguments.
      template<typename Arguments>
      Type predict(const Arguments& args) const;

    private:
      dlib::matrix<Type> m_transformation;

      template<typename ScalarView>
      static dlib::matrix<Type> compute_transformation_vector(
        const ScalarView& trial);
  };

  template<typename T>
  template<typename ScalarView>
  void LinearRegression<T>::learn(const ScalarView& view) {
    m_transformation = compute_transformation_vector(view);
  }

  template<typename T>
  template<typename Arguments>
  typename LinearRegression<T>::Type LinearRegression<T>::predict(const
      Arguments& args) const {
    auto x = dlib::matrix<Type, 1>(args.size() + 1);
    x(0, 0) = static_cast<Type>(1.);
    std::copy(args.begin(), args.end(), x.begin() + 1);
    auto result = x * m_transformation;
    return result;
  }

  template<typename T>
  template<typename ScalarView>
  dlib::matrix<typename LinearRegression<T>::Type>
      LinearRegression<T>::compute_transformation_vector(const ScalarView&
      view) {
    auto x = dlib::matrix<Type>(view.size(), view[0].m_arguments.size() + 1);
    auto y = dlib::matrix<Type>(view.size(), 1);
    for(auto i = std::size_t(0); i < view.size(); ++i) {
      auto sample = view[i];
      x(i, 0) = static_cast<Type>(1.);
      std::copy(sample.m_arguments.begin(), sample.m_arguments.end(), x.begin()
        + i * x.nc() + 1);
      y(0, i) = sample.m_result;
    }
    auto xtr = dlib::trans(x);
    auto result = dlib::inv(xtr * x) * xtr * y;
    return result;
  }
}

#endif


/**
 * \file
 *
 * \copyright
 * Copyright (c) 2012-2022, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include <gtest/gtest.h>

#include <boost/math/constants/constants.hpp>
#include <random>

#include "GeoLib/Utils.h"

struct GeoLibGenerateEquidistantPoints : public testing::Test
{
    GeoLibGenerateEquidistantPoints()
    {
        // enable random engine
        std::random_device rd;
        std::mt19937 random_engine_mt19937(rd());
        constexpr auto pi = boost::math::double_constants::pi;
        std::normal_distribution<> normal_dist_phi(-pi, pi);  // azimuthal angle
        std::normal_distribution<> normal_dist_theta(0, pi);  // polar angle
        // generate point on unit sphere
        double const phi = normal_dist_phi(random_engine_mt19937);
        double const theta = normal_dist_theta(random_engine_mt19937);

        start = MathLib::Point3d{{std::cos(phi) * std::sin(theta),
                                  std::sin(phi) * std::sin(theta),
                                  std::cos(theta)}};
        // mirror of start point at origin
        end = MathLib::Point3d{{-start[0], -start[1], -start[2]}};
    }

    ~GeoLibGenerateEquidistantPoints()
    {
        for (auto* p : equidistant_points)
        {
            delete p;
        }
    }

    void checkLength() const
    {
        constexpr double eps = 4 * std::numeric_limits<double>::epsilon();
        double sum = 0.0;
        for (std::size_t i = 0; i < equidistant_points.size() - 1; ++i)
        {
            sum += std::sqrt(MathLib::sqrDist(*(equidistant_points[i]),
                                              *(equidistant_points[i + 1])));
        }
        auto const sum_start_end = std::sqrt(MathLib::sqrDist(start, end));
        EXPECT_NEAR(sum_start_end, sum, eps)
            << start[0] << " " << start[1] << " " << start[2] << " -- "
            << end[0] << " " << end[1] << " " << end[2];
    }

    MathLib::Point3d start;
    MathLib::Point3d end;
    std::vector<GeoLib::Point*> equidistant_points;
};

TEST_F(GeoLibGenerateEquidistantPoints, IdenticalStartEndPoints)
{
    equidistant_points = GeoLib::generateEquidistantPoints(start, start, 2);
    ASSERT_EQ(4, equidistant_points.size());
    EXPECT_DOUBLE_EQ(start[0], (*equidistant_points[0])[0]);
    EXPECT_DOUBLE_EQ(start[1], (*equidistant_points[0])[1]);
    EXPECT_DOUBLE_EQ(start[2], (*equidistant_points[0])[2]);
    EXPECT_DOUBLE_EQ(start[0], (*equidistant_points[1])[0]);
    EXPECT_DOUBLE_EQ(start[1], (*equidistant_points[1])[1]);
    EXPECT_DOUBLE_EQ(start[2], (*equidistant_points[1])[2]);
}

TEST_F(GeoLibGenerateEquidistantPoints, ZeroSubdivisions)
{
    equidistant_points = GeoLib::generateEquidistantPoints(start, end, 0);
    ASSERT_EQ(2, equidistant_points.size());
    checkLength();
}

TEST_F(GeoLibGenerateEquidistantPoints, OneSubdivision)
{
    equidistant_points = GeoLib::generateEquidistantPoints(start, end, 1);
    ASSERT_EQ(3, equidistant_points.size());
    EXPECT_DOUBLE_EQ(0.0, (*equidistant_points[1])[0]);
    EXPECT_DOUBLE_EQ(0.0, (*equidistant_points[1])[1]);
    EXPECT_DOUBLE_EQ(0.0, (*equidistant_points[1])[2]);
    checkLength();
}

TEST_F(GeoLibGenerateEquidistantPoints, ThreeSubdivisions)
{
    equidistant_points = GeoLib::generateEquidistantPoints(start, end, 3);
    ASSERT_EQ(5, equidistant_points.size());
    EXPECT_DOUBLE_EQ(0.0, (*equidistant_points[2])[0]);
    EXPECT_DOUBLE_EQ(0.0, (*equidistant_points[2])[1]);
    EXPECT_DOUBLE_EQ(0.0, (*equidistant_points[2])[2]);
    checkLength();
}

TEST_F(GeoLibGenerateEquidistantPoints, FiveSubdivisions)
{
    constexpr double eps = std::numeric_limits<double>::epsilon();
    equidistant_points = GeoLib::generateEquidistantPoints(start, end, 5);
    ASSERT_EQ(7, equidistant_points.size());
    EXPECT_NEAR(0.0, (*equidistant_points[3])[0], eps);
    EXPECT_NEAR(0.0, (*equidistant_points[3])[1], eps);
    EXPECT_NEAR(0.0, (*equidistant_points[3])[2], eps);
    EXPECT_NEAR(
        MathLib::sqrDist(start, *(equidistant_points[1])),
        MathLib::sqrDist(*(equidistant_points[1]), *(equidistant_points[2])),
        eps);
    EXPECT_NEAR(MathLib::sqrDist(start, *(equidistant_points[1])),
                MathLib::sqrDist(*(equidistant_points[5]), end),
                eps);
    checkLength();
}


// Haney's induction calculation benchmark.
//
// See: Scott W. Haney, Is C++ Fast Enough for Scientific Computing?
//      Computers in Physics Vol. 8 No. 6 (1994), p. 690
//
//      Arch D. Robison, C++ Gets Faster for Scientific Computing,
//      Computers in Physics Vol. 10 No. 5 (1996), p. 458
//

#include <blitz/vector.h>
#include <blitz/rand-uniform.h>
#include <blitz/benchext.h>
#ifdef BZ_HAVE_STD
#include <valarray>
#else
#include <valarray.h>
#endif

BZ_USING_NAMESPACE(blitz)

#ifndef M_PI
 #define M_PI   3.14159265358979323846
#endif

#ifdef BZ_FORTRAN_SYMBOLS_WITH_TRAILING_UNDERSCORES
#define vecopsf    vecopsf_
#define vecopsfo   vecopsfo_
#endif

extern "C"
{
	void vecopsf(float *li, const float *R, const float *w, const int &N,
	             const int& iters);
	void vecopsfo(float *li, const float *R, const float *w, const int &N,
	              const int& iters);
}

inline float sqr(float x)
{
	return x*x;
}

const float Mu0 = 4.0 * M_PI * 1.0e-7;

void HaneyCVersion(BenchmarkExt<int>& bench);
void HaneyFortranVersion(BenchmarkExt<int>& bench);
void HaneyBlitzVersion(BenchmarkExt<int>& bench);

int main()
{
	BenchmarkExt<int> bench("Haney Inductance Calculation", 3);

	bench.setRateDescription("Operations/s");

	bench.beginBenchmarking();

	HaneyCVersion(bench);
	HaneyFortranVersion(bench);
	HaneyBlitzVersion(bench);

	bench.endBenchmarking();

	bench.saveMatlabGraph("haney.m");

	return 0;
}

void initializeRandom(float* data, int length)
{
	Random<Uniform> unif(1.0, 2.0);
	for (int i=0; i < length; ++i)
		data[i] = unif.random();
}

void HaneyCVersion(BenchmarkExt<int>& bench)
{
	bench.beginImplementation("Inlined C");

	while (!bench.doneImplementationBenchmark()) {
		int length = bench.getParameter();
		long iters = bench.getIterations();

		cout << "length = " << length << " iters = " << iters << endl;

		float* li = new float[length];
		float* R = new float[length];
		float* w = new float[length];

		initializeRandom(li, length);
		initializeRandom(R, length);
		initializeRandom(w, length);

		// Tickle the cache
		for (int i=0; i < length; ++i)
			li[i] = R[i] + log(w[i]);

		bench.start();

		for (long j=0; j < iters; ++j) {
			for (int i=0; i < length; ++i) {
				li[i] = Mu0 * R[i] *
				        (0.5 * (1.0 + (1.0/24.0)
				                * sqr(w[i]/R[i])) * log(32.0 * sqr(R[i]/w[i]))
				         + 0.05 * sqr(w[i]/R[i]) - 0.85);
			}
		}

		bench.stop();

		// Subtract the loop overhead
		bench.startOverhead();

		for (long j=0; j < iters; ++j) {}

		bench.stopOverhead();

		delete [] li;
		delete [] w;
		delete [] R;
	}

	bench.endImplementation();
}

void HaneyFortranVersion(BenchmarkExt<int>& bench)
{
	bench.beginImplementation("Fortran");

	while (!bench.doneImplementationBenchmark()) {
		int length = bench.getParameter();
		int iters = (int)bench.getIterations();

		cout << "length = " << length << " iters = " << iters << endl;

		float* li = new float[length];
		float* R = new float[length];
		float* w = new float[length];

		initializeRandom(li, length);
		initializeRandom(R, length);
		initializeRandom(w, length);

		// Tickle
		int oneIter = 1;
		vecopsf(li, R, w, length, oneIter);

		// Time
		bench.start();
		vecopsf(li, R, w, length, iters);
		bench.stop();

		// Time overhead
		bench.startOverhead();
		vecopsfo(li, R, w, length, iters);
		bench.stopOverhead();

		delete [] li;
		delete [] w;
		delete [] R;
	}

	bench.endImplementation();
}

void HaneyBlitzVersion(BenchmarkExt<int>& bench)
{
	bench.beginImplementation("Blitz++");

	while (!bench.doneImplementationBenchmark()) {
		int length = bench.getParameter();
		int iters = (int)bench.getIterations();

		Vector<float> li(length), R(length), w(length);
		initializeRandom(li.data(), length);
		initializeRandom(R.data(), length);
		initializeRandom(w.data(), length);

		cout << "length = " << length << " iters = " << iters << endl;

		// Tickle
		li = w + log(R);

		// Time
		bench.start();
		for (long i=0; i < iters; ++i) {
#if defined(__GNUC__) && (__GNUC__ < 3)
			li = Mu0 * R * ( (0.5 + (0.5/24.0) * sqr(w/R) )
			                 * log(32.0 * sqr(R/w)) + 0.05 * sqr(w/R) - 0.85);
#else
			li = Mu0 * R * (0.5 * (1.0 + (1.0/24.0) * sqr(w/R))
			                * log(32.0 * sqr(R/w)) + 0.05 * sqr(w/R) - 0.85);
#endif
		}
		bench.stop();

		// Time overhead
		bench.startOverhead();
		for (long i=0; i < iters; ++i) {
		}
		bench.stopOverhead();
	}

	bench.endImplementation();
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
Matrix3i m = Matrix3i::Random();
cout << "Here is the initial matrix m:" << endl << m << endl;
int i = -1;
for(auto c: m.colwise()) {
  c *= i;
  ++i;
}
cout << "Here is the matrix m after the for-range-loop:" << endl << m << endl;
auto cols = m.colwise();
auto it = std::find_if(cols.cbegin(), cols.cend(),
                       [](Matrix3i::ConstColXpr x) { return x.squaredNorm() == 0; });
cout << "The first empty column is: " << distance(cols.cbegin(),it) << endl;

}
  return 0;
}


#include "crit_dijkstra.hpp"
#include "dijkstra.hpp"
#include "graph.hpp"
#include <boost/test/unit_test.hpp>
#include <cfloat>

using namespace sssp;
namespace tt = boost::test_tools;

BOOST_AUTO_TEST_CASE(dijkstra_basic_test_1) {
    graph g;
    for (size_t i = 0; i < 4; ++i) {
        g.add_node();
    }
    g.add_edge(0, 1, 1.0);
    g.add_edge(0, 2, 0.3);
    g.add_edge(2, 3, 0.3);
    g.add_edge(3, 1, 0.3);

    boost::base_collection<criteria> criteria;
    criteria.insert(smallest_tentative_distance(&g, 0));
    node_map<dijkstra_result> result = dijkstra(g, 0, criteria);

    BOOST_TEST_REQUIRE(result.size() == 4);
    BOOST_TEST(result[0].predecessor == -1);
    BOOST_TEST(result[1].predecessor == 3);
    BOOST_TEST(result[2].predecessor == 0);
    BOOST_TEST(result[3].predecessor == 2);
    BOOST_TEST(result[0].distance == 0.0, tt::tolerance(DBL_EPSILON));
    BOOST_TEST(result[1].distance == 0.9, tt::tolerance(DBL_EPSILON));
    BOOST_TEST(result[2].distance == 0.3, tt::tolerance(DBL_EPSILON));
    BOOST_TEST(result[3].distance == 0.6, tt::tolerance(DBL_EPSILON));
}

BOOST_AUTO_TEST_CASE(dijkstra_basic_test_2) {
    graph g;
    for (size_t i = 0; i < 10; ++i) {
        g.add_node();
    }
    for (size_t i = 0; i < 10; ++i) {
        for (size_t j = 0; j < 10; ++j) {
            if (i != j) {
                g.add_edge(i, j, 1.0);
            }
        }
    }

    boost::base_collection<criteria> criteria;
    criteria.insert(smallest_tentative_distance(&g, 0));
    node_map<dijkstra_result> result = dijkstra(g, 0, criteria);

    BOOST_TEST_REQUIRE(result.size() == 10);
    BOOST_TEST(result[0].predecessor == -1);
    BOOST_TEST(result[0].distance == 0.0, tt::tolerance(DBL_EPSILON));
    for (size_t i = 1; i < 10; ++i) {
        BOOST_TEST(result[i].predecessor == 0);
        BOOST_TEST(result[i].distance == 1.0, tt::tolerance(DBL_EPSILON));
    }
}

BOOST_AUTO_TEST_CASE(dijkstra_basic_test_3) {
    graph g;
    g.add_node();
    g.add_node();
    g.add_node();
    g.add_edge(0, 1, 1.0);
    g.add_edge(1, 0, 1.0);
    g.add_edge(2, 1, 1.0);
    g.add_edge(2, 0, 1.0);

    boost::base_collection<criteria> criteria;
    criteria.insert(smallest_tentative_distance(&g, 0));
    node_map<dijkstra_result> result = dijkstra(g, 0, criteria);

    BOOST_TEST_REQUIRE(result.size() == 3);
    BOOST_TEST(result[0].predecessor == -1);
    BOOST_TEST(result[1].predecessor == 0);
    BOOST_TEST(result[2].predecessor == -1);
    BOOST_TEST(result[0].distance == 0.0, tt::tolerance(DBL_EPSILON));
    BOOST_TEST(result[1].distance == 1.0, tt::tolerance(DBL_EPSILON));
    BOOST_TEST(result[2].distance == INFINITY);
}


// Copyright (C) 2017 Minhyuk Sung <mhsung@cs.stanford.edu>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
//

#include "LibiglMesh.h"

#include <Eigen/Geometry>
#include <igl/doublearea.h>
#include <igl/per_face_normals.h>
#include <igl/random_points_on_mesh.h>
#include <modules/PCA.h>
#include <modules/remove_duplicates_custom.h>
#include <utils/utils.h>

void LibiglMesh::sample_points_on_mesh(
    const int _num_points, const bool _with_normals) {
  // NOTE: 03-24-2017
  // Remove duplicated faces in mesh before sampling points.
  MatrixXi newF;
  igl::remove_duplicate_faces_custom(F_, newF);

  SparseMatrix<double> B;
  // NOTE: 02-25-2018
  // Assign face indices to points labels.
  igl::random_points_on_mesh(_num_points, V_, newF, B, PL_);
  const VectorXi& FI = PL_;
  P_ = B * V_;

  if (_with_normals) {
    igl::per_face_normals(V_, F_, FN_);
    PN_ = Utils::slice_rows(FN_, FI);
  }
}

void LibiglMesh::normalize_points() {
  const int num_samples = P_.rows();
  CHECK_GT(num_samples, 0);

  // Compute center and bounding box diagonal.
  const auto bb_min = P_.colwise().minCoeff();
  const auto bb_max = P_.colwise().maxCoeff();

  const RowVector3d bb_center = 0.5 * (bb_min + bb_max);
  P_ = P_.rowwise() - bb_center;

  const auto radius = P_.rowwise().norm().maxCoeff();
  P_ = P_ / radius;
}

void LibiglMesh::compute_point_set_center_and_area(
    const std::string& _out_file, bool _centerize) {
  const int num_samples = P_.rows();
  CHECK_GT(num_samples, 0);

  // Compute center and bounding box diagonal.
  const auto bb_min = P_.colwise().minCoeff();
  const auto bb_max = P_.colwise().maxCoeff();

  // NOTE: 03-24-2017
  // Use bounding box center as center instead of mean of point positions.
  //const RowVector3d center = P_.colwise().mean();
  const RowVector3d bb_center = 0.5 * (bb_min + bb_max);

  // NOTE: 03-24-2017
  // Use sum of face areas as size instead of bounding box diagonal.
  //const double bbox_diagonal = (bb_max - bb_min).norm();
  MatrixXi newF;
  igl::remove_duplicate_faces_custom(F_, newF);
  VectorXd FA;
  igl::doublearea(V_, newF, FA);
  const double sum_face_areas = 0.5 * FA.sum();

  if (_centerize) {
    // Centerize point set.
    P_ = P_.rowwise() - bb_center;
  }

  if (_out_file != "") {
    RowVector4d center_and_area;
    //center_and_area << center, bbox_diagonal;
    center_and_area << bb_center, sum_face_areas;
    Utils::write_eigen_matrix_to_file(_out_file, center_and_area);
  }
}

void LibiglMesh::pca_align_points(const std::string& _out_file) {
  const int num_samples = P_.rows();
  CHECK_GT(num_samples, 0);

  // Compute PCA transformation matrix.
  Affine3d T = Affine3d::Identity();
  igl::PCA(P_, T);

  // PCA-align point set.
  const Eigen::Matrix<double, Dynamic, 3>& P_temp = P_;
  P_ = (T * P_temp.transpose()).transpose();

  // NOTE:
  // P.transpose == R * P_aligned.transpose() + t
  Affine3d T_inv = T.inverse();
  const Matrix3d R = T_inv.rotation();
  const Vector3d t = T_inv.translation();

  const AngleAxisd rotation(R);
  double angle = rotation.angle();
  // Make angle to be in [-pi, +pi) range.
  while (angle < -M_PI) angle += (2.0 * M_PI);
  while (angle >= M_PI) angle -= (2.0 * M_PI);
  Vector3d axis = rotation.axis().normalized();

  const Vector3d r = angle * axis;

  // NOTE: 03-24-2017
  // Add sum of face areas.
  MatrixXi newF;
  igl::remove_duplicate_faces_custom(F_, newF);
  VectorXd FA;
  igl::doublearea(V_, newF, FA);
  const double a = 0.5 * FA.sum();

  // Scale along the first PCA axis.
  const double s = P_.col(0).maxCoeff() - P_.col(0).minCoeff();

  VectorXd transformation(8);
  transformation << r, t, a, s;
  if (_out_file != "") {
    Utils::write_eigen_matrix_to_file(_out_file, transformation.transpose());
  }
}


#include "SimpleHydraulicLib/ConvEigen.h"

#include <Eigen/QR>

#include <fstream>
#include <iostream>
#include <sstream>  // std::stringstream
#include <stdexcept>// std::runtime_error
#include <string>
#include <utility>// std::pair
#include <vector>

using Eigen::VectorXd;
using std::cerr;
using std::cout;

void process() {
    //    // Read three_cols.csv and ones.csv
    //    std::vector<std::pair<std::string, std::vector<double>>> mul_cols = read_csv("Heating_Cooling_data.csv");
    //
    //    // Write to another file to check that this was successful
    //    write_csv("Heating_Cooling_data_copy.csv", mul_cols);
    // matrix to be loaded from a file
    MatrixXd data;

    // load the matrix from the file
    data = openData("inputs/matrix.csv");
    VectorXd Demand_A = data.col(0);
    VectorXd Demand_B = data.col(1);
    VectorXd Demand_C = data.col(2);
    VectorXd Demand_D = data.col(3);
    VectorXd Demand_E = data.col(4);
    VectorXd Demand_F = data.col(5);
    VectorXd Demand_z0(8760);
    Demand_z0.setZero();
    VectorXd Demand_z4(8760);
    Demand_z4.setZero();

    Demand_A = Demand_A / 4.2 / 15;
    Demand_B = Demand_B / 4.2 / 15;
    Demand_C = Demand_C / 4.2 / 15;
    Demand_D = Demand_D / 4.2 / 15;
    Demand_E = Demand_E / 4.2 / 15;
    Demand_F = Demand_F / 4.2 / 15;

    //   cerr << Demand_C.size() << "\n";
    MatrixXd B(8760, 8);
    B << Demand_z0, Demand_C, Demand_B, Demand_D, Demand_z4, Demand_A, Demand_E, Demand_F;
    //   cerr << B << "\n";

    MatrixXd Q = MatrixXd::Zero(8760, 8);
    Q.col(1) = Demand_C;
    Q.col(2) = Demand_B;
    Q.col(3) = Demand_D;
    Q.col(4) = Demand_A + Demand_E + Demand_F;
    Q.col(5) = Demand_A;
    Q.col(6) = Demand_E;
    Q.col(7) = Demand_F;
    Q.col(0) = Demand_C + Demand_B + Demand_D + Demand_A + Demand_E + Demand_F;
    cerr << "The first set of solutions is \n"
         << Q.row(0) << "\n";
    saveData("outputs/flowQ.csv", Q);
    cerr << Q.rows() << " rows and " << Q.cols() << " columns.";
    //    MatrixXd A(8, 8);
    //    A << 1, -1, -1, -1, -1, 0, 0, 0,
    //            0, 1, 0, 0, 0, 0, 0, 0,
    //            0, 0, 1, 0, 0, 0, 0, 0,
    //            0, 0, 0, 1, 0, 0, 0, 0,
    //            0, 0, 0, 0, 1, -1, -1, 1,
    //            0, 0, 0, 0, 0, 1, 0, 0,
    //            0, 0, 0, 0, 0, 0, 1, 0,
    //            0, 0, 0, 0, 0, 0, 0, 1;

    //MatrixXd X = A.colPivHouseholderQr().solve(B.transpose());
    //    MatrixXd X = A.lu().solve(B.transpose());
    //    cerr << "The condition number is " << A.lu().rcond() << "\n";
    //    cerr << "The first set of solutions is \n" << X.col(0) << "\n";
    //    double relative_err = (A*X - B.transpose()).norm()/B.transpose().norm();
    //    cerr << "The relative error is:\n" << relative_err << "\n";
    //    saveData("outputs/flow.csv", X.transpose());
    ////    Eigen::IOFormat csv(-1, 0, ", ", "\n");
    ////    std::cout << X.format(csv) << std::endl;
    //    cerr << X.rows() << " rows and " << X.cols() << " columns.";

    //    for (int i = 0; i < data.rows(); ++i){
    //        Demand_A(i) = Demand_A(i)/4.2/15;
    //        Demand_B(i) = Demand_B(i)/4.2/15;
    //        Demand_C(i) = Demand_C(i)/4.2/15;
    //        Demand_D(i) = Demand_D(i)/4.2/15;
    //        Demand_E(i) = Demand_E(i)/4.2/15;
    //        Demand_F(i) = Demand_F(i)/4.2/15;
    //;    }
    // MatrixXd Q = MatrixXd::Zero(8760, 8);

    //    for (int i = 0; i < data.rows(); ++i){
    //        b << 0.0, Demand_C(i), Demand_B(i), Demand_D(i), 0.0, Demand_A(i), Demand_E(i), Demand_F(i);
    //        VectorXd x = A.colPivHouseholderQr().solve(b);
    //        cout << "The solution is:\n" << x << "\n";
    //    }
}

/*
 * Our accuracy are good but we are getting some systematic error as the solutions from Matlab and C++ deviate.
 * This might due to the non-empty nullspace. If our matrix is rank deficient the solution may have an arbitrary large component added in the null space.
 * In other words if Ax = b then the computed solution might be x' = x* + x0, where A*x0 = 0, so r = A*x'-b = 0.
 * So, we could have perfect accuracy but still produce different solutions due to the non-empty nullspace.
 * If my system is well-conditioned, I will not need to worry about that.  If it isn't, it might help explain the systematic error I'm getting.
 *
 */

/*
 * Precision could be lost when reading/writing CSV file
 * Residual error (or existence of solutions)
 * Rank deficiency
 * Well conditionness
 */