/* ----------------------------------------------------------------------------
 * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file  main.cpp
 * @brief Spider trajectory optimization with pre-specified footholds.
 * @Author: Alejandro Escontrela
 * @Author: Stephanie McCormick
 * @Author: Disha Das
 * @Author: Tarushree Gandhi
 */

#include <gtdynamics/dynamics/DynamicsGraph.h>
#include <gtdynamics/dynamics/OptimizerSetting.h>
#include <gtdynamics/factors/MinTorqueFactor.h>
#include <gtdynamics/factors/PointGoalFactor.h>
#include <gtdynamics/universal_robot/Robot.h>
#include <gtdynamics/universal_robot/sdf.h>
#include <gtdynamics/utils/DynamicsSymbol.h>
#include <gtdynamics/utils/initialize_solution_utils.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/PriorFactor.h>

#include <algorithm>
#include <boost/algorithm/string/join.hpp>
#include <boost/optional.hpp>
#include <fstream>
#include <iostream>
#include <utility>

#define GROUND_HEIGHT -1.75

using std::string;
using std::vector;

using gtsam::Point3;
using gtsam::Pose3;
using gtsam::Rot3;
using gtsam::Values;
using gtsam::Vector;
using gtsam::Vector3;
using gtsam::Vector6;
using gtsam::noiseModel::Isotropic;

using namespace gtdynamics;

int main(int argc, char** argv) {
  // Load Stephanie's spider robot.
  auto robot = gtdynamics::CreateRobotFromFile(kSdfPath + string("spider.sdf"),
                                               "spider");

  double sigma_dynamics = 1e-5;    // std of dynamics constraints.
  double sigma_objectives = 1e-6;  // std of additional objectives.
  double sigma_joints = 1.85e-4;   // 1.85e-4

  // Noise models.
  auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics),
       dynamics_model_3 = Isotropic::Sigma(3, sigma_dynamics),
       dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics),
       dynamics_model_1_2 = Isotropic::Sigma(1, sigma_joints),
       objectives_model_6 = Isotropic::Sigma(6, sigma_objectives),
       objectives_model_3 = Isotropic::Sigma(3, sigma_objectives),
       objectives_model_1 = Isotropic::Sigma(1, sigma_objectives);

  // TODO(aescontrela): Make a constructor for OptimizerSetting that
  //     initializes all noise models with the same sigma.
  auto opt = gtdynamics::OptimizerSetting();
  opt.bp_cost_model = dynamics_model_6;
  opt.bv_cost_model = dynamics_model_6;
  opt.ba_cost_model = dynamics_model_6;
  opt.p_cost_model = dynamics_model_6;
  opt.v_cost_model = dynamics_model_6;
  opt.a_cost_model = dynamics_model_6;
  opt.f_cost_model = dynamics_model_6;
  opt.fa_cost_model = dynamics_model_6;
  opt.t_cost_model = dynamics_model_1;
  opt.cp_cost_model = dynamics_model_1;
  opt.cfriction_cost_model = dynamics_model_1;
  opt.cv_cost_model = dynamics_model_3;
  opt.ca_cost_model = dynamics_model_3;
  opt.planar_cost_model = dynamics_model_3;
  opt.prior_q_cost_model = dynamics_model_1;
  opt.prior_qv_cost_model = dynamics_model_1;
  opt.prior_qa_cost_model = dynamics_model_1;
  opt.prior_t_cost_model = dynamics_model_1;
  opt.q_col_cost_model = dynamics_model_1;
  opt.v_col_cost_model = dynamics_model_1;
  opt.time_cost_model = dynamics_model_1;

  // Env parameters.
  Vector3 gravity(0, 0, -9.8);
  double mu = 1.0;

  auto graph_builder = gtdynamics::DynamicsGraph(opt, gravity);

  vector<string> links = {"tarsus_1_L1", "tarsus_2_L2", "tarsus_3_L3",
                          "tarsus_4_L4", "tarsus_5_R4", "tarsus_6_R3",
                          "tarsus_7_R2", "tarsus_8_R1"};
  // All contacts.
  const Point3 contact_in_com(0, 0.19, 0);
  PointOnLink cp1(robot.link("tarsus_1_L1"), contact_in_com);  // Front left.
  PointOnLink cp2(robot.link("tarsus_2_L2"), contact_in_com);  // Hind left.
  PointOnLink cp3(robot.link("tarsus_3_L3"), contact_in_com);  // Front right.
  PointOnLink cp4(robot.link("tarsus_4_L4"), contact_in_com);  // Hind right.
  PointOnLink cp5(robot.link("tarsus_5_R4"), contact_in_com);  // Front left.
  PointOnLink cp6(robot.link("tarsus_6_R3"), contact_in_com);  // Hind left.
  PointOnLink cp7(robot.link("tarsus_7_R2"), contact_in_com);  // Front right.
  PointOnLink cp8(robot.link("tarsus_8_R1"), contact_in_com);  // Hind right.

  // Contact points for each phase.
  // This gait moves one leg at a time.
  using CPs = PointOnLinks;
  CPs t00 = {cp1, cp2, cp3, cp4, cp5, cp6, cp7, cp8};
  // Initially stationary.
  CPs p0 = {cp1, cp2, cp3, cp4, cp5, cp6, cp7, cp8};
  CPs t01 = {cp2, cp3, cp4, cp5, cp6, cp7, cp8};
  CPs p1 = {cp2, cp3, cp4, cp5, cp6, cp7, cp8};
  CPs t12 = {cp3, cp4, cp5, cp6, cp7, cp8};
  CPs p2 = {cp1, cp3, cp4, cp5, cp6, cp7, cp8};
  CPs t23 = {cp1, cp4, cp5, cp6, cp7, cp8};
  CPs p3 = {cp1, cp2, cp4, cp5, cp6, cp7, cp8};
  CPs t34 = {cp1, cp2, cp5, cp6, cp7, cp8};
  CPs p4 = {cp1, cp2, cp3, cp5, cp6, cp7, cp8};
  CPs t45 = {cp1, cp2, cp3, cp6, cp7, cp8};
  CPs p5 = {cp1, cp2, cp3, cp4, cp6, cp7, cp8};
  CPs t56 = {cp1, cp2, cp3, cp4, cp7, cp8};
  CPs p6 = {cp1, cp2, cp3, cp4, cp5, cp7, cp8};
  CPs t67 = {cp1, cp2, cp3, cp4, cp5, cp8};
  CPs p7 = {cp1, cp2, cp3, cp4, cp5, cp6, cp8};
  CPs t78 = {cp1, cp2, cp3, cp4, cp5, cp6};
  CPs p8 = {cp1, cp2, cp3, cp4, cp5, cp6, cp7};
  CPs t80 = {cp2, cp3, cp4, cp5, cp6, cp7};

  // This gait moves four legs at a time (alternating tetrapod).
  CPs t0a = {cp2, cp4, cp6, cp8};
  CPs pa = {cp2, cp4, cp6, cp8};
  CPs tab = {};
  CPs pb = {cp1, cp3, cp5, cp7};
  CPs tb0 = {cp1, cp3, cp5, cp7};

  // Define contact points for each phase, transition contact points,
  // and phase durations.
  // Alternating Tetrapod:
  vector<CPs> phase_cps = {p0, pa, p0, pb, p0, pa, p0, pb, p0, pa,
                           p0, pb, p0, pa, p0, pb, p0, pa, p0, pb};
  vector<CPs> trans_cps = {t0a, t0a, tb0, tb0, t0a, t0a, tb0, tb0, t0a, t0a,
                           tb0, tb0, t0a, t0a, tb0, tb0, t0a, t0a, tb0};
  vector<int> phase_steps = {20, 20, 20, 20, 20, 20, 20, 20, 20, 20,
                             20, 20, 20, 20, 20, 20, 20, 20, 20, 20};

  // Define noise to be added to initial values, desired timestep duration,
  // vector of link name strings, robot model for each phase, and
  // phase transition initial values.
  double gaussian_noise = 1e-5;

  double dt_des = 1. / 240;
  vector<Values> transition_graph_init;

  // Define the cumulative phase steps.
  vector<int> cum_phase_steps;
  for (int i = 0; i < phase_steps.size(); i++) {
    int cum_val =
        i == 0 ? phase_steps[0] : phase_steps[i] + cum_phase_steps[i - 1];
    cum_phase_steps.push_back(cum_val);
    std::cout << cum_val << std::endl;
  }
  int t_f = cum_phase_steps[cum_phase_steps.size() - 1];  // Final timestep.

  // Collocation scheme.
  auto collocation = gtdynamics::CollocationScheme::Euler;

  // Graphs for transition between phases + their initial values.
  vector<gtsam::NonlinearFactorGraph> transition_graphs;
  for (int p = 1; p < phase_cps.size(); p++) {
    std::cout << "Creating transition graph" << std::endl;
    transition_graphs.push_back(graph_builder.dynamicsFactorGraph(
        robot, cum_phase_steps[p - 1], trans_cps[p - 1], mu));
    std::cout << "Creating initial values" << std::endl;
    transition_graph_init.push_back(ZeroValues(
        robot, cum_phase_steps[p - 1], gaussian_noise, trans_cps[p - 1]));
  }

  // Construct the multi-phase trajectory factor graph.
  std::cout << "Creating dynamics graph" << std::endl;
  auto graph = graph_builder.multiPhaseTrajectoryFG(
      robot, phase_steps, transition_graphs, collocation, phase_cps, mu);

  // Build the objective factors.
  gtsam::NonlinearFactorGraph objective_factors;
  auto base_link = robot.link("body");

  std::map<string, gtdynamics::LinkSharedPtr> link_map;
  for (auto&& link : links)
    link_map.insert(std::make_pair(link, robot.link(link)));

  // Previous contact point goal.
  std::map<string, Point3> prev_cp;
  for (auto&& link : links) {
    prev_cp.insert(std::make_pair(link, link_map[link]->bMcom() * cp1.point));
  }

  // Distance to move contact point per time step during swing.
  auto contact_offset = Point3(0, 0.007, 0);

  // Set this to 'right' or 'left' to make the spider rotate in place
  string turn = "right";

  // Add contact point objectives to factor graph.
  for (int p = 0; p < phase_cps.size(); p++) {
    // Phase start and end timesteps.
    int t_p_i = cum_phase_steps[p] - phase_steps[p];
    if (p != 0) t_p_i += 1;
    int t_p_f = cum_phase_steps[p];

    // Obtain the contact links and swing links for this phase.
    vector<string> phase_contact_links;
    for (auto&& cp : phase_cps[p]) {
      phase_contact_links.push_back(cp.link->name());
    }
    vector<string> phase_swing_links;
    for (auto&& l : links) {
      if (std::find(phase_contact_links.begin(), phase_contact_links.end(),
                    l) == phase_contact_links.end()) {
        phase_swing_links.push_back(l);
      }
    }

    for (int t = t_p_i; t <= t_p_f; t++) {
      // Normalized phase progress.
      double t_normed = (double)(t - t_p_i) / (double)(t_p_f - t_p_i);

      for (auto&& pcl : phase_contact_links) {
        // TODO(aescontrela): Use correct contact point for each link.
        // TODO(frank): #179 make sure height is handled correctly.
        objective_factors.add(gtdynamics::PointGoalFactor(
            internal::PoseKey(link_map[pcl]->id(), t),
            Isotropic::Sigma(3, 1e-7), cp1.point,
            Point3(prev_cp[pcl].x(), prev_cp[pcl].y(), GROUND_HEIGHT - 0.05)));
      }

      double h =
          GROUND_HEIGHT + std::pow(t_normed, 1.1) * std::pow(1 - t_normed, 0.7);

      for (auto&& psl : phase_swing_links) {
        objective_factors.add(gtdynamics::PointGoalFactor(
            internal::PoseKey(link_map[psl]->id(), t),
            Isotropic::Sigma(3, 1e-7), cp1.point,
            Point3(prev_cp[psl].x(), prev_cp[psl].y(), h)));
      }

      // Update the goal point for the swing links.
      for (auto&& psl : phase_swing_links) {
        if (turn.compare("right") == 0) {
          if (psl.find_first_of("1234") != string::npos)
            prev_cp[psl] = prev_cp[psl] + contact_offset;
          else
            prev_cp[psl] = prev_cp[psl] - contact_offset;
        } else {
          if (psl.find_first_of("5678") != string::npos)
            prev_cp[psl] = prev_cp[psl] + contact_offset;
          else
            prev_cp[psl] = prev_cp[psl] - contact_offset;
        }
      }
    }
  }

  // Add base goal objectives to the factor graph.
  for (int t = 0; t <= t_f; t++) {
    objective_factors.add(gtsam::PriorFactor<gtsam::Pose3>(
        internal::PoseKey(base_link->id(), t),
        gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0, 0.0, 0.5)),
        Isotropic::Sigma(6, 6e-5)));  // 6.2e-5
  }

  // Add link boundary conditions to FG.
  for (auto&& link : robot.links()) {
    // Initial link pose, twists.
    objective_factors.add(gtsam::PriorFactor<gtsam::Pose3>(
        internal::PoseKey(link->id(), 0), link->bMcom(), dynamics_model_6));
    objective_factors.add(gtsam::PriorFactor<Vector6>(
        internal::TwistKey(link->id(), 0), Vector6::Zero(), dynamics_model_6));

    // Final link twists, accelerations.
    objective_factors.add(
        gtsam::PriorFactor<Vector6>(internal::TwistKey(link->id(), t_f),
                                    Vector6::Zero(), objectives_model_6));
    objective_factors.add(
        gtsam::PriorFactor<Vector6>(internal::TwistAccelKey(link->id(), t_f),
                                    Vector6::Zero(), objectives_model_6));
  }

  // Add joint boundary conditions to FG.
  for (auto&& joint : robot.joints()) {
    // Add priors to joint angles
    for (int t = 0; t <= t_f; t++) {
      if (joint->name().find("hip_") == 0) {
        objective_factors.add(gtsam::PriorFactor<double>(
            internal::JointAngleKey(joint->id(), t), 0, dynamics_model_1_2));
      } else if (joint->name().find("hip2") == 0) {
        objective_factors.add(gtsam::PriorFactor<double>(
            internal::JointAngleKey(joint->id(), t), 0.9, dynamics_model_1_2));
      } else if (joint->name().find("knee") == 0) {
        objective_factors.add(
            gtsam::PriorFactor<double>(internal::JointAngleKey(joint->id(), t),
                                       -1.22, dynamics_model_1_2));
      } else {
        objective_factors.add(gtsam::PriorFactor<double>(
            internal::JointAngleKey(joint->id(), t), 0.26, dynamics_model_1_2));
      }
    }

    objective_factors.add(gtsam::PriorFactor<double>(
        internal::JointVelKey(joint->id(), 0), 0.0, dynamics_model_1));

    objective_factors.add(gtsam::PriorFactor<double>(
        internal::JointVelKey(joint->id(), t_f), 0.0, objectives_model_1));
    objective_factors.add(gtsam::PriorFactor<double>(
        internal::JointAccelKey(joint->id(), t_f), 0.0, objectives_model_1));
  }

  // Add prior factor constraining all Phase keys to have duration of 1 / 240.
  for (int phase = 0; phase < phase_steps.size(); phase++)
    objective_factors.add(gtsam::PriorFactor<double>(
        PhaseKey(phase), dt_des,
        gtsam::noiseModel::Isotropic::Sigma(1, 1e-30)));

  // Add min torque objectives.
  for (int t = 0; t <= t_f; t++) {
    for (auto&& joint : robot.joints())
      objective_factors.add(gtdynamics::MinTorqueFactor(
          internal::TorqueKey(joint->id(), t),
          gtsam::noiseModel::Gaussian::Covariance(gtsam::I_1x1)));
  }
  graph.add(objective_factors);

  // Initialize solution.
  gtsam::Values init_vals;
  init_vals = gtdynamics::MultiPhaseZeroValuesTrajectory(
      robot, phase_steps, transition_graph_init, dt_des, gaussian_noise,
      phase_cps);

  // Optimize!
  gtsam::LevenbergMarquardtParams params;
  params.setVerbosityLM("SUMMARY");
  params.setlambdaInitial(1e0);
  params.setlambdaLowerBound(1e-7);
  params.setlambdaUpperBound(1e10);
  gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params);
  auto results = optimizer.optimize();

  vector<string> joint_names;
  for (auto&& joint : robot.joints()) joint_names.push_back(joint->name());
  string joint_names_str = boost::algorithm::join(joint_names, ",");
  std::ofstream traj_file;

  traj_file.open("rotation_traj.csv");
  // angles, vels, accels, torques, time.
  traj_file << joint_names_str << "," << joint_names_str << ","
            << joint_names_str << "," << joint_names_str << ",t"
            << "\n";
  int t = 0;
  for (int phase = 0; phase < phase_steps.size(); phase++) {
    for (int phase_step = 0; phase_step < phase_steps[phase]; phase_step++) {
      vector<string> vals;
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(JointAngle(results, joint->id(), t)));
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(JointVel(results, joint->id(), t)));
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(JointAccel(results, joint->id(), t)));
      for (auto&& joint : robot.joints())
        vals.push_back(std::to_string(Torque(results, joint->id(), t)));
      vals.push_back(std::to_string(results.atDouble(PhaseKey(phase))));
      t++;
      string vals_str = boost::algorithm::join(vals, ",");
      traj_file << vals_str << "\n";
    }
  }
  traj_file.close();

  return 0;

}  // namespace gtdynamics


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. 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_
//

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/detail/lightweight_test.hpp>
#include <boost/array.hpp>
#include "test.hpp"

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/random.hpp>

#ifdef TEST_GMP
#include <boost/multiprecision/gmp.hpp>
#endif
#ifdef TEST_TOMMATH
#include <boost/multiprecision/tommath.hpp>
#endif
#ifdef TEST_MPFR
#include <boost/multiprecision/mpfr.hpp>
#endif

int main()
{
   using namespace boost::multiprecision;
   using namespace boost::random;

   independent_bits_engine<mt11213b, 1024, cpp_int> gen;

   for(unsigned i = 0; i < 100; ++i)
   {
      cpp_int c = gen();
      //
      // Integer to integer conversions first:
      //
#ifdef TEST_GMP
      mpz_int z(c);
      cpp_int t(z);
      BOOST_CHECK_EQUAL(t, c);
      z.assign(-c);
      t.assign(-z);
      BOOST_CHECK_EQUAL(t, c);
#endif
#ifdef TEST_TOMMATH
      tom_int tom(c);
      cpp_int t2(tom);
      BOOST_CHECK_EQUAL(t2, c);
      tom.assign(-c);
      t2.assign(-tom);
      BOOST_CHECK_EQUAL(t2, c);
#endif
      //
      // Now integer to float:
      //
      typedef number<cpp_dec_float<500> > dec_float_500;
      dec_float_500 df(c);
      dec_float_500 df2(c.str());
      BOOST_CHECK_EQUAL(df, df2);
      df.assign(-c);
      df2 = -df2;
      BOOST_CHECK_EQUAL(df, df2);
#ifdef TEST_GMP
      typedef number<gmp_float<500> > mpf_type;
      mpf_type mpf(c);
      mpf_type mpf2(c.str());
      BOOST_CHECK_EQUAL(mpf, mpf2);
      mpf.assign(-c);
      mpf2 = -mpf2;
      BOOST_CHECK_EQUAL(mpf, mpf2);
#endif
#ifdef TEST_MPFR
      typedef number<mpfr_float_backend<500> > mpfr_type;
      mpfr_type mpfr(c);
      mpfr_type mpfr2(c.str());
      BOOST_CHECK_EQUAL(mpfr, mpfr2);
      mpfr.assign(-c);
      mpfr2 = -mpfr2;
      BOOST_CHECK_EQUAL(mpfr, mpfr2);
#endif
      //
      // Now float to float:
      //
      df.assign(c);
      df /= dec_float_500(gen());
      dec_float_500 tol("1e-500");
#ifdef TEST_GMP
      mpf.assign(df);
      mpf2 = static_cast<mpf_type>(df.str());
      BOOST_CHECK_EQUAL(mpf, mpf2);
      df.assign(mpf);
      df2 = static_cast<dec_float_500>(mpf.str());
      BOOST_CHECK(fabs((df - df2) / df) < tol);
#endif
#ifdef TEST_MPFR
      mpfr.assign(df);
      mpfr2 = static_cast<mpfr_type>(df.str());
      BOOST_CHECK_EQUAL(mpfr, mpfr2);
      df.assign(mpfr);
      df2 = static_cast<dec_float_500>(mpfr.str());
      BOOST_CHECK(fabs((df - df2) / df) < tol);
#endif
      //
      // Rational to rational conversions:
      //
      cpp_rational cppr(c, gen());
#ifdef TEST_GMP
      mpq_rational mpq(cppr);
      cpp_rational cppr2(mpq);
      BOOST_CHECK_EQUAL(cppr, cppr2);
#endif
#ifdef TEST_TOMMATH
      tom_rational tr(cppr);
      cpp_rational cppr3(tr);
      BOOST_CHECK_EQUAL(cppr, cppr3);
#endif
      //
      // Integer to rational conversions:
      //
#ifdef TEST_GMP
      mpq.assign(c);
      mpq_rational mpq2 = static_cast<mpq_rational>(c.str());
      BOOST_CHECK_EQUAL(mpq, mpq2);
#endif
#ifdef TEST_TOMMATH
      tr.assign(c);
      tom_rational tr2 = static_cast<tom_rational>(c.str());
      BOOST_CHECK_EQUAL(tr, tr2);
#endif
      //
      // Rational to float:
      //
      df.assign(cppr);
      df2.assign(numerator(cppr));
      df2 /= dec_float_500(denominator(cppr));
      BOOST_CHECK(fabs(df - df2) / df2 < tol);
   }

   return boost::report_errors();
}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/robust_rotation_estimator.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <ceres/rotation.h>
#include <unordered_map>

#include "theia/math/l1_solver.h"
#include "theia/math/matrix/sparse_cholesky_llt.h"
#include "theia/math/rotation.h"
#include "theia/sfm/types.h"
#include "theia/util/hash.h"
#include "theia/util/map_util.h"

namespace theia {

bool RobustRotationEstimator::EstimateRotations(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    std::unordered_map<ViewId, Eigen::Vector3d>* global_orientations) {
  for (const auto& view_pair : view_pairs) {
    AddRelativeRotationConstraint(view_pair.first, view_pair.second.rotation_2);
  }
  return EstimateRotations(global_orientations);
}

void RobustRotationEstimator::AddRelativeRotationConstraint(
    const ViewIdPair& view_id_pair, const Eigen::Vector3d& relative_rotation) {
  // Store the relative orientation constraint.
  relative_rotations_.emplace_back(view_id_pair, relative_rotation);
}

bool RobustRotationEstimator::EstimateRotations(
    std::unordered_map<ViewId, Eigen::Vector3d>* global_orientations) {
  CHECK_GT(relative_rotations_.size(), 0)
      << "Relative rotation constraints must be added to the robust rotation "
         "solver before estimating global rotations.";
  global_orientations_ = CHECK_NOTNULL(global_orientations);

  if (fixed_view_ids_.size() == 0) {
    // just set the first rotation fix
    fixed_view_ids_.insert(std::begin(*global_orientations)->first);
    nr_fixed_rotations_ = 1;
  }
  // Compute a mapping of view ids to indices in the linear system. One rotation
  // will have an index of -1 and will not be added to the linear system. This
  // will remove the gauge freedom (effectively holding one camera as the
  // identity rotation).
  int index = 0;
  int fix_index = -nr_fixed_rotations_;
  view_id_to_index_.reserve(global_orientations->size());
  for (const auto& orientation : *global_orientations) {
      if (fixed_view_ids_.find(orientation.first) == fixed_view_ids_.end()) {
          view_id_to_index_[orientation.first] = index;
          ++index;
      } else {
          view_id_to_index_[orientation.first] = fix_index;
          ++fix_index;
      }
  }

  Eigen::SparseMatrix<double> sparse_mat;
  SetupLinearSystem();

  if (!SolveL1Regression()) {
    LOG(ERROR) << "Could not solve the L1 regression step.";
    return false;
  }

  if (!SolveIRLS()) {
    LOG(ERROR) << "Could not solve the least squares error step.";
    return false;
  }

  return true;
}

// Set up the sparse linear system.
void RobustRotationEstimator::SetupLinearSystem() {
  // The rotation change is one less than the number of global rotations because
  // we keep one rotation constant.
  tangent_space_step_.resize((global_orientations_->size() - nr_fixed_rotations_) * 3);
  tangent_space_residual_.resize(relative_rotations_.size() * 3);
  sparse_matrix_.resize(relative_rotations_.size() * 3,
                        (global_orientations_->size() - nr_fixed_rotations_) * 3);

  // For each relative rotation constraint, add an entry to the sparse
  // matrix. We use the first order approximation of angle axis such that:
  // R_ij = R_j - R_i. This makes the sparse matrix just a bunch of identity
  // matrices.
  int rotation_error_index = 0;
  std::vector<Eigen::Triplet<double> > triplet_list;
  for (const auto& relative_rotation : relative_rotations_) {

    // see if this rotation should be fixed
    if (fixed_view_ids_.find(relative_rotation.first.first) == fixed_view_ids_.end()) {
      const int view1_index =
        FindOrDie(view_id_to_index_, relative_rotation.first.first);

      triplet_list.emplace_back(3 * rotation_error_index,
                                3 * view1_index,
                                -1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 1,
                                3 * view1_index + 1,
                                -1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 2,
                                3 * view1_index + 2,
                                -1.0);

    }
    if (fixed_view_ids_.find(relative_rotation.first.second) == fixed_view_ids_.end())  {
      const int view2_index =
        FindOrDie(view_id_to_index_, relative_rotation.first.second);
      triplet_list.emplace_back(3 * rotation_error_index + 0,
                                3 * view2_index + 0,
                                1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 1,
                                3 * view2_index + 1,
                                1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 2,
                                3 * view2_index + 2,
                                1.0);

    }

    ++rotation_error_index;
  }
  sparse_matrix_.setFromTriplets(triplet_list.begin(), triplet_list.end());
}

bool RobustRotationEstimator::SolveL1Regression() {
  L1Solver<Eigen::SparseMatrix<double> >::Options options;
  options.max_num_iterations = 5;
  L1Solver<Eigen::SparseMatrix<double> > l1_solver(options, sparse_matrix_);

  tangent_space_step_.setZero();
  ComputeResiduals();
  for (int i = 0; i < options_.max_num_l1_iterations; i++) {
    l1_solver.Solve(tangent_space_residual_, &tangent_space_step_);
    UpdateGlobalRotations();
    ComputeResiduals();

    double avg_step_size = ComputeAverageStepSize();

    if (avg_step_size <= options_.l1_step_convergence_threshold) {
      break;
    }
    options.max_num_iterations *= 2;
    l1_solver.SetMaxIterations(options.max_num_iterations);
  }
  return true;
}

bool RobustRotationEstimator::SolveIRLS() {
  const int num_edges = tangent_space_residual_.size() / 3;

  // Set up the linear solver and analyze the sparsity pattern of the
  // system. Since the sparsity pattern will not change with each linear solve
  // this can help speed up the solution time.
  SparseCholeskyLLt linear_solver;
  linear_solver.AnalyzePattern(sparse_matrix_.transpose() * sparse_matrix_);
  if (linear_solver.Info() != Eigen::Success) {
    LOG(ERROR) << "Cholesky decomposition failed.";
    return false;
  }

  VLOG(2) << "Iteration   SqError         Delta";
  const std::string row_format = "  % 4d     % 4.4e     % 4.4e";

  ComputeResiduals();

  Eigen::ArrayXd weights(num_edges * 3);
  Eigen::SparseMatrix<double> at_weight;
  for (int i = 0; i < options_.max_num_irls_iterations; i++) {
    // Compute the Huber-like weights for each error term.
    const double& sigma = options_.irls_loss_parameter_sigma;
    for (int k = 0; k < num_edges; ++k) {
      double e_sq = tangent_space_residual_.segment<3>(3 * k).squaredNorm();
      double tmp = e_sq + sigma * sigma;
      double w = sigma / (tmp * tmp);
      weights.segment<3>(3 * k).setConstant(w);
    }

    // Update the factorization for the weighted values.
    at_weight = sparse_matrix_.transpose() * weights.matrix().asDiagonal();
    linear_solver.Factorize(at_weight * sparse_matrix_);
    if (linear_solver.Info() != Eigen::Success) {
      LOG(ERROR) << "Failed to factorize the least squares system.";
      return false;
    }

    // Solve the least squares problem..
    tangent_space_step_ =
        linear_solver.Solve(at_weight * tangent_space_residual_);
    if (linear_solver.Info() != Eigen::Success) {
      LOG(ERROR) << "Failed to solve the least squares system.";
      return false;
    }

    UpdateGlobalRotations();
    ComputeResiduals();
    const double avg_step_size = ComputeAverageStepSize();

    VLOG(2) << StringPrintf(row_format.c_str(),
                            i,
                            tangent_space_residual_.squaredNorm(),
                            avg_step_size);

    if (avg_step_size < options_.irls_step_convergence_threshold) {
      VLOG(1) << "IRLS Converged in " << i + 1 << " iterations.";
      break;
    }
  }
  return true;
}

// Update the global orientations using the current value in the
// rotation_change.
void RobustRotationEstimator::UpdateGlobalRotations() {
  for (auto& rotation : *global_orientations_) {
    if (fixed_view_ids_.find(rotation.first) != fixed_view_ids_.end()) {
      continue;
    }

    const int view_index = FindOrDie(view_id_to_index_, rotation.first);
    // Apply the rotation change to the global orientation.
    const Eigen::Vector3d& rotation_change =
        tangent_space_step_.segment<3>(3 * view_index);
    rotation.second = MultiplyRotations(rotation.second, rotation_change);
  }
}

// Computes the relative rotation error based on the current global
// orientation estimates.
void RobustRotationEstimator::ComputeResiduals() {
  int rotation_error_index = 0;
  for (const auto& relative_rotation : relative_rotations_) {
    const Eigen::Vector3d& relative_rotation_aa = relative_rotation.second;
    const Eigen::Vector3d& rotation1 =
        FindOrDie(*global_orientations_, relative_rotation.first.first);
    const Eigen::Vector3d& rotation2 =
        FindOrDie(*global_orientations_, relative_rotation.first.second);

    // Compute the relative rotation error as:
    //   R_err = R2^t * R_12 * R1.
    tangent_space_residual_.segment<3>(3 * rotation_error_index) =
        MultiplyRotations(-rotation2,
                          MultiplyRotations(relative_rotation_aa, rotation1));
    ++rotation_error_index;
  }
}

double RobustRotationEstimator::ComputeAverageStepSize() {
  // compute the average step size of the update in tangent_space_step_
  const int numVertices = tangent_space_step_.size() / 3;
  double delta_V = 0;
  for (int k = 0; k < numVertices; ++k) {
    delta_V += tangent_space_step_.segment<3>(3 * k).norm();
  }
  return delta_V / numVertices;
}

bool RobustRotationEstimator::EstimateRotationsWrapper(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    std::unordered_map<ViewId, Eigen::Vector3d>& init_global_orientations) {
    return EstimateRotations(view_pairs, &init_global_orientations);
}

void RobustRotationEstimator::SetFixedGlobalRotations(const std::set<ViewId>& fixed_views) {
    fixed_view_ids_ = fixed_views;
    nr_fixed_rotations_ = fixed_view_ids_.size();

}

}  // namespace theia


/*=============================================================================
    Copyright (c) 2001-2011 Joel de Guzman

    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)
=============================================================================*/
#if !defined(PAWN_RAST_HPP)
#define PAWN_RAST_HPP

#include <boost/config/warning_disable.hpp>
#include <boost/variant/recursive_variant.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>
#include <boost/optional.hpp>
#include <list>
#include <map>

#include "mast.hpp"
#include "sast.hpp"

namespace client { namespace relational { namespace ast
{
    enum class optoken : int
    {
        equal,
        not_equal,
        less,
        less_equal,
        greater,
        greater_equal
    };

    using mathOperand = client::math::ast::expr;
    using strOperand = client::str::ast::expr;

    struct mathOp {
        mathOperand lhs;
        optoken operator_;
        mathOperand rhs;
    };

    struct strOp {
        strOperand lhs;
        optoken operator_;
        strOperand rhs;
    };

    using expr = boost::variant<mathOp, strOp>;

    struct printer {
        typedef void result_type;

        void operator()(optoken const &o) const {
            switch (o)
            {
                case optoken::equal: std::cout << " eq"; break;
                case optoken::not_equal: std::cout << " neq"; break;
                case optoken::less: std::cout << " lt"; break;
                case optoken::less_equal: std::cout << " leq"; break;
                case optoken::greater: std::cout << " gt"; break;
                case optoken::greater_equal: std::cout << " geq"; break;
            }
        }

        void operator()(mathOp const& x) const {
            client::math::ast::printer print;
            print(x.lhs);
            (*this)(x.operator_);
            std::cout << ' ';
            print(x.rhs);
        }

        void operator()(strOp const& x) const {
            client::str::ast::printer print;
            print(x.lhs);
            (*this)(x.operator_);
            std::cout << ' ';
            print(x.rhs);
        }

        void operator()(expr const& x) const {
          boost::apply_visitor(*this, x);
        }
    };

    struct colsEval {
    private:
      using ColIndices = client::helper::ColIndices;
      using Global = client::helper::Global;
      const ColIndices &_v;
      const Global &_global;
      bool _isInitial {true};
      std::vector<std::string> _headers;
    public:
        using result_type = std::pair<ColIndices, std::string>;
        colsEval(const ColIndices &v, const Global &global) : _v{v}, _global{global} {}
        void setHeaders(const std::vector<std::string>& h) { _headers = h; }
        void notInitial() { _isInitial = false; }
        result_type operator()(mathOp const& x) const {
            client::math::ast::colsEval mColsEval(_v, _global);
            mColsEval.setHeaders(_headers);
            if (!_isInitial) mColsEval.notInitial();
            auto res = mColsEval(x.lhs);
            if (res.second.size() > 0) return res;
            auto y = mColsEval(x.rhs);
            if (y.second.size() > 0) return y;
            res.first.add(y.first);
            return res;
        }
        result_type operator()(strOp const& x) const {
            client::str::ast::colsEval mColsEval(_v, _global);
            mColsEval.setHeaders(_headers);
            if (!_isInitial) mColsEval.notInitial();
            auto res = mColsEval(x.lhs);
            if (res.second.size() > 0) return res;
            auto y = mColsEval(x.rhs);
            if (y.second.size() > 0) return y;
            res.first.add(y.first);
            return res;
        }
        result_type operator()(expr const& e) const {
            return boost::apply_visitor(*this, e);
        }

    };

    ///////////////////////////////////////////////////////////////////////////
    //  The AST evaluator
    ///////////////////////////////////////////////////////////////////////////
    struct evaluator {
    private:
        client::math::ast::evaluator _meval;
        client::str::ast::evaluator _seval;
    public:
        using mretFnT = client::math::ast::evaluator::retFnT;
        using sretFnT = client::str::ast::evaluator::retFnT;

        using retFnT = std::function<bool(const std::vector<std::string>&, const std::vector<double>&)>;
        evaluator(helper::positionTeller p, const helper::Global &g) : _meval{p, g}, _seval{p, g} {}
        evaluator(client::math::ast::evaluator m, client::str::ast::evaluator s) :
            _meval{m}, _seval{s} {}
        typedef retFnT result_type;

        retFnT operator()(optoken const &o, mretFnT const &lhs, mretFnT const &rhs) const {
            switch (o)
            {
                case optoken::equal: return [lhs, rhs](const std::vector<std::string> &, const std::vector<double> &v) { return lhs(v) == rhs(v); };
                case optoken::not_equal: return [lhs, rhs](const std::vector<std::string> &, const std::vector<double> &v) { return lhs(v) != rhs(v); };
                case optoken::less: return [lhs, rhs](const std::vector<std::string> &, const std::vector<double> &v) { return lhs(v) < rhs(v); };
                case optoken::less_equal: return [lhs, rhs](const std::vector<std::string> &, const std::vector<double> &v) { return lhs(v) <= rhs(v); };
                case optoken::greater: return [lhs, rhs](const std::vector<std::string> &, const std::vector<double> &v) { return lhs(v) > rhs(v); };
                case optoken::greater_equal: return [lhs, rhs](const std::vector<std::string> &, const std::vector<double> &v) { return lhs(v) >= rhs(v); };
                default: BOOST_ASSERT(0); return result_type{};
            }
            BOOST_ASSERT(0);
            return result_type{};
        }

        retFnT operator()(mathOp const& x) const {
           return (*this)(x.operator_, _meval(x.lhs), _meval(x.rhs));
        }

        retFnT operator()(optoken const &o, sretFnT const &lhs, sretFnT const &rhs) const {
            switch (o) {
                case optoken::equal: return [lhs, rhs](const std::vector<std::string> &v, const std::vector<double> &) { return lhs(v) == rhs(v); };
                case optoken::not_equal: return [lhs, rhs](const std::vector<std::string> &v, const std::vector<double> &) { return lhs(v) != rhs(v); };
                case optoken::less: return [lhs, rhs](const std::vector<std::string> &v, const std::vector<double> &) { return lhs(v) < rhs(v); };
                case optoken::less_equal: return [lhs, rhs](const std::vector<std::string> &v, const std::vector<double> &) { return lhs(v) <= rhs(v); };
                case optoken::greater: return [lhs, rhs](const std::vector<std::string> &v, const std::vector<double> &) { return lhs(v) > rhs(v); };
                case optoken::greater_equal: return [lhs, rhs](const std::vector<std::string> &v, const std::vector<double> &) { return lhs(v) >= rhs(v); };
                default: BOOST_ASSERT(0); return result_type{};
            }
            BOOST_ASSERT(0);
            return result_type{};
        }

        retFnT operator()(strOp const& x) const {
           return (*this)(x.operator_, _seval(x.lhs), _seval(x.rhs));
        }

        retFnT operator()(expr const& x) const {
          return boost::apply_visitor(*this, x);
        }
    };

}}}

BOOST_FUSION_ADAPT_STRUCT(
    client::relational::ast::mathOp,
    (client::math::ast::expr, lhs)
    (client::relational::ast::optoken, operator_)
    (client::math::ast::expr, rhs)
)

BOOST_FUSION_ADAPT_STRUCT(
    client::relational::ast::strOp,
    (client::str::ast::expr, lhs)
    (client::relational::ast::optoken, operator_)
    (client::str::ast::expr, rhs)
)

#endif


#include <gtest/gtest.h>

#include <boost/lexical_cast.hpp>

#include <iostream>
#include <vector>

#include <workserver/util.hpp>
#include <workserver/work_handler.hpp>

TEST (bigint, basic)
{
	nano_pow_server::u128 val128;
	ASSERT_NO_THROW (val128.from_hex (std::string (32, 'F')));
	ASSERT_EQ (val128.to_hex (), std::string (32, 'F'));
	ASSERT_EQ (val128.bytes.size (), 16);
	ASSERT_EQ (val128.number (), std::numeric_limits<boost::multiprecision::uint128_t>::max ());
	val128.set (100);
	ASSERT_EQ (val128.number (), 100);

	nano_pow_server::u256 val256;
	ASSERT_NO_THROW (val256.from_hex (std::string (64, 'F')));
	ASSERT_EQ (val256.to_hex (), std::string (64, 'F'));
	ASSERT_EQ (val256.bytes.size (), 32);
	ASSERT_EQ (val256.number (), std::numeric_limits<boost::multiprecision::uint256_t>::max ());

	nano_pow_server::u512 val512;
	ASSERT_NO_THROW (val512.from_hex (std::string (128, 'F')));
	ASSERT_EQ (val512.to_hex (), std::string (128, 'F'));
	ASSERT_EQ (val512.bytes.size (), 64);
	ASSERT_EQ (val512.number (), std::numeric_limits<boost::multiprecision::uint512_t>::max ());
}

TEST (queue, priority)
{
	nano_pow_server::config config;
	nano_pow_server::work_handler handler (config, nullptr);

	constexpr unsigned count = 10;
	constexpr unsigned count_priority_boosted = 4;
	std::vector<nano_pow_server::job> jobs (count);
	for (unsigned i = 0; i < count; i++)
	{
		jobs[i].request.root_hash = nano_pow_server::u256 (i);

		// Increase priority on the last N items, make sure they're sorted first,
		// yet stable-sorted on job id (i.e. jobs are FIFO within priority)
		jobs[i].set_priority (i >= (count - count_priority_boosted) ? 100 : i);
		handler.push_job (jobs[i]);
	}

	unsigned prev_id = 0;
	for (unsigned i = 0; i < count; i++)
	{
		auto job (*handler.pop_job ());
		if (i < count_priority_boosted)
		{
			ASSERT_EQ (100, job.get_priority ());
			if (i > 0)
			{
				ASSERT_GT (job.get_job_id (), prev_id);
			}
		}
		else
		{
			ASSERT_GT (100, job.get_priority ());
		}
		prev_id = job.get_job_id ();
	}
}

TEST (queue, remove)
{
	nano_pow_server::config config;
	nano_pow_server::work_handler handler (config, nullptr);

	constexpr unsigned count = 10;
	std::vector<nano_pow_server::job> jobs (count);
	for (unsigned i = 0; i < count; i++)
	{
		jobs[i].request.root_hash = nano_pow_server::u256 (i);
		handler.push_job (jobs[i]);
	}

	ASSERT_EQ (handler.get_queue ().size (), 10);
	handler.remove_job (nano_pow_server::u256 (5));
	ASSERT_EQ (handler.get_queue ().size (), 9);
}

TEST (difficulty, low)
{
	double expected_multiplier = 1.0;
	nano_pow_server::u128 base (0x2000000000000000);
	nano_pow_server::u128 diff (0x2000000000000001);
	double multiplier = nano_pow_server::to_multiplier (diff, base);
	ASSERT_NEAR (expected_multiplier, multiplier, 1e-10);
	std::cout << multiplier << std::endl;
}

TEST (difficulty, high)
{
	double expected_multiplier = 7.9999770129;
	nano_pow_server::u128 base (0x2000000000000000);
	nano_pow_server::u128 diff (0xffffcfcaef105e00);
	double multiplier = nano_pow_server::to_multiplier (diff, base);
	ASSERT_NEAR (expected_multiplier, multiplier, 1e-10);
}

TEST (difficulty, max)
{
	nano_pow_server::u128 base (0x2000000000000000);
	nano_pow_server::u128 diff;
	diff.from_hex ("100000000000000000000");
	double multiplier = nano_pow_server::to_multiplier (diff, base);
	auto diff_from_multiplier = nano_pow_server::from_multiplier (multiplier, base).number ();

	// Max multiplier is 2^(80-61)
	ASSERT_EQ (multiplier, 0x80000);
	ASSERT_EQ (diff_from_multiplier, diff.number ());
}


#define BOOST_TEST_MODULE nnls_unit_tests

#include <boost/test/included/unit_test.hpp>
#include <armadillo>
#include "../src/analytical/linear/ls_solver.h"
#include "../src/non-negative/greedy_nn_solver.cpp"
#include "test_utils.cpp"
#include "../src/logging/easylogging++.h"

INITIALIZE_EASYLOGGINGPP

using namespace arma;

mat signals = create_signals();
LSSolver solver = LSSolver(signals);
std::shared_ptr<LSSolver> ls_solver_ptr(new LSSolver(solver));
GreedyNNSolver nn_solver(ls_solver_ptr);

BOOST_AUTO_TEST_CASE(fit_positive_values)
{
    mat weights = {1, 1, 2};
    mat signal = sum_signal(weights, signals);
    mat result = nn_solver.solve(signal);
    BOOST_CHECK(is_equal(weights, result));
}

BOOST_AUTO_TEST_CASE(fit_zero_values)
{
    mat weights = {0, 0, 0};
    mat signal = sum_signal(weights, signals);
    mat result = nn_solver.solve(signal);
    BOOST_CHECK(is_equal(weights, result));
}

BOOST_AUTO_TEST_CASE(fit_positive_and_negative_values)
{
    mat weights = {-1, 1, 2};
    mat signal = sum_signal(weights, signals);
    mat result = nn_solver.solve(signal);
    BOOST_CHECK(result[0] == 0);
    BOOST_CHECK(result[1] > 0);
    BOOST_CHECK(result[2] > 0);

    weights = {1, -1, 2};
    signal = sum_signal(weights, signals);
    result = nn_solver.solve(signal);
    BOOST_CHECK(result[0] > 0);
    BOOST_CHECK(result[1] == 0);
    BOOST_CHECK(result[2] > 0);
}

BOOST_AUTO_TEST_CASE(fit_only_negative_values)
{
    mat weights = {-1, -1, -2};
    mat signal = sum_signal(weights, signals);
    mat result = nn_solver.solve(signal);
    BOOST_CHECK(is_equal(mat{0, 0, 0}, result));
}

BOOST_AUTO_TEST_CASE(solver_should_produce_same_result_with_different_constructor_methods)
{

    mat weights = {1, -1, 2};
    mat signal = sum_signal(weights, signals);

    LSSolver solver = LSSolver(signals);
    std::shared_ptr<LSSolver> ls_solver_ptr(new LSSolver(solver));
    GreedyNNSolver nn_solver1(ls_solver_ptr);

    GreedyNNSolver nn_solver2(signals);

    mat result1 = nn_solver1.solve(signal);
    mat result2 = nn_solver2.solve(signal);

    BOOST_CHECK(is_equal(result1, result2));
}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/constant/maxexponent.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check maxexponent behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxexponent;
  using boost::simd::Maxexponent;

  STF_TYPE_IS(decltype(Maxexponent<T>()), T);
  STF_EQUAL(Maxexponent<T>(), T(0));
  STF_EQUAL(maxexponent( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check maxexponent behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxexponent;
  using boost::simd::Maxexponent;

  STF_TYPE_IS(decltype(Maxexponent<T>()), int64_t);
  STF_IEEE_EQUAL(Maxexponent<T>(), 1023);
  STF_IEEE_EQUAL(maxexponent( as(T{}) ), 1023);
}
STF_CASE_TPL( "Check maxexponent behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::maxexponent;
  using boost::simd::Maxexponent;

  STF_TYPE_IS(decltype(Maxexponent<T>()), int32_t);
  STF_IEEE_EQUAL(Maxexponent<T>(), 127);
  STF_IEEE_EQUAL(maxexponent( as(T{}) ), 127);
}


#include <Eigen/Core>
#include "iris/iris.h"
#include "iris/iris_mosek.h"
#include "test_util.h"

int main() {
  Eigen::MatrixXd A(3,2);
  A << -1, 0,
        0, -1,
        1, 1;
  Eigen::VectorXd b(3);
  b << 0, 0, -1;
  iris::Polyhedron polyhedron(A, b);

  iris::Ellipsoid ellipsoid(2);

  try {
    iris_mosek::inner_ellipsoid(polyhedron, &ellipsoid);
  } catch (iris_mosek::InnerEllipsoidInfeasibleError &e) {
    return 0;
  }
  throw(std::runtime_error("expected an infeasible ellipsoid error"));
  return 1;
}


//
// Created by shiqing on 19-5-14.
//

#include <tuple>
#include <random>
#include <boost/tuple/tuple_io.hpp>
#include <bitset>
#include <regex>
#include <fstream>
#include "../common/CommonHeaders.h"

/*
 * Exercise 17.1: Define a tuple that holds three int values and initialize the members to 10, 20, and 30.
 * @namespace ch17_1;
 *
 * Exercise 17.2: Define a tuple that holds a string, a vector<string>, and a pair<string, int>.
 * @namespace ch17_1;
 *
 *
 * Exercise 17.3: Rewrite the TextQuery programs from § 12.3 (p. 484) to use a tuple instead of the QueryResult class.
 * Explain which design you think is better and why.
 * @namespace ch17_1;
 */
const string filePhone = "../../part_4_advancedtheme/phone";
namespace ch17_1 {

    template<typename Tuple, unsigned N>
    struct TuplePrint {
        static std::ostream &printTuple(std::ostream &os, const Tuple &tuple) {
            TuplePrint<Tuple, N - 1>::printTuple(os, tuple);
            os << std::get<N - 1>(tuple) << " ";
            return os;
        }
    };

    template<typename Tuple>
    struct TuplePrint<Tuple, 1> {
        static std::ostream &printTuple(std::ostream &os, const Tuple &tuple) {
            os << std::get<0>(tuple) << " ";
            return os;
        }
    };

    template<typename...Args>
    std::ostream &printTuple(std::ostream &os, const std::tuple<Args...> &tuple) {
        os << "{";
        TuplePrint<decltype(tuple), sizeof...(Args)>::printTuple(os, tuple);
        os << "}";
        return os;
    }

    void testingTuple() {
        std::tuple<int, int, int> threeI(10, 20, 30);
        printTuple(cout, threeI);
        cout << boost::make_tuple("InsideZhang", 23, "HeNan") << endl;
    }
}

/*
 * Exercise 17.4: Write and test your own version of the findBook function.
 * already done.
 * Exercise 17.5: Rewrite findBook to return a pair that holds an index and a pair of iterators.
 * already done.
 * Exercise 17.6: Rewrite findBook so that it does not use tuple or pair.
 * already done.
 * Exercise 17.7: Explain which version of findBook you prefer and why.
 * already done.
 * Exercise 17.8: What would happen if we passed Sales_data() as the third parameter to accumulate in the last code example in this section?
 * already done.
 *
 */

/*
 * Exercise 17.9: Explain the bit pattern each of the following bitset objects contains:
 * @see testingBitSet();
 */

/*
 * Exercise 17.10: Using the sequence 1, 2, 3, 5, 8, 13, 21, initialize a bitset that has a 1 bit in each position
 * corresponding to a number in this sequence. Default initialize another bitset and write a small program to turn on each of the appropriate bits.
 * @see testingBitSet();
 *
 *
 * Exercise 17.11: Define a data structure that contains an integral object to
 * track responses to a true/false quiz containing 10 questions. What changes, if any,
 * would you need to make in your data structure if the quiz had 100 questions?
 * Answer:unsigned , bisset<100>
 *
 * Exercise 17.12: Using the data structure from the previous question, write a function that takes a question number
 * and a value to indicate a true/false answer and updates the quiz results accordingly.
 * @see validate();
 * Exercise 17.13: Write an integral object that contains the correct answers for the true/false quiz.
 * Use it to generate grades on the quiz for the data structure from the previous two exercises.
 * already done.
 *
 */

namespace ch17_2 {
    void testingBitSet() {
        std::bitset<64> bitvec(32);
        std::bitset<32> bv(1010101);
        cout << bitvec << endl;
        cout << bv << endl;
        cout << bv[0] << endl;
        cout << 0xffffffffU << endl;

        std::bitset<22> ans;
        ans.set(1, 1);
        ans.set(2, 1);
        ans.set(3, 1);
        ans.set(5, 1);
        ans.set(8, 1);
        ans.set(13, 1);
        ans.set(21, 1);

        cout << ans << endl;
    }

    std::bitset<100> vi;

    bool validate(unsigned pos, bool val) {
        vi.set(pos, val);
        return vi[pos];
    }

}

/*
 * Exercise 17.14: Write several regular expressions designed to trigger various errors.
 * Run your program to see what output your compiler generates for each error.
 * @see namespace regx;
 *
 * Exercise 17.15: Write a program using the pattern that finds words that violate the “i before e except after c” rule.
 * Have your program prompt the user to supply a word and indicate whether the word is okay or not.
 * Test your program with words that do and do not violate the rule.
 * @see namespace regx;
 *
 *
 * Exercise 17.16: What would happen if your regex object in the previous program were initialized with "[^c]ei"?
 * Test your program using that pattern to see whether your expectations were correct.
 * @see namespace regx;
 *
 *
 *
 */

/*
 * Exercise 17.17: Update your program so that it finds all the words in an
 * input sequence that violiate the “ei” grammar rule.
 * @see useRegex();
 *
 * Exercise 17.18: Revise your program to ignore words that contain “ei” but are not misspellings, such as “albeit” and “neighbor.”
 * @see useRegex();
 *
 *
 */

namespace regx {
    void useRegex() {
        string pattern = "[[:alpha:]]*" + string("[^c]ei") + "[[:alpha:]]*";
        std::regex reg(pattern);
        string testStr = "receipt freind theif receive";
        std::smatch results;
        if (std::regex_search(testStr, results, reg)) {
            cout << results.str() << endl;
        }
        for (std::sregex_iterator it(testStr.begin(), testStr.cend(), reg), endIt; it != endIt; ++it) {
            cout << it->str() << endl;
        }
    }

    void matchCPlusPlusExtends() {
        try {
            string pattern = "[[:alnum:]]+\\.(cpp|cxx|cc)$";
            std::regex r(pattern, std::regex::icase);
            string fileName = "/home/shiqing/workspace/clion/PrimerAdvanced/part_4_advancedtheme/chapter_17_speciallib.cpp";
            std::smatch result;
            if (std::regex_search(fileName, result, r)) {
                cout << result.str() << endl;
            }
        } catch (std::regex_error e) {
            cout << e.what() << "\ncode: " << e.code() << endl;
        }

    }

    bool valid(const std::smatch &m) {
        if (m[1].matched) {
            return m[3].matched && (m[4].matched == 0 || m[4].str() == " ");
        } else {
            return !m[3].matched && m[4].str() == m[6].str();
        }
    }

    void searchMatchedPone() {
        string phonePattern = "(\\()?(\\d{3})(\\))?([-. ])?(\\d{3})([-. ])?(\\d{4})";
        std::regex r(phonePattern);
        std::smatch m;
        string s;
        std::ifstream ifs(filePhone);
        while (getline(ifs, s) && s != "no") {
            for (std::sregex_iterator it(s.begin(), s.cend(), r), endIt; it != endIt; ++it) {
                if (valid(*it)) {
                    cout << "valid: " << it->str() << endl;
                } else {
                    cout << "not valid: " << it->str() << endl;
                }
            }
        }

    }

}

/*
 * Exercise 17.19: Why is it okay to call m[4].str() without first checking whether m[4] was matched?
 * Answer:
 * We expect that the two delimiters in m[4] and m[6] are the same.
 * If m[4](or m[6]) is mismatched, m[4].str()(or m[6].str() respectively)
 * returns an empty string which can also be compared with the other delimiter.
 *
 * Exercise 17.20: Write your own version of the program to validate phone nubers.
 * @searchMatchedPone();
 *
 * Exercise 17.21: Rewrite your phone number program from § 8.3.2 (p. 323) to use the valid function defined in this section.
 * @searchMatchedPone();
 *
 * Exercise 17.22: Rewrite your phone program so that it allows any number of whitespace characters to separate the three
 * parts of a phone number.
 * @searchMatchedPone();
 *
 *
 * Exercise 17.23: Write a regular expression to find zip codes. A zip code can have five or nine digits.
 * The first five digits can be separated from the remaining four by a dash.
 * @printPostalCode();
 */

/*
 * Exercise 17.24: Write your own version of the program to reformat phone numbers.
 * @namespace regx;
 *
 * Exercise 17.25: Rewrite your phone program so that it writes only the first phone number for each person.
 * @namespace regx;
 *
 *
 * Exercise 17.26: Rewrite your phone program so that it writes only the second and subsequent phone numbers for
 * people with more than one phone number.
 * @namespace regx;
 *
 *
 * Exercise 17.27: Write a program that reformats a nine-digit zip code as ddddd-dddd.
 * @namespace regx;
 *
 *
 */

namespace regx {

    using namespace std::regex_constants;

    bool validCode(const std::smatch &m) {
        if (m[2].matched) {
            return m[1].matched && m[3].matched;
        } else {
            return m[1].matched && (!m[3].matched || m[2].str() == m[3].str());
        }
    }

    void printPostalCode() {
        string postalCodePattern = "(\\d{5})([-])?(\\d{4})?";
        std::regex r(postalCodePattern);
        string s;
        std::ifstream ifs(filePhone);
        while (getline(ifs, s) && s != "no") {
            for (std::sregex_iterator it(s.begin(), s.end(), r), endIt; it != endIt; ++it) {
                if (validCode(*it)) {
                    cout << it->str() << endl;
                } else {
                    cout << it->prefix().str()
                         << "no validate: "
                         << it->str()
                         << it->suffix().str()
                         << endl;
                }
            }
        }
    }

    void writeFormatPhone() {
        std::ifstream ifs(filePhone, std::ifstream::in);
        string pattern = "(\\()?(\\d{3})(\\))?([-. ])?(\\d{3})([-. ])?(\\d{4})";
        std::regex r(pattern);
        string format = "($2) $5-$7";
        std::ofstream ofs(filePhone + ".post");
        string line;
        while (getline(ifs, line)) {
            ofs << std::regex_replace(line, r, format) << "\n";
        }
    }

    void writeFormatPhoneNotNames() {
        std::ifstream ifs(filePhone, std::ifstream::in);
        string pattern = "(\\()?(\\d{3})(\\))?([-. ])?(\\d{3})([-. ])?(\\d{4})";
        std::regex r(pattern);
        string format = "($2) $5-$7";
        std::ofstream ofs(filePhone + ".post.post");
        string line;
        while (getline(ifs, line)) {
            ofs << std::regex_replace(line, r, format, format_no_copy) << "\n";
        }
    }

    void writeOnePhone() {
        std::ifstream ifs(filePhone, std::ifstream::in);
        string pattern = "(\\()?(\\d{3})(\\))?([-. ])?(\\d{3})([-. ])?(\\d{4})";
        std::regex r(pattern);
        string fmt = "($2) $5-$7";
        std::ofstream ofs(filePhone + ".post.post.post");
        string line;
        while (getline(ifs, line)) {
            int cnt = 0;
            for (std::sregex_iterator it(line.begin(), line.end(), r), endIt; it != endIt; ++it) {
                if (++cnt == 1) {
                    ofs << it->prefix().str();
                    ofs << it->format(fmt, format_default) << "\n";
                    break;
                }
            }
        }
    }

    void writeFromSecondPhone() {
        std::ifstream ifs(filePhone, std::ifstream::in);
        string pattern = "(\\()?(\\d{3})(\\))?([-. ])?(\\d{3})([-. ])?(\\d{4})";
        std::regex r(pattern);
        string fmt = "($2) $5-$7";
        std::ofstream ofs(filePhone + ".post.post.post.post");
        string line;
        while (getline(ifs, line)) {
            int cnt = 0;
            for (std::sregex_iterator it(line.begin(), line.end(), r), endIt; it != endIt; ++it) {
                auto tmp = it;
                if (++cnt == 1 && ++tmp == endIt) {
                    ofs << it->format(fmt, format_default) << "\n";
                } else {
                    ofs << it->format(fmt) << "\n";
                }
            }
        }
    }
}

/*
 * Exercise 17.28: Write a function that generates and returns a uniformly distributed random unsigned int each time it is called.
 * @generatePrimitiveNums();
 *
 * Exercise 17.29: Allow the user to supply a seed as an optional argument to the function you wrote in the previous exercise.
 * @generatePrimitiveNums();
 *
 *
 * Exercise 17.30: Revise your function again this time to take a minimum and maximum value for the numbers that the
 * function should return.
 * @generatePrimitiveNums();
 */

/*
 * Exercise 17.31: What would happen if we defined b and e inside the do loop of the game-playing program from this section?
 * @testingPlay():
 * Exercise 17.32: What would happen if we defined resp inside the loop?
 * @testingPlay():
 *
 * Exercise 17.33: Write a version of the word transformation program from §11.3.6 (p. 440) that allows multiple
 * transformations for a given word and randomly selects which transformation to apply.
 * @testingPlay():
 */

namespace stochastic {

    void generatePrimitiveNums() {
        static std::default_random_engine e;
        static std::uniform_int_distribution<unsigned> u(0, 9);
        for (int i = 0; i != 10; ++i) {
            cout << e() << " ";
        }
        cout << endl;
        cout << "e.max() = " << e.max() << ", e.min() = " << e.min() << endl;
        for (int j = 0; j != 10; ++j) {
            cout << u(e) << " ";
        }
        cout << endl;

    }

    void testingNormalDistribution() {
        static std::default_random_engine e;
        static std::normal_distribution<> n(4, 1.5);
        vector<unsigned> vals(9);
        for (size_t i = 0; i != 200; ++i) {
            unsigned v = lround(n(e));
            if (v < vals.size()) {
                ++vals[v];
            }
        }

        for (size_t j = 0; j != vals.size(); ++j) {
            cout << j << ": " << string(vals[j], '*') << endl;
        }

    }

    bool play(bool p) noexcept {
        static std::default_random_engine e;
        static std::bernoulli_distribution b;
        if (p) {
            cout << "my first" << endl;
        } else {
            cout << "you first " << endl;
        }
        return b(e);
    }

    void testingPlay() {
        string resp;
        static std::default_random_engine e;
        static std::bernoulli_distribution b;
        do {
            bool first = b(e);
            cout << (first ? "We go first" : "You get to go first") << endl;
            cout << (play(first) ? "Sorry, you lost" : "congrates, you won") << endl;
            cout << "play again? Enter 'yes' or 'no'" << endl;
        } while (cin >> resp && resp[0] != 'n');

    }
}

namespace cio {

    void testingStdin() {
        int i = 0, j = 0, k = 0;
        float fp1 = .0;
        char word1[20] = "", word2[20] = "";
//        string word1, word2;
//        k = scanf("%f %d %d %[a-z] %*1d %s", &fp1, &i, &j, word1, word2);
        k = scanf("%4f %4d %d %*d %[a-z] %*1d %[^o]", &fp1, &i, &j, word1, word2);
        printf("\nCount of values read = %d\n", k);
        printf("\nfp1 = %f i = %d j = %d", fp1, i, j);
        printf("\nword1 = %s word2 = %s\n", word1, word2);
    }

    void testingStdinAnother() {
        int i = 0, j = 0, k = 0;
        float fp1 = 0.0;
//        inputs:fp1=3.1415926i=78
        k = scanf("fp1=%9fi=%1d%d", &fp1, &i, &j);
        printf("\nCount of values read = %d", k);
        printf("\nfp1 = %f\ti = %d\tj = %d\n", fp1, i, j);
    }

    void testingStdinFloat() {
        float i = .0f, j = .0f, k = .0f;
        int kCnt = 0;
//        kCnt = scanf("%f %f %f", &i, &j, &k);
        kCnt = scanf("%e %g %f", &i, &j, &k);
        printf("\nCounts of vlaue read = %d", kCnt);
        printf("\ni = %f, j = %f, k = %f", i, j, k);
    }

    void testingStdout() {
        int i = 15, j = 345, k = 4567;
        long li = 56789L, lj = 678912L, lk = 23456789L;
        printf("\ni = %d\tj = %d\tk = %d\ti = %6.3d\tj = %6.3d\tk = %6.3d\n", i, j, k, i, j, k);
        printf("\ni = %-d\tj = %+d\tk = %-d\ti = %-6.3d\tj = %-6.3d\tk = %-6.3d\n", i, j, k, i, j, k);
        printf("\nli = %ld\tlj = %ld\tlk = %ld\n", li, lj, lk);
        printf("\nli = %ld\tlj = %ld\tlk = %ld\n", li, lj, lk);

        int m = 678;
//        Display format as heading then display the values
        printf("\n\n%%8d\t\t%%-8d\t\t%%08d\t\t%%-+8d");
        printf("\n%8d\t\t%-8d\t\t%08d\t\t%-+8d", k, k, k, k);

        float fp1 = 345.678f, fp2 = 1.234E6f;
        double fp3 = 234567898.0, fp4 = 11.22334455e-6;
        printf("\n\n%f    %+f    %-10.4f    %6.4f\n", fp1, fp2, fp3, fp4);
        printf("\n%e    %+E\n", fp1, fp2);
        printf("\n%f    %g    %#+f    %10.4g\n", fp3, fp3, fp3, fp4);

        char str[] = "Jabberwocky!\n";
        puts(str);
        char *cp = str;
        while (*cp) {
            putchar(*cp++);
        }

    }

    const char *fileSrcText = "../../part_2_stdlib/tiny_text.txt";
    const char *myFile = "../../part_4_advancedtheme/myfile.txt";
    void testingIOFile() {
//        open a file that existed or not
//        Open file myFile.txt to write it
        fopen(myFile, "w");
        const char *tmp = "../../part_4_advancedtheme/myfile_tmp.txt";
        const char *myCopy = "../../part_4_advancedtheme/myfile_copy.txt";
        fopen(tmp, "w");

//        rename file
        int flag = rename(tmp, myCopy);
        if (flag == 0) {
            printf("File renamed successfully.");
        }

//        first create a file
//        second remove it
        fopen(tmp, "w");
        remove(tmp);

        printf("\nEnter an interesting string.\n");
        FILE *filePtr = fopen(fileSrcText, "r");
        if (filePtr == NULL) {
            printf("Error opening %s for writig. program terminated", fileSrcText);
            abort();
        }

        int ch = 0;
        while ((ch = fgetc(filePtr)) != EOF) {
            putchar(ch);
        }
        fclose(filePtr);

        filePtr = fopen(myFile, "a");
        if (filePtr) {
            FILE *pf = fopen(fileSrcText, "r");
            if (pf) {
                while ((ch = fgetc(pf)) != EOF) {
                    fputc(ch, filePtr);
                }
                fclose(pf);
            }
            fclose(filePtr);
        }

    }

}

int main() {
    regx::writeFromSecondPhone();
    regx::writeOnePhone();
    regx::writeFormatPhoneNotNames();
    regx::writeFormatPhone();
    regx::printPostalCode();
    regx::searchMatchedPone();
    regx::matchCPlusPlusExtends();
    regx::useRegex();
    ch17_1::testingTuple();
    ch17_2::testingBitSet();
    for (int i = 0; i != 10; ++i) {
        stochastic::generatePrimitiveNums();
    }
    stochastic::testingNormalDistribution();
//    stochastic::testingPlay();
//    cio::testingStdin();
//    cio::testingStdinAnother();
    cio::testingStdout();
    cio::testingIOFile();
    return 0;
}


#include <iostream>
#include <fstream>
#include <sstream>

#include <Eigen/Sparse>
#include <Eigen/SPQRSupport>

int main() {
  Eigen::SparseMatrix<double> sm1(3, 3);
  /*
  sm1.reserve(3000000);

  {
    std::vector<Eigen::Triplet<double>> tripletList;
    tripletList.reserve(3000000);

    std::ifstream ifile ("output.txt");

    std::string substr;
    int i, j;
    double v;

    while (ifile) {
      std::getline(ifile, substr, ',');
      if (!ifile) break;
      i = std::stoi(substr);

      std::getline(ifile, substr, ',');
      j = std::stoi(substr);

      std::getline(ifile, substr);
      v = std::stod(substr);

      tripletList.push_back(Eigen::Triplet<double>(i, j, v));
    }

    sm1.setFromTriplets(tripletList.begin(), tripletList.end());
  }
  */

  std::vector<Eigen::Triplet<double>> tripletList = {
    {0, 0, 5},
    {1, 1, 3},
  };

  sm1.setFromTriplets(tripletList.begin(), tripletList.end());

  Eigen::SPQR<Eigen::SparseMatrix<double>> spqr(sm1);

  std::cout << "info : " << spqr.info() << std::endl;
  std::cout << "rank : " << spqr.rank() << std::endl;

  return 0;
}


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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>
#include <boost/bind/bind.hpp>
using namespace boost::placeholders;

#include "tudat/astro/relativity/relativisticLightTimeCorrection.h"
#include "tudat/astro/ephemerides/constantEphemeris.h"
#include "tudat/astro/observation_models/corrections/firstOrderRelativisticCorrection.h"

namespace tudat
{

namespace unit_tests
{

using namespace tudat::observation_models;
using namespace tudat::relativity;
using namespace tudat::ephemerides;

BOOST_AUTO_TEST_SUITE( test_shapiro_delay )

BOOST_AUTO_TEST_CASE( testShapiroDelay )
{
    Eigen::Vector6d groundStationState;
    groundStationState << 0.0, 0.0, 6378.0, 0.0, 0.0, 0.0;
    Eigen::Vector6d satelliteState;
    satelliteState  <<  0.0, 0.0, 26600.0, 0.0, 0.0, 0.0;
    Eigen::Vector6d centralBodyPosition = Eigen::Vector6d::Zero( );

    std::shared_ptr< ConstantEphemeris > ephemeris = std::make_shared< ConstantEphemeris >(
                [ & ]( ){ return centralBodyPosition; } );

    double earthGravitationalParameter = 398600.44189E9;

    double directCalculation = calculateFirstOrderLightTimeCorrectionFromCentralBody(
                earthGravitationalParameter, groundStationState.segment( 0, 3 ),
                satelliteState.segment( 0, 3 ), centralBodyPosition.segment( 0, 3 ) );

    std::vector< std::function< Eigen::Vector6d( const double ) > > perturbingBodyStateFunctions;
    std::vector< std::function< double( ) > > perturbingBodyGravitationalParameterFunctions;

    perturbingBodyStateFunctions.push_back( std::bind( &Ephemeris::getCartesianState, ephemeris, std::placeholders::_1 ) );
    perturbingBodyGravitationalParameterFunctions.push_back( [ & ]( ){ return earthGravitationalParameter; } );

    FirstOrderLightTimeCorrectionCalculator correctionCalculator(
                perturbingBodyStateFunctions, perturbingBodyGravitationalParameterFunctions,
                std::vector< std::string >{ "Earth" }, "Satellite", "Earth" );

    double classInterfaceCalculation = correctionCalculator.calculateLightTimeCorrection(
                groundStationState, satelliteState, 0.0, 0.0 );

    // Living reviews in relativity, GPS.
    double expectedResult = 6.3E-3;

    BOOST_CHECK_CLOSE_FRACTION( 0.5 * classInterfaceCalculation * physical_constants::SPEED_OF_LIGHT, expectedResult, 6.0E-2 );
    BOOST_CHECK_CLOSE_FRACTION( 0.5 * directCalculation * physical_constants::SPEED_OF_LIGHT, expectedResult, 6.0E-2 );
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_SUBS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_SUBS_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/subs.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/is_less_equal.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/adds.hpp>
#include <boost/simd/include/functions/simd/any.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/one.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::simd::sub(a0, a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<uint_<A0>,X>))((simd_<uint_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 a0ma1 = a0-a1;
      return if_else_zero(le(a0ma1, a0), a0ma1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::subs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<int_<A0>,X>))((simd_<int_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 res =  adds(a0, -a1);
      if (boost::simd::any(eq(a1, Valmin<A0>())))
        return if_else(eq(a1, Valmin<A0>()), adds(adds(a0, Valmax<A0>()),One<A0>()), res);
      else
        return res;
    }
  };
} } }

#endif


#include "Fraction.h"
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cmath>

nori::Fraction::Fraction(){
    Fraction(1,1);
}
nori::Fraction::Fraction(
        boost::multiprecision::cpp_dec_float_50 numerator,
        boost::multiprecision::cpp_dec_float_50 denominator)
{
    this->numeratorRaw = numerator;
    this->denominatorRaw = denominator;

    numerator = 1;
    denominator = 1;
}

//ユークリッドの互除法
int nori::Fraction::euclidean(
        int m,
        int n)
{
    if(n==0){
        return m;
    }else{
        return euclidean(n, m % n);
    }
}
//約分関数
void nori::Fraction::reduce(){
    int eucResult=euclidean(numerator, denominator);

    setNumerator(numerator / eucResult);
    setDenominator(denominator / eucResult);
}

// ----

int nori::Fraction::getDenominator(){
    return nori::Fraction::denominator;
}
int nori::Fraction::getNumerator(){
    return nori::Fraction::numerator;
}

void nori::Fraction::setDenominator(boost::multiprecision::cpp_dec_float_50 denominatorRaw){
    this->denominatorRaw =  denominatorRaw;
    this->denominator = (int) this->denominatorRaw;
}
void nori::Fraction::setNumerator(boost::multiprecision::cpp_dec_float_50 numeratorRaw){
    this->numeratorRaw = numeratorRaw;
    this->numerator = (int) this->numeratorRaw;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      101111    E. Iorfida        File created.
 *      101111    E. Iorfida        Implementation of all the equations up to the Newton method.
 *      101117    E. Iorfida        Velocities computations added.
 *      101126    E. Iorfida        Get/set codes deleted.
 *      101206    E. Iorfida        LambertTargetingElements class deleted,
 *                                  added setInitialState, modified punctuation. Set single
 *                                  variables, change variables names in more understandable ones.
 *      101209    E. Iorfida        Corrected some coding errors.
 *      101213    E. Iorfida        Deleted lambertAngle, added numberOfRevolution, modified
 *                                  implementation.
 *      101214    E. Iorfida        Implementation only for the case with numberOfRevolution = 0.
 *      110113    E. Iorfida        Added necessary elements to build pointer-to-member-function
 *                                  to RootFinderAlgorithms and NewtonRaphsonMethod classes.
 *      110124    E. Iorfida        Added necessary piece of code to be able to use the last
 *                                  version of Newton-Raphson code.
 *      110126    E. Iorfida        Initialized member functions.
 *      110130    J. Melman         Simplified variable names, e.g., 'normOfdeletVelocityVector'
 *                                  became 'speed'. Requested references to specific formulas. Also
 *                                  corrected 'tangential' to 'transverse'. Simplified computation
 *                                  of radial unit vector. Corrected computation of transverse
 *                                  heliocentric velocity.
 *      110201    E. Iorfida        Added pointerToCelestialBody and modified variable names (from
 *                                  heliocentric, to inertial). Added patch for negative case of
 *                                  initialLambertGuess_. Added equations references.
 *      110206    E. Iorfida        Added unique function for Newton-Raphson method. Added
 *                                  computeAbsoluteValue to the initialLambertGuess_ for
 *                                  non-converging cases.
 *      110208    E. Iorfida        Added CartesianPositionElements objects as input and
 *                                  CartesianVelocityElements objects as output.
 *      110418    E. Iorfida        Added a new normal plane that take into account the case of two
 *                                  parallel position vector (with a relative angle of 180
 *                                  degrees). Better defined the pointers to the output
 *                                  CartesianVelocityElements.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120620    T. Secretin       Adapted and moved code from LambertTargeter.cpp.
 *      120813    P. Musegaas       Changed code to new root finding structure.
 *      140117    E. Brandon        Changed constructor input argument naming to be consistent with
 *                                  other Lambert classes.
 *
 *    References
 *
 *    Notes
 *
 */

#include <boost/make_shared.hpp>

#include <Eigen/Geometry>

#include "Tudat/Astrodynamics/MissionSegments/lambertRoutines.h"
#include "Tudat/Astrodynamics/MissionSegments/lambertTargeterGooding.h"

//! Tudat library namespace.
namespace tudat
{
namespace mission_segments
{

using namespace root_finders;

//! Constructor with immediate definition of parameters and execution of the algorithm.
LambertTargeterGooding::LambertTargeterGooding(
        const Eigen::Vector3d& aCartesianPositionAtDeparture,
        const Eigen::Vector3d& aCartesianPositionAtArrival,
        const double aTimeOfFlight,
        const double aGravitationalParameter,
        RootFinderPointer aRootFinder )
    : LambertTargeter( aCartesianPositionAtDeparture, aCartesianPositionAtArrival,
                       aTimeOfFlight, aGravitationalParameter ),
      rootFinder( aRootFinder )
{
    // Required because the make_shared in the function definition gives problems for MSVC.
    if ( !rootFinder.get( ) )
    {
        rootFinder = boost::make_shared< NewtonRaphson >( 1.0e-12, 1000 );
    }

    // Execute algorithm.
    execute( );
}

//! Execute Lambert targeting solver.
void LambertTargeterGooding::execute( )
{
    // Call Gooding's Lambert targeting routine.
    solveLambertProblemGooding( cartesianPositionAtDeparture, cartesianPositionAtArrival,
                                timeOfFlight, gravitationalParameter,
                                cartesianVelocityAtDeparture, cartesianVelocityAtArrival,
                                rootFinder );
}

//! Get radial velocity at departure.
double LambertTargeterGooding::getRadialVelocityAtDeparture( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );

    // Compute radial velocity at departure.
    return cartesianVelocityAtDeparture.dot( radialUnitVectorAtDeparture );
}

//! Get radial velocity at arrival.
double LambertTargeterGooding::getRadialVelocityAtArrival( )
{
    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute radial velocity at arrival.
    return cartesianVelocityAtArrival.dot( radialUnitVectorAtArrival );
}

//! Get transverse velocity at departure.
double LambertTargeterGooding::getTransverseVelocityAtDeparture( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtDeparture.cross( cartesianVelocityAtDeparture );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture
            = cartesianPositionAtDeparture.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtDeparture =
                angularMomentumUnitVector.cross( radialUnitVectorAtDeparture );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtDeparture.dot( tangentialUnitVectorAtDeparture );
}

//! Get transverse velocity at arrival.
double LambertTargeterGooding::getTransverseVelocityAtArrival( )
{
    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtArrival.cross( cartesianVelocityAtArrival );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtArrival =
                angularMomentumUnitVector.cross( radialUnitVectorAtArrival );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtArrival.dot( tangentialUnitVectorAtArrival );
}

//! Get semi-major axis.
double LambertTargeterGooding::getSemiMajorAxis( )
{
    // Compute specific orbital energy: eps = v^2/ - mu/r.
    const double specificOrbitalEnergy = cartesianVelocityAtDeparture.squaredNorm( ) / 2.0
            - gravitationalParameter / cartesianPositionAtDeparture.norm( );

    // Compute semi-major axis: a = -mu / 2*eps.
    return -gravitationalParameter / ( 2.0 * specificOrbitalEnergy );
}

} // namespace mission_segments
} // namespace tudat


// nonfinite_legacy.cpp

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

// Copyright (c) 2006 Johan Rade
// Copyright (c) 2011 Paul A. Bristow

/*!
\file
\brief Basic tests of nonfinite loopback with output and input facet.

\detail Basic loopback test outputs using the so-called 'legacy' facets,
"1.#INF"  and "1.#QNAN".

and reads back in using nonfinite input 'legacy' facet, and
(if possible) checks if loopback OK.

*/

#include <boost/math/special_functions/nonfinite_num_facets.hpp>
using boost::math::nonfinite_num_put;
using boost::math::nonfinite_num_get;

using boost::math::legacy;

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

#include <iomanip>
using std::setfill;
using std::setw;

#include <locale>
using std::locale;

#include <sstream>
using std::stringstream;
#include <limits>
using std::numeric_limits;

#include <assert.h>

int main()
{
  // Create a new locale with both the nonfinite facets.
  std::locale new_locale(std::locale(std::locale(),
    new boost::math::nonfinite_num_put<char>),
    new boost::math::nonfinite_num_get<char>);

  {
    stringstream ss;
    ss.imbue(new_locale);
    double inf = numeric_limits<double>::infinity();
    ss << inf; // Write out.
    double r;
    ss >> r; // Read back in.

    cout << "infinity output was " << inf << endl;
    cout << "infinity input was " << r << endl;

    assert(inf == r);
  }
  {
    stringstream ss;
    ss.imbue(new_locale);

    double nan = numeric_limits<double>::quiet_NaN();
    ss << nan; // Write out.
    double v;
    ss >> v; // Read back in.

    cout << "NaN output was " << nan << endl;
    cout << "NaN input was " << v << endl;

    // assert(nan == v); // Always fails because NaN == NaN fails!
    // assert(nan == numeric_limits<double>::quiet_NaN()); asserts!
  }

} // int main()

/*

Output:

infinity output was 1.#INF
infinity input was 1.#INF
NaN output was 1.#QNAN
NaN input was 1.#QNAN

*/


// Copyright 2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/assert.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/accumulators.hpp>
#include <boost/histogram/serialization.hpp>
#include <boost/histogram/weight.hpp>
#include "throw_exception.hpp"
#include "utility_serialization.hpp"

using namespace boost::histogram;

int main(int argc, char** argv) {
  BOOST_ASSERT(argc == 2);

  // mean v0
  {
    const auto filename = join(argv[1], "accumulators_serialization_test_mean_v0.xml");
    accumulators::mean<> a;
    load_xml(filename, a);
    BOOST_TEST_EQ(a.count(), 3);
    BOOST_TEST_EQ(a.value(), 2);
    BOOST_TEST_EQ(a.variance(), 0.5);
  }

  // mean
  {
    const auto filename = join(argv[1], "accumulators_serialization_test_mean.xml");
    accumulators::mean<> a;
    a(1);
    a(weight(0.5), 2);
    a(3);
    print_xml(filename, a);

    accumulators::mean<> b;
    BOOST_TEST_NOT(a == b);
    load_xml(filename, b);
    BOOST_TEST(a == b);
  }

  // sum
  {
    const auto filename = join(argv[1], "accumulators_serialization_test_sum.xml");
    accumulators::sum<> a;
    a += 1e100;
    a += 1;
    print_xml(filename, a);

    accumulators::sum<> b;
    BOOST_TEST_NOT(a == b);
    load_xml(filename, b);
    BOOST_TEST(a == b);
  }

  // weighted_mean
  {
    const auto filename =
        join(argv[1], "accumulators_serialization_test_weighted_mean.xml");
    accumulators::weighted_mean<> a;
    a(1);
    a(weight(0.5), 2);
    a(3);
    print_xml(filename, a);

    accumulators::weighted_mean<> b;
    BOOST_TEST_NOT(a == b);
    load_xml(filename, b);
    BOOST_TEST(a == b);
  }

  // weighted_sum
  {
    const auto filename =
        join(argv[1], "accumulators_serialization_test_weighted_sum.xml");
    accumulators::weighted_sum<> a;
    a += 1;
    a += 10;
    print_xml(filename, a);

    accumulators::weighted_sum<> b;
    BOOST_TEST_NOT(a == b);
    load_xml(filename, b);
    BOOST_TEST(a == b);
  }

  return boost::report_errors();
}


//
// Created by cerussite on 19/06/15.
//

#include <boost/algorithm/string.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
#include <sstream>
#include <unordered_map>

#include "dependencies.hpp"
#include "types/rule.hpp"

namespace make {
    DependGraph createDependGraph(const std::vector<Rule> &rules,
                                  const std::vector<std::string> &files) {
        Graph graph;
        boost::property_map<Graph, boost::vertex_name_t>::type nameMap =
            boost::get(boost::vertex_name, graph);

        std::unordered_map<std::string, std::size_t> vertex;
        std::vector<std::string> vertexName;
        auto add_vertex = [&graph, &nameMap](const std::string &target) {
            auto vert = boost::add_vertex(graph);
            boost::put(nameMap, vert, target);
        };
        auto assignIfNotContains = [&vertex, &add_vertex,
                                    &vertexName](const std::string &test) {
            static std::size_t index = 0;
            if (vertex.find(test) == std::end(vertex)) {
                vertex[test] = index++;
                add_vertex(test);
                vertexName.emplace_back(test);
            }
        };

        for (const auto &rule : rules) {
            for (const auto &target : rule.targets()) {
                assignIfNotContains(target);
                for (const auto &dependency : rule.dependencies()) {

                    assignIfNotContains(dependency);

                    boost::add_edge(vertex[target], vertex[dependency], graph);
                }
            }
        }

        return {graph, vertexName};
    }

    void printGraph(std::ostream &os, const DependGraph &dg) {
        const auto &[graph, vname] = dg;
        boost::print_graph(graph, vname.data(), os);
    }
} // namespace make