// Deal.ii
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/smartpointer.h>
#include <deal.II/base/convergence_table.h>
#include <deal.II/base/numbers.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/dofs/dof_renumbering.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

// STL
#include <array>
#include <fstream>
#include <iostream>
#include <cmath>

namespace Helmholtz
{
  using namespace dealii;

  ///////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////

  template <int dim>
  class Solution : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;

    virtual Tensor<1, dim>
    gradient(const Point<dim> & p,
             const unsigned int component = 0) const override;
  };

  template <int dim>
  double Solution<dim>::value(const Point<dim> &p, const unsigned int) const
  {
    double return_value = 1;

    for (unsigned int i = 0; i<dim; ++i)
        	return_value *= cos(2*numbers::PI*p(i));

    return return_value;
  }

  template <>
  Tensor<1, 1> Solution<1>::gradient(const Point<1> &p,
                                         const unsigned int) const
  {
    Tensor<1, 1> return_value;

	return_value[0] = - 2 * numbers::PI * sin(2*numbers::PI*p(0));

    return return_value;
  }

  template <>
  Tensor<1, 2> Solution<2>::gradient(const Point<2> &p,
                                         const unsigned int) const
  {
    Tensor<1, 2> return_value;

	return_value[0] = - 2 * numbers::PI * sin(2*numbers::PI*p(0)) * cos(2*numbers::PI*p(1));
	return_value[1] = - 2 * numbers::PI * cos(2*numbers::PI*p(0)) * sin(2*numbers::PI*p(1));

    return return_value;
  }

  template <>
    Tensor<1, 3> Solution<3>::gradient(const Point<3> &p,
                                           const unsigned int) const
    {
      Tensor<1, 3> return_value;

      return_value[0] = - 2 * numbers::PI * sin(2*numbers::PI*p(0)) * cos(2*numbers::PI*p(1)) * cos(2*numbers::PI*p(2));
      return_value[1] = - 2 * numbers::PI * cos(2*numbers::PI*p(0)) * sin(2*numbers::PI*p(1)) * cos(2*numbers::PI*p(2));
      return_value[2] = - 2 * numbers::PI * cos(2*numbers::PI*p(0)) * cos(2*numbers::PI*p(1)) * sin(2*numbers::PI*p(2));

      return return_value;
    }

  ///////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };

  template <int dim>
  double RightHandSide<dim>::value(const Point<dim> &p,
                                   const unsigned int) const
  {
    double return_value = (1 + 4 * dim * std::pow(numbers::PI,2));

    for (unsigned int i = 0; i<dim; ++i)
    	return_value *= cos(2*numbers::PI*p(i));

    return return_value;
  }

  ///////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////

  template <int dim>
  class HelmholtzSolver
  {
  public:
    HelmholtzSolver(const FiniteElement<dim> &fe);

    ~HelmholtzSolver();

    void run();

  private:
    void setup_system();
    void assemble_system();
    void solve();
    void process_solution(const unsigned int cycle);
    void output_vtk(const unsigned int cycle);

    Triangulation<dim> triangulation;
    DoFHandler<dim>    dof_handler;

    SmartPointer<const FiniteElement<dim>> fe;

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> system_rhs;

    ConvergenceTable convergence_table;
  };

  template <int dim>
  HelmholtzSolver<dim>::HelmholtzSolver(const FiniteElement<dim> &fe)
    : dof_handler(triangulation)
    , fe(&fe)
  {}

  template <int dim>
  HelmholtzSolver<dim>::~HelmholtzSolver()
  {
    dof_handler.clear();
  }

  template <int dim>
  void HelmholtzSolver<dim>::setup_system()
  {
    dof_handler.distribute_dofs(*fe);
    DoFRenumbering::Cuthill_McKee(dof_handler);

    constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler,
                                            constraints);

    Solution<dim>	exact_solution;

    VectorTools::interpolate_boundary_values(dof_handler,
                                               0,
                                               exact_solution,
                                               constraints);

    constraints.close();

    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);
    constraints.condense(dsp);
    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);

    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }

  template <int dim>
  void HelmholtzSolver<dim>::assemble_system()
  {
    QGauss<dim>     quadrature_formula(fe->degree + 1);

    const unsigned int n_q_points      = quadrature_formula.size();

    const unsigned int dofs_per_cell = fe->dofs_per_cell;

    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     cell_rhs(dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    FEValues<dim> fe_values(*fe,
                            quadrature_formula,
                            update_values | update_gradients |
                              update_quadrature_points | update_JxW_values);

    RightHandSide<dim>  right_hand_side;
    std::vector<double> rhs_values(n_q_points);

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix = 0.;
        cell_rhs    = 0.;

        fe_values.reinit(cell);

        right_hand_side.value_list(fe_values.get_quadrature_points(),
                                   rhs_values);

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            {
              for (unsigned int j = 0; j < dofs_per_cell; ++j)
                cell_matrix(i, j) +=
                  ((fe_values.shape_grad(i, q_point) *     // grad phi_i(x_q)
                      fe_values.shape_grad(j, q_point)     // grad phi_j(x_q)
                    +                                      //
                    fe_values.shape_value(i, q_point) *    // phi_i(x_q)
                      fe_values.shape_value(j, q_point)) * // phi_j(x_q)
                   fe_values.JxW(q_point));                // dx

              cell_rhs(i) += (fe_values.shape_value(i, q_point) * // phi_i(x_q)
                              rhs_values[q_point] *               // f(x_q)
                              fe_values.JxW(q_point));            // dx
            }

        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(
                cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
      }
  }

  template <int dim>
  void HelmholtzSolver<dim>::solve()
  {
    SolverControl solver_control(1000, 1e-12);
    SolverCG<>    cg(solver_control);

    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    cg.solve(system_matrix, solution, system_rhs, preconditioner);

    std::cout << "   " << solver_control.last_step()
                << " CG iterations needed to obtain convergence." << std::endl;

    constraints.distribute(solution);
  }

  template <int dim>
  void HelmholtzSolver<dim>::process_solution(const unsigned int cycle)
  {
	Solution<dim> exact_solution;

    Vector<float> difference_per_cell(triangulation.n_active_cells());
    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      exact_solution,
                                      difference_per_cell,
                                      QGauss<dim>(fe->degree + 1),
                                      VectorTools::L2_norm);
    const double L2_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::L2_norm);

    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      Solution<dim>(),
                                      difference_per_cell,
                                      QGauss<dim>(fe->degree + 1),
                                      VectorTools::H1_seminorm);
    const double H1_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::H1_seminorm);

    const unsigned int n_active_cells = triangulation.n_active_cells();
    const unsigned int n_dofs         = dof_handler.n_dofs();

    std::cout << "Cycle " << cycle << ':' << std::endl
              << "   Number of active cells:       " << n_active_cells
              << std::endl
              << "   Number of degrees of freedom: " << n_dofs << std::endl;

    convergence_table.add_value("cycle", cycle);
    convergence_table.add_value("cells", n_active_cells);
    convergence_table.add_value("dofs", n_dofs);
    convergence_table.add_value("L2", L2_error);
    convergence_table.add_value("H1", H1_error);
  }

  template <int dim>
  void HelmholtzSolver<dim>::output_vtk(const unsigned int cycle)
  {
	std::string vtk_filename
		= "solution-" + Utilities::int_to_string(dim,1) + "D";

	switch (fe->degree)
	  {
		case 1:
		  vtk_filename += "_q1";
		  break;
		case 2:
		  vtk_filename += "_q2";
		  break;
		default:
		  Assert(false, ExcNotImplemented());
	  }

	vtk_filename += "_cycle-" + Utilities::int_to_string(cycle,1);
	vtk_filename += ".vtk";

	std::ofstream output(vtk_filename);

	DataOut<dim> data_out;
	data_out.attach_dof_handler(dof_handler);
	data_out.add_data_vector(solution, "solution");

	data_out.build_patches(fe->degree);
	data_out.write_vtk(output);
}

  template <int dim>
  void HelmholtzSolver<dim>::run()
  {

    const unsigned int n_cycles = 6;

    for (unsigned int cycle = 0; cycle < n_cycles; ++cycle)
      {
        if (cycle == 0)
          {
            GridGenerator::hyper_cube(triangulation, 0, 1);
            triangulation.refine_global(1);
          }
        else
        	triangulation.refine_global();

        setup_system();

        assemble_system();
        solve();

        process_solution(cycle);

        output_vtk(cycle);
      }

    convergence_table.set_precision("L2", 3);
    convergence_table.set_precision("H1", 3);

    convergence_table.set_scientific("L2", true);
    convergence_table.set_scientific("H1", true);

    convergence_table.set_tex_caption("cells", "\\# cells");
    convergence_table.set_tex_caption("dofs", "\\# dofs");
    convergence_table.set_tex_caption("L2", "$L^2$-error");
    convergence_table.set_tex_caption("H1", "$H^1$-error");

    convergence_table.set_tex_format("cells", "r");
    convergence_table.set_tex_format("dofs", "r");

    std::cout << std::endl;
    convergence_table.write_text(std::cout);

    // Write table into a LaTeX file.
    std::string error_filename
		= "error-" + Utilities::int_to_string(dim,1) + "D";

    switch (fe->degree)
      {
        case 1:
          error_filename += "_q1";
          break;
        case 2:
          error_filename += "_q2";
          break;
        default:
          Assert(false, ExcNotImplemented());
      }

    error_filename += ".tex";
    std::ofstream error_table_file(error_filename);

    convergence_table.write_tex(error_table_file);

    /////////////////

	convergence_table.add_column_to_supercolumn("cycle", "n cells");
	convergence_table.add_column_to_supercolumn("cells", "n cells");

	std::vector<std::string> new_order;
	new_order.emplace_back("n cells");
	new_order.emplace_back("H1");
	new_order.emplace_back("L2");
	convergence_table.set_column_order(new_order);

	convergence_table.evaluate_convergence_rates(
	  "L2", ConvergenceTable::reduction_rate);
	convergence_table.evaluate_convergence_rates(
	  "L2", ConvergenceTable::reduction_rate_log2);
	convergence_table.evaluate_convergence_rates(
	  "H1", ConvergenceTable::reduction_rate);
	convergence_table.evaluate_convergence_rates(
	  "H1", ConvergenceTable::reduction_rate_log2);

	std::cout << std::endl;
	convergence_table.write_text(std::cout);

	std::string conv_filename
		= "convergence-" + Utilities::int_to_string(dim,1) + "D";

	switch (fe->degree)
	  {
		case 1:
		  conv_filename += "_q1";
		  break;
		case 2:
		  conv_filename += "_q2";
		  break;
		default:
		  Assert(false, ExcNotImplemented());
	  }
	conv_filename += ".tex";

	std::ofstream table_file(conv_filename);
	convergence_table.write_tex(table_file);
  }

} // namespace Helmholtz

  ///////////////////////////////////////////
  ///////////////////////////////////////////
  ///////////////////////////////////////////

int main()
{
  const unsigned int dim = 1;

  try
    {
      using namespace dealii;

      {
        std::cout << "Solving with Q1 elements in " << dim << "D" << std::endl
                  << std::endl
                  << "=============================="
                  << std::endl
                  << std::endl;

        FE_Q<dim>             fe(1);
        Helmholtz::HelmholtzSolver<dim> helmholtz_problem_2d(fe);

        helmholtz_problem_2d.run();

        std::cout << std::endl;
      }

      {
        std::cout << "Solving with Q2 elements in " << dim << "D" << std::endl
                  << "==============================" << std::endl
                  << std::endl;

        FE_Q<dim>             fe(2);
        Helmholtz::HelmholtzSolver<dim> helmholtz_problem_2d(fe);

        helmholtz_problem_2d.run();

        std::cout << std::endl;
      }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#ifndef _EIGEN_SPACE_
#define _EIGEN_SPACE_ 1

#include <string>
#include <vector>
#include <Eigen/Dense>
#include <cmath>
#include <exception>
#include <algorithm>

#include "naive_bayes.cpp"

#define _l_ std::cout<<__LINE__<<std::endl;

typedef struct {
  Eigen::VectorXd longMean;
  Eigen::MatrixXd transformationMatrix;
  ClassInfo ci;
} ReducedDimClassInfo;

bool
EigenvectorCompare(
    const std::pair<double, Eigen::VectorXd>& p,
    const std::pair<double, Eigen::VectorXd>& q) {

  return p.first < q.first;
}

Eigen::MatrixXd
GetSortedEigenvectors(const Eigen::MatrixXd& matrix) {
  Eigen::EigenSolver<Eigen::MatrixXd> eigenSolver;
  eigenSolver.compute(matrix, true);

  std::vector<std::pair<double, Eigen::VectorXd>> sortedEigenvectors;
  for (unsigned i = 0; i < eigenSolver.eigenvalues().size(); ++i) {
    if (eigenSolver.eigenvalues().imag()(i) == 0) {
      sortedEigenvectors.push_back(
        {
          eigenSolver.eigenvalues().real()(i),
          eigenSolver.eigenvectors().col(i).real()
        }
      );
    }
  }
  std::sort(sortedEigenvectors.begin(), sortedEigenvectors.end(), EigenvectorCompare);

  Eigen::MatrixXd rowMatrixOfSortedEigenvectors;
  rowMatrixOfSortedEigenvectors.resizeLike(matrix);

  for (unsigned i = 0; i < matrix.cols(); ++i) {
	if (sortedEigenvectors[i].second.rows() == rowMatrixOfSortedEigenvectors.rows()) {
 	   rowMatrixOfSortedEigenvectors.col(i) = sortedEigenvectors[i].second;
	} else {
		rowMatrixOfSortedEigenvectors.col(i).setZero();
	}
  }

  return rowMatrixOfSortedEigenvectors.transpose();
}

ReducedDimClassInfo
ComputeClassInfoInPcaDimensionReducedSpace(
    const std::vector<ClassificationObject>& data,
    const ClassInfo& cartesianClassInfo,
    const unsigned reducedDimensionality) {

  // create the space transformation matrix
  unsigned originalDimensionality = cartesianClassInfo.mean.size();
  Eigen::MatrixXd eigenspaceTransformMatrix(
    reducedDimensionality,
    originalDimensionality
  );

  eigenspaceTransformMatrix =
    (GetSortedEigenvectors(cartesianClassInfo.covariance)).topLeftCorner(
      reducedDimensionality,
      originalDimensionality
    );

  // transform all of the data to the new space
  std::vector<ClassificationObject> reducedData;
  reducedData.reserve(data.size());
  for (unsigned i = 0; i < data.size(); ++i) {
    ClassificationObject temp;
    temp.label = data[i].label;

    temp.features.resize(reducedDimensionality);
    temp.features = eigenspaceTransformMatrix * data[i].features;

    reducedData.push_back(temp);
  }

  // compute the new class info
  ReducedDimClassInfo info;
  info.longMean = cartesianClassInfo.mean;
  info.transformationMatrix = eigenspaceTransformMatrix;
  info.ci = ComputeClassInfo(
    reducedData,
    cartesianClassInfo.label,
    reducedDimensionality
  );

  return info;
}

unsigned
PcaClassifyObject(const ClassificationObject& object, const std::vector<ReducedDimClassInfo>& classSummaries) {
  unsigned mostLikelyClass = classSummaries.front().ci.label;
  double highestProbability = 0.0;

  for (const ReducedDimClassInfo& classSummary : classSummaries) {
    Eigen::VectorXd scaledFeatureVector = object.features - classSummary.longMean;
    Eigen::VectorXd reducedFeatureVector = classSummary.transformationMatrix * scaledFeatureVector;
    double probability = GaussianPdf(reducedFeatureVector, classSummary.ci) * classSummary.ci.prior;
    if (probability > highestProbability) {
      mostLikelyClass = classSummary.ci.label;
      highestProbability = probability;
    }
  }

std::cout<<highestProbability<<std::endl;

  return mostLikelyClass;
}

#endif //_EIGEN_SPACE_


// SPDX-License-Identifier: Apache-2.0
// Copyright 2021 - 2021, the Anboto author and contributors
#include <Core/Core.h>
#include <ScatterDraw/ScatterDraw.h>
#include <Eigen/Eigen.h>
#include <Functions4U/Functions4U.h>
#include <STEM4U/Mooring.h>
#include <plugin/png/png.h>

using namespace Upp;

void TestMooring(bool test) {
	UppLog() << "\n\nCatenary mooring demo";

	double rho_m = 346.7;		// Kg/m
	double rho_m3 = 7850;
	double rho_water = 1025;

	double BL = 1.49E9;			// Mooring line break load
	double moorlen = 490;

	double xanchorvessel = 480;
	double zanchor = 0;
	double zvessel = 54;

	double Fhanchorvessel, Fvanchor, Fvvessel, xonfloor;
	Vector<double> x, z;
	MooringStatus status;

	Catenary(moorlen, xanchorvessel, zanchor, zvessel, xonfloor);

	double moorlen2;
	CatenaryGetLen(xonfloor, xanchorvessel, zanchor, zvessel, moorlen2);
	VERIFY(EqualDecimals(moorlen, moorlen2, 4));

	CatenaryGetLen(0.1*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(484.4677, moorlen2, 4));
	CatenaryGetLen(0.15*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(484.7264, moorlen2, 4));
	CatenaryGetLen(0.2*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(485.0166, moorlen2, 4));
	CatenaryGetLen(0.25*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(485.3445, moorlen2, 4));
	CatenaryGetLen(0.3*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(485.7176, moorlen2, 4));

	status = Catenary(rho_m, rho_m3, rho_water, moorlen, BL, xanchorvessel, zanchor, zvessel, Fhanchorvessel, Fvanchor, Fvvessel, xonfloor, x, z, 10);
	UppLog() << "\nStatus is: " << MooringStatusStr(status);
	VERIFY(status == CATENARY_ON_FLOOR);
	VERIFY(EqualDecimals(xonfloor, 292.5933, 4));
	VERIFY(EqualDecimals(Fhanchorvessel, 987138.9765, 4));
	VERIFY(EqualDecimals(Fvvessel, 583737.6409, 4));

	double Fanchor = sqrt(sqr(Fhanchorvessel) + sqr(Fvanchor));
	double anganchor = atan2(Fvanchor, Fhanchorvessel)*180./M_PI;
	double Fvessel = sqrt(sqr(Fhanchorvessel) + sqr(Fvvessel));
	double angvessel = atan2(Fvvessel, Fhanchorvessel)*180./M_PI;
	UppLog() << Format("\nFhanchorvessel=%.2f, Fvanchor=%.2f, Fanchor=%.2f, ang_anchor=%.1f grad\nFvvessel=%.2f, Fvessel=%.2f, ang_vessel=%.1f, xonfloor=%.1f",
		Fhanchorvessel, -Fvanchor, Fanchor, anganchor, -Fvvessel, Fvessel, angvessel, xonfloor);
	VERIFY(FormatF(angvessel, 4) == "30.5976");

	UppLog() << "\nX\tY";
	for (int i = 0; i < x.size(); ++i)
		UppLog() << Format("\n%.2f\t%.4f", x[i], z[i]);
	VERIFY(FormatF(z[7], 4) == "9.8116");

	if (!test) {
		int num = 100;
		Vector<Vector<double>> xx(5), zz(5);
		String dir = AppendFileNameX(GetDesktopFolder(), "STEM4U_Demo");
		RealizeDirectory(dir);
		ScatterDraw scatter;
		scatter.SetLabelX("Distance from anchor to vessel [m]").
				SetLabelY("Height from floor to vessel [m]").
				SetLeftMargin(70).SetBottomMargin(50).SetSize(Size(1000, 400)).
				SetLegendAnchor(ScatterDraw::LEFT_TOP);
		{
			scatter.RemoveAllSeries().SetTitle("Shifting the vessel to the anchor");
			for (int iz = 0; iz < 5; ++iz) {
				Catenary(rho_m, rho_m3, rho_water, moorlen, BL,
					xanchorvessel-iz*15, zanchor, zvessel, Fhanchorvessel, Fvanchor, Fvvessel,
					xonfloor, xx[iz], zz[iz], num);
				String legend = Format("Xanchorvessel:%.0f, Fx:%.0f, Fzanchor:%.0f, Fzvessel:%.0f, xonfloor:%.1f",
						xanchorvessel-iz*15, Fhanchorvessel/1000., Fvanchor/1000., Fvvessel/1000., xonfloor);
				scatter.AddSeries(xx[iz], zz[iz]).Legend(legend).NoMark();
				scatter.AddSeries(&zz[iz][0], 1, xx[iz][0], 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
				scatter.AddSeries(&zz[iz].Top(), 1, xx[iz].Top(), 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
			}
			scatter.ZoomToFit(true, true);
			PNGEncoder().SaveFile(AppendFileNameX(dir, "Moor xanchorvessel.png"), scatter.GetImage());
		}
		{
			scatter.RemoveAllSeries().SetTitle("Raising the vessel");
			for (int iz = 0; iz < 5; iz++) {
				Catenary(rho_m, rho_m3, rho_water, moorlen, BL,
					xanchorvessel, zanchor, zvessel+iz*10, Fhanchorvessel, Fvanchor, Fvvessel,
					xonfloor, xx[iz], zz[iz], num);
				String legend = Format("Zvessel:%.0f, Fx:%.0f, Fzanchor:%.0f, Fzvessel:%.0f, xonfloor:%.1f",
						zvessel+iz*10, Fhanchorvessel/1000., Fvanchor/1000., Fvvessel/1000., xonfloor);
				scatter.AddSeries(xx[iz], zz[iz]).Legend(legend).NoMark();
				scatter.AddSeries(&zz[iz][0], 1, xx[iz][0], 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
				scatter.AddSeries(&zz[iz].Top(), 1, xx[iz].Top(), 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
			}
			scatter.ZoomToFit(true, true);
			PNGEncoder().SaveFile(AppendFileNameX(dir, "Moor zvessel.png"), scatter.GetImage());
		}
		{
			scatter.RemoveAllSeries().SetTitle("Raising the anchor");
			for (int iz = 0; iz < 5; iz++) {
				Catenary(rho_m, rho_m3, rho_water, moorlen, BL,
					xanchorvessel, zanchor+iz*30, zvessel, Fhanchorvessel, Fvanchor, Fvvessel,
					xonfloor, xx[iz], zz[iz], num);
				String legend = Format("Zanchor:%.0f, Fx:%.0f, Fzanchor:%.0f, Fzvessel:%.0f, xonfloor:%.1f",
						zanchor+iz*30, Fhanchorvessel/1000., Fvanchor/1000., Fvvessel/1000., xonfloor);
				scatter.AddSeries(xx[iz], zz[iz]).Legend(legend).NoMark();
				scatter.AddSeries(&zz[iz][0], 1, xx[iz][0], 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
				scatter.AddSeries(&zz[iz].Top(), 1, xx[iz].Top(), 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
			}
			scatter.ZoomToFit(true, true);
			PNGEncoder().SaveFile(AppendFileNameX(dir, "Moor zanchor.png"), scatter.GetImage());
		}
	}
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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 <fcppt/math/pi.hpp>
#include <fcppt/math/matrix/componentwise_equal.hpp>
#include <fcppt/math/matrix/identity.hpp>
#include <fcppt/math/matrix/output.hpp>
#include <fcppt/math/matrix/rotation_axis.hpp>
#include <fcppt/math/matrix/rotation_x.hpp>
#include <fcppt/math/matrix/rotation_y.hpp>
#include <fcppt/math/matrix/rotation_z.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/math/matrix/vector.hpp>
#include <fcppt/math/vector/componentwise_equal.hpp>
#include <fcppt/math/vector/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

namespace
{

float const epsilon{
	0.001f
};

template<
	typename Matrix
>
bool
compare_matrices(
	Matrix const &_m1,
	Matrix const &_m2
)
{
	return
		fcppt::math::matrix::componentwise_equal(
			_m1,
			_m2,
			epsilon
		);
}

}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_rotation_axis
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		float,
		4,
		4
	>
	matrix_type;

	typedef
	fcppt::math::vector::static_<
		float,
		3
	>
	vector_rotation_type;

	matrix_type const trans(
		fcppt::math::matrix::rotation_axis(
			fcppt::math::pi<
				float
			>(),
			vector_rotation_type(
				1.f,
				0.f,
				0.f
			)
		)
	);

	typedef
	fcppt::math::vector::static_<
		float,
		4
	>
	vector_type;

	vector_type const vec(
		0.f,
		1.f,
		0.f,
		1.f
	);

	BOOST_CHECK(
		fcppt::math::vector::componentwise_equal(
			trans
			*
			vec,
			vector_type(
				0.f,
				-1.f,
				0.f,
				1.f
			),
			epsilon
		)
	);

	BOOST_CHECK((
		::compare_matrices(
			fcppt::math::matrix::rotation_axis(
				0.0f,
				vector_rotation_type(
					0.f,
					0.f,
					0.f
				)
			),
			fcppt::math::matrix::identity<
				matrix_type
			>()
		)
	));

	float const angle{
		fcppt::math::pi<
			float
		>()
		/
		2.f
	};

	BOOST_CHECK((
		::compare_matrices(
			fcppt::math::matrix::rotation_axis(
				angle,
				vector_rotation_type(
					1.0f,
					0.0f,
					0.0f
				)
			),
			fcppt::math::matrix::rotation_x(
				angle
			)
		)
	));

	BOOST_CHECK((
		::compare_matrices(
			fcppt::math::matrix::rotation_axis(
				angle,
				vector_rotation_type(
					0.0f,
					1.0f,
					0.0f
				)
			),
			fcppt::math::matrix::rotation_y(
				angle
			)
		)
	));

	BOOST_CHECK((
		::compare_matrices(
			fcppt::math::matrix::rotation_axis(
				angle,
				vector_rotation_type(
					0.0f,
					0.0f,
					1.0f
				)
			),
			fcppt::math::matrix::rotation_z(
				angle
			)
		)
	));
}


#include <limbus/OpenglWindow.hpp>
#include <limbus/Timer.hpp>
#include <limbus/opengl.h>
#include <pingo/SpriteBuffer.hpp>
#include <pingo/Texture.hpp>

#include <iostream>
#include <complex>
#include <cmath>
#include <boost/thread.hpp>

class FractalRenderer
{
public:
	struct Color
	{
		Color() {}
		Color(double r, double g, double b) : r(r), g(g), b(b) {}
		double r, g, b;

		Color operator * (double factor)
		{
			return Color(r*factor, g*factor, b*factor);
		}

		Color operator + (const Color& other)
		{
			return Color(r + other.r, g + other.g, b + other.b);
		}
	};

	struct Job
	{
		size_t y_start;
		size_t y_count;
		double scale;
		std::complex<double> offset;
	};

	static const int max_iterations = 1000;
	static const int max_colors = 50;

	FractalRenderer(unsigned char* texture_data, Color* color_ramp, double texture_width, double texture_height)
		: texture_data(texture_data), color_ramp(color_ramp), texture_width(texture_width), texture_height(texture_height), job_done(false), running(true)
	{
	}

	typedef boost::lock_guard<boost::mutex> lock_guard;

	void setJob(Job new_job)
	{
		current_job = new_job;
		lock_guard lock(job_mutex);
		job_done = false;
	}

	bool jobDone()
	{
		lock_guard lock(job_mutex);
		return job_done;
	}

	void stop()
	{
		lock_guard lock(running_mutex);
		running = false;
	}

	void operator () ()
	{
		bool _running;

		do
		{
			bool done;
			{
				lock_guard lock(job_mutex);
				done = job_done;
			}

			if (!done)
			{
				for (size_t _y = current_job.y_start; _y < current_job.y_start + current_job.y_count; ++_y)
				{
					for (size_t _x = 0; _x < (size_t)texture_width; ++_x)
					{
						std::complex<double> c(((double)_x / texture_width) * 2.0 - 1.0,
											   ((double)_y / texture_height) * 2.0 - 1.0);

						c *= current_job.scale;
						c += current_job.offset;

						std::complex<double> z(0.0, 0.0);

						double q = (z.real() - 1/4)*(z.real() - 1/4) + z.imag()*z.imag();
						bool inside_cardioid = q * (q + (z.real() - 1/4)) < (1/4)*z.imag()*z.imag();
						bool inside_period2_bulb = (z.real() + 1)*(z.real() + 1) + z.imag()*z.imag() < 1/16;

						size_t i = 0;
						if (!inside_cardioid || !inside_period2_bulb)
						{
							while (i < max_iterations && z.real()*z.real() + z.imag()*z.imag() < 2.0*2.0)
							{
								z = z*z + c;
								i += 1;
							}
						}
						else
						{
							i = max_iterations;
						}

						if (z.real()*z.real() + z.imag()*z.imag() < 2.0*2.0 || i == max_iterations)
							WritePixel(_x, _y, -1, 0, 0.0);
						else
						{
							double s = i + (log(log((double)max_iterations)) - log(log(abs(z))))/log(2.0);

							int first = (int)s;
							int second = first + 1;
							double factor = s - first;
							WritePixel(_x, _y, first, second, factor);
						}
					}
				}

				lock_guard lock(job_mutex);
				job_done = true;
			}
			else
			{
				boost::thread::sleep(boost::get_system_time() + boost::posix_time::milliseconds(1));
			}

			lock_guard lock(running_mutex);
			_running = running;
		} while (_running);
	}

private:
	FractalRenderer(FractalRenderer& other);

	Job current_job;
	bool job_done;
	bool running;

	boost::mutex job_mutex, running_mutex;

	double texture_width;
	double texture_height;

	unsigned char* texture_data;
	Color* color_ramp;

	void WritePixel(size_t x, size_t y, int first, int second, double factor)
	{
		size_t offset = y * (size_t)texture_width + x;

		Color color;
		if (first == -1)
			color = Color(0, 0, 0);
		else
			color = color_ramp[second % (max_colors*2)] * factor + color_ramp[first % (max_colors*2)] * (1.0 - factor);

		texture_data[offset * 3 + 0] = (unsigned char)(color.r * 255.0);
		texture_data[offset * 3 + 1] = (unsigned char)(color.g * 255.0);
		texture_data[offset * 3 + 2] = (unsigned char)(color.b * 255.0);
	}
};

class Application : public Limbus::OpenglWindow::EventHandler
{
	bool running;

	void onClose(Limbus::OpenglWindow& self)
	{
		running = false;
	}

	Limbus::OpenglWindow window;
	Pingo::SpriteBuffer* sprite_buffer;
	Pingo::Texture* texture;
	unsigned char* texture_data;

	FractalRenderer::Color color_ramp[FractalRenderer::max_colors*2];

public:
	void run()
	{
		running = true;
		window.setCaption("Fractal Demo");
		window.addEventHandler(this);
		window.setWidth(256);
		window.setHeight(256);
		window.create();

		glClearColor( 0.0f, 0.0f, 0.0f, 0.0f );
		glDisable( GL_DEPTH_TEST );
		glEnable( GL_BLEND );
		glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
		glEnable( GL_TEXTURE_2D );

		glMatrixMode( GL_PROJECTION );
		glLoadIdentity();
		glOrtho( 0.0f, window.getWidth(), window.getHeight(), 0.0f, -100.0f, 100.0f );
		glMatrixMode( GL_MODELVIEW );
		glLoadIdentity();

		texture_data = new unsigned char[window.getWidth() * window.getHeight() * 3];

		texture = new Pingo::Texture();
		texture->loadFromMemory(texture_data, window.getWidth(), window.getHeight(), 3);
		sprite_buffer = new Pingo::SpriteBuffer(texture, 1, true);

		sprite_buffer->setWritable(true);
		sprite_buffer->setRectangle(0, 0.0f, 0.0f, (float)window.getWidth(), (float)window.getHeight());
		sprite_buffer->setColor(0, 1.0f, 1.0f, 1.0f, 1.0f);
		sprite_buffer->setTextureRectangle(0, 0.0f, 0.0f, (float)window.getWidth(), (float)window.getHeight());
		sprite_buffer->setWritable(false);

		for (size_t i = 0; i < FractalRenderer::max_colors; ++i)
		{
			float factor = (float)i / FractalRenderer::max_colors;
			float inverse_factor = (float)(FractalRenderer::max_colors - i) / FractalRenderer::max_colors;

			color_ramp[i] = FractalRenderer::Color(sqrt(sqrt(factor)), factor, inverse_factor * 0.5f);
			color_ramp[FractalRenderer::max_colors*2 - 1 - i] = color_ramp[i];
		}

		FractalRenderer worker(texture_data, color_ramp, (double)window.getWidth(), (double)window.getHeight());
		FractalRenderer worker2(texture_data, color_ramp, (double)window.getWidth(), (double)window.getHeight());

		FractalRenderer::Job job;
		job.offset = std::complex<double>(+0.001643721971153, +0.822467633298876);
		job.scale = 2.0;
		job.y_start = 0;
		job.y_count = window.getHeight() / 2;

		FractalRenderer::Job job2;
		job2.offset = std::complex<double>(+0.001643721971153, +0.822467633298876);
		job2.scale = 2.0;
		job2.y_start = window.getHeight() / 2;
		job2.y_count = window.getHeight() - job2.y_start;

		worker.setJob(job);
		worker2.setJob(job2);

		boost::thread worker_thread(boost::ref(worker));
		boost::thread worker_thread2(boost::ref(worker2));

		Limbus::Timer timer;
		while (running)
		{
			window.pollEvents();
			double elapsed = 1.0f + timer.getElapsed() * 0.005;

			if (worker.jobDone() && worker2.jobDone())
			{
				texture->update(texture_data);

				job.scale = 1.0 / (elapsed * (1.0 / job.scale));
				if (job.scale < 0.00000000001)
					job.scale = 0.00000000001;
				worker.setJob(job);

				job2.scale = job.scale;
				worker2.setJob(job2);
			}

			sprite_buffer->draw(0, 1, 0, 0);
			window.swapBuffers();
		}

		worker.stop();
		worker2.stop();
		worker_thread.join();
		worker_thread2.join();

		delete sprite_buffer;
		delete texture;
		delete [] texture_data;
	}
};

void main()
{
	Application app;
	app.run();
}


/*
* compile with flags:   g++ test.cc softmax_classifier.cc   ../datasets/datasets.cc -std=c++14 -larmadillo -I ../../include/
* author: Yuzhen Liu
* Date: 2019.3.29 10:55
*/

#include <iostream>
#include <armadillo>
#include <cmath>
#include <logistic/logistic_classifier.h>
#include <logistic/softmax_classifier.h>
#include <datasets/datasets.h>

using namespace std;
using namespace arma;

int main() {
    Datasets dataset = Datasets("iris");

    // Logistic classifier testing
    // vec y = dataset.y.subvec(0, 99);
    // mat x = dataset.x.submat(0, 0, 3, 99);
    // Logisitic_Classifier lr_classifier = Logisitic_Classifier();
    // lr_classifier.train(x, y);
    // vec res = lr_classifier.predict(mat({{6.1, 2.9, 4.7, 1.4}, {5.1, 3.5, 1.4, 0.2}}).t());
    // res.print();

    // softmax classifier test (for multiple classies)
    Softmax_Classifier softmax_classifier = Softmax_Classifier();
    softmax_classifier.train(dataset.x, dataset.y, 3);
    vec res = softmax_classifier.predict(dataset.x);
    // vec res = softmax_classifier.predict(mat({{6.1, 2.9, 4.7, 1.4}, {5.1, 3.5, 1.4, 0.2}, {6.1, 2.6, 5.6, 1.4}}).t());
    res.print();
    return 0;
}


/*
 * Copyright (c) 2015, Jonathan Ventura
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 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.
 */

#ifndef POLYNOMIAL_INTERNAL_HPP
#define POLYNOMIAL_INTERNAL_HPP

#include <Eigen/Core>
#include <vector>
#include <queue>

namespace polynomial
{
    namespace Internal
    {

        template<int a,int b> struct max{ static const int value=(a<b)?b:a; };
        template<int a,int b> struct min{ static const int value=(a>b)?b:a; };

        template <int length>
        inline
        Eigen::Map< Eigen::Matrix<double,1,length> > vecmap(double *data)
        {
            return Eigen::Map< Eigen::Matrix<double,1,length> >(data);
        }

        template <int length>
        inline
        Eigen::Map< const Eigen::Matrix<double,1,length> > vecmap(const double *data)
        {
            return Eigen::Map< const Eigen::Matrix<double,1,length> >(data);
        }

        template <int deg1,int deg2,int k>
        struct
        _PolyConv
        {
            static void
            compute( double *coefout, const double *coef1, const double *coef2 )
            {
                coefout[k] =
                (
                 vecmap<min<k,deg1>::value-max<0,k-deg2>::value+1>
                 (
                  coef1+max<0,k-deg2>::value
                  )
                 .array()
                 *
                 vecmap<min<k,deg1>::value-max<0,k-deg2>::value+1>
                 (
                  coef2+k-min<k,deg1>::value
                  ).reverse().array()
                 ).sum();
                _PolyConv<deg1,deg2,k-1>::compute( coefout, coef1, coef2 );
            }
        };

        template <int deg1,int deg2>
        struct
        _PolyConv<deg1,deg2,-1>
        {
            static void
            compute( double *coefout, const double *coef1, const double *coef2 )
            {
            }
        };

        template <int deg1,int deg2>
        struct
        PolyConv
        {
            static void
            compute( Eigen::Matrix<double,deg1+deg2+1,1> &coefout, const Eigen::Matrix<double,deg1+1,1> &coef1, const Eigen::Matrix<double,deg2+1,1> &coef2 )
            {
                _PolyConv<deg1,deg2,deg1+deg2>::compute(coefout.data(),coef1.data(),coef2.data());
            }
        };

        template <>
        struct
        PolyConv<Eigen::Dynamic,Eigen::Dynamic>
        {
            static void
            compute( Eigen::VectorXd &coefout, const Eigen::VectorXd &coef1, const Eigen::VectorXd &coef2 )
            {
                int deg1 = coef1.rows()-1;
                int deg2 = coef2.rows()-1;

                for ( int k = deg1+deg2; k >= 0; k-- )
                {
                    coefout[k] =
                    (
                     coef1.block(
                                 std::max(0,k-deg2),0,
                                 std::min(k,deg1)-std::max(0,k-deg2)+1,1
                                 ).array()
                     *
                     coef2.block(
                                 k-std::min(k,deg1),0,
                                 std::min(k,deg1)-std::max(0,k-deg2)+1,1
                                 ).reverse().array()
                     ).sum();
                }
            }
        };

        template<int deg,int k>
        struct
        _PolyVal
        {
            static double
            compute( const Eigen::Matrix<double,deg+1,1> &coef, const double &x )
            {
                return coef[k] + x*_PolyVal<deg,k-1>::compute(coef,x);
            }
        };

        template<int deg>
        struct
        _PolyVal<deg,0>
        {
            static double
            compute( const Eigen::Matrix<double,deg+1,1> &coef, const double &x )
            {
                return coef[0];
            }
        };

        template<int deg>
        struct
        PolyVal
        {
            static double
            compute( const Eigen::Matrix<double,deg+1,1> &coef, const double &x )
            {
                return Internal::_PolyVal<deg,deg>::compute( coef, x );
            }
        };

        template<>
        struct
        PolyVal<Eigen::Dynamic>
        {
            static double
            compute( const Eigen::VectorXd &coef, const double &x )
            {
                int deg = coef.rows()-1;
                double val = 0;
                for ( int i = 0; i <= deg; i++ ) val = coef(i) + x*val;
                return val;
            }
        };

        template<int deg>
        struct BisectionMethod
        {
            static double compute(const Eigen::Matrix<double,deg+1,1> &coef, const double lower, const double upper )
            {
                double a = lower;
                double fa = PolyVal<deg>::compute(coef,a);
                char fapos = (fa>0);
                double b = upper;
                double c = 0.5*(a+b);

                for ( int i = 0; i < 24; i++ )
                {
                    const double fc = PolyVal<deg>::compute(coef,c);
                    const char fcpos = (fc>0);
                    if ( fcpos == fapos )
                    {
                        a = c;
                        fa = fc;
                    } else {
                        b = c;
                    }
                    c = 0.5*(a+b);
                }

                return c;
            }
        };

        template<>
        struct BisectionMethod<Eigen::Dynamic>
        {
            static double compute(const Eigen::VectorXd &coef, const double lower, const double upper )
            {
                double a = lower;
                double fa = PolyVal<Eigen::Dynamic>::compute(coef,a);
                char fapos = (fa>0);
                double b = upper;
                double c = 0.5*(a+b);

                for ( int i = 0; i < 100; i++ )
                {
                    const double fc = PolyVal<Eigen::Dynamic>::compute(coef,c);
                    const char fcpos = (fc>0);
                    if ( fcpos == fapos )
                    {
                        a = c;
                        fa = fc;
                    } else {
                        b = c;
                    }
                    c = 0.5*(a+b);
                }

                return c;
            }
        };

        template<int deg>
        struct SturmChain
        {
            Eigen::Matrix<double,deg+1,1> coef;
            SturmChain<deg-1> next;
        };

        template<>
        struct SturmChain<0>
        {
            Eigen::Matrix<double,1,1> coef;
        };

        template<int degin,int degout,int k>
        struct _GetSturmChain
        {
            static SturmChain<degout>& compute( SturmChain<degin> &sc )
            {
                return _GetSturmChain<degin-1,degout,k-1>::compute(sc.next);
            }
        };

        template<int degin,int degout>
        struct _GetSturmChain<degin,degout,0>
        {
            static SturmChain<degout>& compute( SturmChain<degin> &sc )
            {
                return sc;
            }
        };

        template<int degin,int degout>
        struct GetSturmChain
        {
            static SturmChain<degout>& compute( SturmChain<degin> &sc )
            {
                return _GetSturmChain<degin,degout,degin-degout>::compute(sc);
            }
        };

        template <int deg,int k>
        struct _ModPoly
        {
            static const void compute( SturmChain<deg> &sc )
            {
                const Eigen::Matrix<double,k+3,1> &u( GetSturmChain<deg,k+2>::compute(sc).coef );
                const Eigen::Matrix<double,k+2,1> &v( GetSturmChain<deg,k+1>::compute(sc).coef );
                Eigen::Matrix<double,k+3,1> r = u;
                const double s = v(0)/fabs(v(0));
                r(k+2) *= s;
                const int blocklength = k+1;
                r.block(1,0,blocklength,1) = s * r.block(1,0,blocklength,1) - r(0) * v.block(1,0,blocklength,1);
                r.block(2,0,blocklength,1) = s * r.block(2,0,blocklength,1) - r(1) * v.block(1,0,blocklength,1);
                Eigen::Matrix<double,k+1,1> &rout( GetSturmChain<deg,k>::compute(sc).coef );
                rout = r.block(2,0,k+1,1);
                double f = -fabs(rout(0));
                rout /= f;
                _ModPoly<deg,k-1>::compute(sc);
            }
        };

        template <int deg>
        struct _ModPoly<deg,0>
        {
            static const void compute( SturmChain<deg> &sc )
            {
                const Eigen::Matrix<double,3,1> &u( GetSturmChain<deg,2>::compute(sc).coef );
                const Eigen::Matrix<double,2,1> &v( GetSturmChain<deg,1>::compute(sc).coef );
                Eigen::Matrix<double,3,1> r = u;
                const double s = v(0)/fabs(v(0));
                r(2) *= s;
                r(1) = s * r(1) - r(0) * v(1);
                r(2) = s * r(2) - r(1) * v(1);
                GetSturmChain<deg,0>::compute(sc).coef[0] = -r(2);
            }
        };

        template <int deg>
        struct ModPoly
        {
            static const void compute( SturmChain<deg> &sc )
            {
                _ModPoly<deg,deg-2>::compute(sc);
            }
        };

        template <int deg>
        struct _SignChanges
        {
            static const void compute( SturmChain<deg> &sc, const double x, const double lf, int &count )
            {
                double f = PolyVal<deg>::compute( sc.coef, x );
                count += (f*lf<0);
                _SignChanges<deg-1>::compute( sc.next, x, f, count );
            }
        };

        template <>
        struct _SignChanges<0>
        {
            static const void compute( SturmChain<0> &sc, const double x, const double lf, int &count )
            {
                double f = sc.coef[0];
                count += (f*lf<0);
            }
        };

        template <int deg>
        struct SignChanges
        {
            static const void compute( SturmChain<deg> &sc, const double x, int &count )
            {
                double f = PolyVal<deg>::compute( sc.coef, x );
                count = 0;
                _SignChanges<deg-1>::compute( sc.next, x, f, count );
            }
        };

        template <>
        struct SignChanges<0>
        {
            static const void compute( SturmChain<0> &sc, const double x, int &count )
            {
                count = 0;
            }
        };

        struct SturmInterval
        {
            double lb, ub;
            int sclb, scub;
            SturmInterval(const double _lb, const double _ub, const int _sclb, const int _scub)
            : lb(_lb), ub(_ub), sclb(_sclb), scub(_scub) { }
        };

        struct RootInterval
        {
            double lb, ub;
            RootInterval(const double _lb, const double _ub)
            : lb(_lb), ub(_ub) { }
        };

        template<int deg>
        class SturmRootFinder
        {
            void bisection(const double lb, const double ub, std::vector<double> &roots)
            {
                int sclb = 0;
                int scub = 0;
                SignChanges<deg>::compute(sc,lb,sclb);
                SignChanges<deg>::compute(sc,ub,scub);
                std::queue<SturmInterval> intervals;
                intervals.push( SturmInterval(lb,ub,sclb,scub) );

                std::vector<RootInterval> root_intervals;

                while ( !intervals.empty() )
                {
                    SturmInterval interval = intervals.front();
                    intervals.pop();

                    // get num roots in interval
                    int n = interval.sclb-interval.scub;

                    if ( n == 0 )
                    {

                    }
                    else if ( n == 1 )
                    {
                        root_intervals.push_back( RootInterval(interval.lb,interval.ub) );
                    }
                    else if ( fabs(interval.ub-interval.lb)>1e-15 )
                    {
                        double m = (interval.lb+interval.ub)*0.5;

                        // get sign changes at m
                        int scm = 0;
                        SignChanges<deg>::compute(sc,m,scm);

                        // add [lb,m] interval
                        intervals.push( SturmInterval(interval.lb,m,interval.sclb,scm) );

                        // add [m,ub] interval
                        intervals.push( SturmInterval(m,interval.ub,scm,interval.scub) );
                    }
                }

                roots.resize(root_intervals.size());
                for ( int i = 0; i < root_intervals.size(); i++ )
                {
                    roots[i] = BisectionMethod<deg>::compute(sc.coef,root_intervals[i].lb,root_intervals[i].ub);
                }
            }
        public:
            SturmChain<deg> sc;

            SturmRootFinder( const Eigen::Matrix<double,deg+1,1> &coef )
            {
                sc.coef = coef;
                sc.next.coef = coef.head(deg);
                double f = fabs(sc.coef[0] * deg);
                for ( int i = 0; i < deg; i++ ) sc.next.coef[i] *= (deg-i)/f;
                ModPoly<deg>::compute(sc);
            }

            void realRoots(const double lb, const double ub, std::vector<double> &roots)
            {
                bisection(lb,ub,roots);
            }
        };

        template<>
        class SturmRootFinder<Eigen::Dynamic>
        {
            std::vector<Eigen::VectorXd> sc;

            Eigen::VectorXd modpoly( const Eigen::VectorXd &u, const Eigen::VectorXd &v )
            {
                int udeg = u.rows()-1;
                Eigen::VectorXd r = u;
                double s = v(0)/fabs(v(0));
                r(udeg) *= s;
                int blocklength = udeg-1;
                r.block(1,0,blocklength,1) = s * r.block(1,0,blocklength,1) - r(0) * v.block(1,0,blocklength,1);
                r.block(2,0,blocklength,1) = s * r.block(2,0,blocklength,1) - r(1) * v.block(1,0,blocklength,1);
                return r.block(2,0,blocklength,1);
            }

            int signchanges(double x)
            {
                int deg = sc[0].rows()-1;
                Eigen::VectorXd f(deg+1);
                for ( int i = 0; i < deg+1; i++ ) f[i] = PolyVal<Eigen::Dynamic>::compute(sc[i],x);
                int n = 0;
                // NB: not skipping zeros here
                for ( int i = 1; i < deg+1; i++ ) n += (f[i]*f[i-1]<0);
                return n;
            }

            void bisection(const double lb, const double ub, std::vector<double> &roots)
            {
                int sclb = signchanges(lb);
                int scub = signchanges(ub);
                std::queue<SturmInterval> intervals;
                intervals.push( SturmInterval(lb,ub,sclb,scub) );

                std::vector<RootInterval> root_intervals;

                while ( !intervals.empty() )
                {
                    SturmInterval interval = intervals.front();
                    intervals.pop();

                    // get num roots in interval
                    int n = interval.sclb-interval.scub;

                    if ( n == 0 )
                    {

                    }
                    else if ( n == 1 )
                    {
                        root_intervals.push_back( RootInterval(interval.lb,interval.ub) );
                    }
                    else if ( fabs(interval.ub-interval.lb)>1e-15 )
                    {
                        double m = (interval.lb+interval.ub)*0.5;

                        // get sign changes at m
                        int scm = signchanges(m);

                        // add [lb,m] interval
                        intervals.push( SturmInterval(interval.lb,m,interval.sclb,scm) );

                        // add [m,ub] interval
                        intervals.push( SturmInterval(m,interval.ub,scm,interval.scub) );
                    }
                }

                roots.resize(root_intervals.size());
                for ( int i = 0; i < root_intervals.size(); i++ )
                {
                    roots[i] = BisectionMethod<Eigen::Dynamic>::compute(sc[0],root_intervals[i].lb,root_intervals[i].ub);
                }
            }
        public:
            SturmRootFinder( const Eigen::VectorXd &coef )
            {
                int deg = coef.rows()-1;
                sc.resize(deg+1);

                sc[0] = coef;

                double f = fabs(sc[0](0) * deg);
                sc[1] = sc[0].block(0,0,deg,1);
                for ( int i = 0; i < deg; i++ ) sc[1][i] *= (deg-i)/f;

                for ( int i = 2; i < deg+1; i++ )
                {
                    sc[i] = modpoly(sc[i-2],sc[i-1]);
                    if ( i != deg )
                    {
                        double f = -fabs(sc[i](0));
                        sc[i] /= f;
                    }
                    else
                    {
                        sc[i] = -sc[i];
                    }
                }
            }

            void realRoots(const double lb, const double ub, std::vector<double> &roots)
            {
                bisection(lb,ub,roots);
            }
        };

        template<int deg>
        struct RootFinder
        {
            static void compute( const Eigen::Matrix<double,deg+1,1> &p, std::vector<double> &roots )
            {
                Eigen::PolynomialSolver<double,deg> ps;
                ps.compute(p.reverse());
                ps.realRoots(roots);
            }
        };

        template<>
        struct RootFinder<Eigen::Dynamic>
        {
            static void compute( const Eigen::Matrix<double,Eigen::Dynamic,1> &p, std::vector<double> &roots )
            {
                Eigen::PolynomialSolver<double,Eigen::Dynamic> ps;
                ps.compute(p.reverse());
                ps.realRoots(roots);
            }
        };

        template<>
        struct RootFinder<2>
        {
            static void compute( const Eigen::Matrix<double,3,1> &p, std::vector<double> &roots )
            {
                const double discrim = p[1]*p[1] - 4.*p[0]*p[2];
                if ( discrim < 0 )
                {
                    roots.clear();
                }
                else if ( discrim == 0 )
                {
                    roots.resize(1);
                    roots[0] = -p[1] / ( 2. * p[0] );
                }
                else
                {
                    const double twoa = 2. * p[0];
                    const double sqrt_discrim = sqrt(discrim);
                    roots.resize(2);
                    roots[0] = (-p[1] + sqrt_discrim) / twoa;
                    roots[1] = (-p[1] - sqrt_discrim) / twoa;
                }
            }
        };

    } // end namespace Internal

} // end namespace Polynomial

#endif


#include <iostream>
#include <boost/dynamic_bitset.hpp>

// Actually, you may replace dynamic bitset with anything else
// like std::bitset or std::vector, I made my choice because of
// this totally reliable (random link from google) research:
// https://www.researchgate.net/publication/220803585_Performance_of_C_bit-vector_implementations

static bool buildAtkinSieve(boost::dynamic_bitset<>& sieve) {
	if (&sieve == nullptr || sieve.size() < 2)
		return false;
	int size = sieve.size() - 1;
	sieve[2] = true;
	sieve[3] = true;
	int n, x = 0, y;
	int sizeSqrt = sqrt(size);
	for (int i = 1; i <= sizeSqrt; ++i) {
		x += (i << 1) - 1;
		y = 0;
		for (int j = 1; j <= sizeSqrt; ++j) {
			y += (j << 1) - 1;
			n = (x << 2) + y;
			if ((n <= size) && (n % 12 == 1 || n % 12 == 5))
				sieve[n].flip();
			n -= x;
			if ((n <= size) && (n % 12 == 7))
				sieve[n].flip();
			n -= (y << 1);
			if ((i > j) && (n <= size) && (n % 12 == 11))
				sieve[n].flip();
		}
	}
	for (int i = 5; i <= sizeSqrt; ++i) {
		if (sieve[i]) {
			n = i * i;
			for (int j = n; j <= size; j += n)
				sieve[j] = false;
		}
	}
	return true;
}

int main() {
	std::cout << "Enter the initial capacity of the sieve: ";
	int size;
	std::cin >> size;
	boost::dynamic_bitset<> sieve(size + 1);
	if (!buildAtkinSieve(sieve)) {
		std::cout << "It's too small for me to work with, so I'm done";
		return 1;
	}
	int answer;
	while (true) {	// feel free to Alt + F4
		std::cout << "\nEnter the number to check if it's prime: ";
		std::cin >> answer;
		if (answer >= sieve.size() || answer < 1) {
			std::cout << "Wrong input\n";
			std::cin.clear();
			std::cin.ignore(INT_MAX, '\n');		// ignores current input just in case
		}
		else
			std::cout << (sieve[answer] ? "It's prime.\n" : "It's not prime.\n");
	}
	return 0;
}


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

#include <basalt/vi_estimator/marg_helper.h>

#include "gtest/gtest.h"

TEST(QRTestSuite, QRvsLLT) {
  Eigen::MatrixXd J;
  J.setRandom(10, 6);

  Eigen::HouseholderQR<Eigen::MatrixXd> qr(J);
  Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
  Eigen::MatrixXd LT = (J.transpose() * J).llt().matrixU();

  std::cout << "J\n" << J << "\ncol_norms: " << J.colwise().norm() << std::endl;
  std::cout << "R\n" << R << "\ncol_norms: " << R.colwise().norm() << std::endl;
  std::cout << "LT\n"
            << LT << "\ncol_norms: " << LT.colwise().norm() << std::endl;
}

TEST(QRTestSuite, QRvsLLTRankDef) {
  Eigen::MatrixXd J;
  J.setRandom(10, 6);

  J.col(2) = J.col(4);

  Eigen::HouseholderQR<Eigen::MatrixXd> qr(J);
  Eigen::MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
  Eigen::MatrixXd LT = (J.transpose() * J).llt().matrixU();

  std::cout << "J\n" << J << "\ncol_norms: " << J.colwise().norm() << std::endl;
  std::cout << "R\n" << R << "\ncol_norms: " << R.colwise().norm() << std::endl;
  std::cout << "LT\n"
            << LT << "\ncol_norms: " << LT.colwise().norm() << std::endl;
}

#ifdef BASALT_INSTANTIATIONS_DOUBLE
TEST(QRTestSuite, RankDefLeastSquares) {
  Eigen::MatrixXd J;
  Eigen::VectorXd r;
  J.setRandom(10, 6);
  r.setRandom(10);

  J.col(1) = J.col(4);

  auto decomp = J.completeOrthogonalDecomposition();

  Eigen::VectorXd original_solution = decomp.solve(r);

  std::cout << "full solution: " << original_solution.transpose() << std::endl;
  std::cout << "Rank " << decomp.rank() << std::endl;

  std::cout << "sol OR:\t" << original_solution.tail<4>().transpose()
            << std::endl;

  std::set<int> idx_to_marg = {0, 1};
  std::set<int> idx_to_keep = {2, 3, 4, 5};

  Eigen::VectorXd sol_sc, sol_sqrt_sc, sol_sqrt_sc2, sol_qr;

  // sqrt SC version
  {
    Eigen::MatrixXd H = J.transpose() * J;
    Eigen::VectorXd b = J.transpose() * r;

    Eigen::MatrixXd marg_sqrt_H;
    Eigen::VectorXd marg_sqrt_b;

    basalt::MargHelper<double>::marginalizeHelperSqToSqrt(
        H, b, idx_to_keep, idx_to_marg, marg_sqrt_H, marg_sqrt_b);

    auto dec = marg_sqrt_H.completeOrthogonalDecomposition();

    sol_sqrt_sc = dec.solve(marg_sqrt_b);
    std::cout << "sol SQ:\t" << sol_sqrt_sc.transpose() << std::endl;
    std::cout << "rank " << dec.rank() << std::endl;

    auto dec2 = (marg_sqrt_H.transpose() * marg_sqrt_H)
                    .completeOrthogonalDecomposition();

    sol_sqrt_sc2 = dec2.solve(marg_sqrt_H.transpose() * marg_sqrt_b);
    std::cout << "sol SH:\t" << sol_sqrt_sc2.transpose() << std::endl;
    std::cout << "rank " << dec2.rank() << std::endl;
  }

  // SC version
  {
    Eigen::MatrixXd H = J.transpose() * J;
    Eigen::VectorXd b = J.transpose() * r;

    Eigen::MatrixXd marg_H;
    Eigen::VectorXd marg_b;

    basalt::MargHelper<double>::marginalizeHelperSqToSq(
        H, b, idx_to_keep, idx_to_marg, marg_H, marg_b);

    auto dec = marg_H.completeOrthogonalDecomposition();

    sol_sc = dec.solve(marg_b);
    std::cout << "sol SC:\t" << sol_sc.transpose() << std::endl;
    std::cout << "rank " << dec.rank() << std::endl;
  }

  // QR version
  {
    Eigen::MatrixXd J1 = J;
    Eigen::VectorXd r1 = r;

    Eigen::MatrixXd marg_sqrt_H;
    Eigen::VectorXd marg_sqrt_b;

    basalt::MargHelper<double>::marginalizeHelperSqrtToSqrt(
        J1, r1, idx_to_keep, idx_to_marg, marg_sqrt_H, marg_sqrt_b);

    auto dec = marg_sqrt_H.completeOrthogonalDecomposition();

    sol_qr = dec.solve(marg_sqrt_b);
    std::cout << "sol QR:\t" << sol_qr.transpose() << std::endl;
    std::cout << "rank " << dec.rank() << std::endl;
  }

  EXPECT_TRUE(sol_qr.isApprox(sol_sc));
  EXPECT_TRUE(sol_qr.isApprox(sol_sqrt_sc2));
}
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2018-2019 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2019.
// Modifications copyright (c) 2019, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/arithmetic/infinite_line_functions.hpp>
#include <boost/geometry/geometries/infinite_line.hpp>
#include <boost/geometry/algorithms/detail/make/make.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>

namespace
{
    // Boost.Test does not support BOOST_CHECK_CLOSE for integral types
    template <typename T>
    bool is_small(T const& value)
    {
        static long double const epsilon = 1.0e-5;
        return bg::math::abs(value) < epsilon;
    }
}

template <typename T, typename C>
void verify_point_on_line(bg::model::infinite_line<T> const& line,
                          C const& x, C const& y)
{
    BOOST_CHECK_MESSAGE(is_small(line.a * x + line.b * y + line.c),
                        "Point is not located on the line");
}

template <typename T>
void test_side_value()
{
    typedef bg::model::infinite_line<T> line_type;

    // Horizontal line going right
    line_type line = bg::detail::make::make_infinite_line<T>(0, 0, 10, 0);

    // Point above (= on left side)
    T d = bg::arithmetic::side_value(line, 5, 5);
    BOOST_CHECK_MESSAGE(d > 0, "point not on left side");

    // Point below (= on right side)
    d = bg::arithmetic::side_value(line, 5, -5);
    BOOST_CHECK_MESSAGE(d < 0, "point not on right side");

    // Diagonal not through origin, from right (down) to left (up)
    line = bg::detail::make::make_infinite_line<T>(5, 2, -7, 10);
    d = bg::arithmetic::side_value(line, 5, 2);
    BOOST_CHECK_MESSAGE(is_small(d), "point not on line");
    d = bg::arithmetic::side_value(line, -7, 10);
    BOOST_CHECK_MESSAGE(is_small(d), "point not on line");

    // vector is (-12, 8), move (-3,2) on the line from (5,2)
    d = bg::arithmetic::side_value(line, 2, 4);
    BOOST_CHECK_MESSAGE(is_small(d), "point not on line");

    // Go perpendicular (2,3) from (2,4) up, so right of the line (negative)
    d = bg::arithmetic::side_value(line, 4, 7);
    BOOST_CHECK_MESSAGE(d < 0, "point not on right side");

    // Go perpendicular (2,3) from (2,4) down, so left of the line (positive)
    d = bg::arithmetic::side_value(line, 0, 1);
    BOOST_CHECK_MESSAGE(d > 0, "point not on left side");
}

template <typename T>
void test_get_intersection()
{
    typedef bg::model::infinite_line<T> line_type;

    // Diagonal lines (first is same as in distance measure,
    // second is perpendicular and used there for distance measures)
    line_type p = bg::detail::make::make_infinite_line<T>(5, 2, -7, 10);
    line_type q = bg::detail::make::make_infinite_line<T>(4, 7, 0, 1);

    typedef bg::model::point<T, 2, bg::cs::cartesian> point_type;
    point_type ip;
    BOOST_CHECK(bg::arithmetic::intersection_point(p, q, ip));

    BOOST_CHECK_MESSAGE(is_small(bg::get<0>(ip) - 2), "x-coordinate wrong");
    BOOST_CHECK_MESSAGE(is_small(bg::get<1>(ip) - 4), "y-coordinate wrong");

    verify_point_on_line(p, bg::get<0>(ip), bg::get<1>(ip));
    verify_point_on_line(q, bg::get<0>(ip), bg::get<1>(ip));
}

template <typename T>
void test_same_direction()
{
    bg::model::infinite_line<T> p, q;

    // Exactly opposite, diagonal
    p = bg::detail::make::make_infinite_line<T>(2, 1, 12, 11);
    q = bg::detail::make::make_infinite_line<T>(12, 11, 2, 1);
    BOOST_CHECK(! bg::arithmetic::similar_direction(p, q));

    // Exactly opposite, horizontal
    p = bg::detail::make::make_infinite_line<T>(0, 0, 10, 0);
    q = bg::detail::make::make_infinite_line<T>(10, 0, 0, 0);
    BOOST_CHECK(! bg::arithmetic::similar_direction(p, q));

    // Exactly opposite, vertical
    p = bg::detail::make::make_infinite_line<T>(0, 0, 0, 10);
    q = bg::detail::make::make_infinite_line<T>(0, 10, 0, 0);
    BOOST_CHECK(! bg::arithmetic::similar_direction(p, q));

    // Exactly equal, diagonal
    p = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    q = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    BOOST_CHECK(bg::arithmetic::similar_direction(p, q));

    // Exactly equal, horizontal
    p = bg::detail::make::make_infinite_line<T>(0, 0, 10, 0);
    q = bg::detail::make::make_infinite_line<T>(0, 0, 10, 0);
    BOOST_CHECK(bg::arithmetic::similar_direction(p, q));

    // Exactly equal, vertical
    p = bg::detail::make::make_infinite_line<T>(0, 0, 0, 10);
    q = bg::detail::make::make_infinite_line<T>(0, 0, 0, 10);
    BOOST_CHECK(bg::arithmetic::similar_direction(p, q));

    // Coming together, diagonal
    p = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    q = bg::detail::make::make_infinite_line<T>(20, 20, 10, 10);
    BOOST_CHECK(! bg::arithmetic::similar_direction(p, q));

    // Leaving from common point, diagonal
    p = bg::detail::make::make_infinite_line<T>(10, 10, 0, 0);
    q = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    BOOST_CHECK(! bg::arithmetic::similar_direction(p, q));

    // Continuing each other, diagonal
    p = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    q = bg::detail::make::make_infinite_line<T>(10, 10, 20, 20);
    BOOST_CHECK(bg::arithmetic::similar_direction(p, q));

    // (Nearly) perpendicular
    p = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    q = bg::detail::make::make_infinite_line<T>(0, 0, -10, 10);
    BOOST_CHECK(! bg::arithmetic::similar_direction(p, q));

    // 45 deg
    p = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    q = bg::detail::make::make_infinite_line<T>(0, 0, 0, 10);
    BOOST_CHECK(bg::arithmetic::similar_direction(p, q));

    // a bit more than 45 deg
    p = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    q = bg::detail::make::make_infinite_line<T>(0, 0, -1, 10);
    BOOST_CHECK(! bg::arithmetic::similar_direction(p, q));

    // 135 deg
    p = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    q = bg::detail::make::make_infinite_line<T>(0, 0, -10, 0);
    BOOST_CHECK(! bg::arithmetic::similar_direction(p, q));
}

template <typename T>
void test_degenerate()
{
    typedef bg::model::infinite_line<T> line_type;

    line_type line = bg::detail::make::make_infinite_line<T>(0, 0, 10, 0);
    BOOST_CHECK(! bg::arithmetic::is_degenerate(line));

    line = bg::detail::make::make_infinite_line<T>(0, 0, 0, 10);
    BOOST_CHECK(! bg::arithmetic::is_degenerate(line));

    line = bg::detail::make::make_infinite_line<T>(0, 0, 10, 10);
    BOOST_CHECK(! bg::arithmetic::is_degenerate(line));

    line = bg::detail::make::make_infinite_line<T>(0, 0, 0, 0);
    BOOST_CHECK(bg::arithmetic::is_degenerate(line));
}

template <typename T>
void test_all()
{
    test_side_value<T>();
    test_get_intersection<T>();
    test_same_direction<T>();
    test_degenerate<T>();
}

int test_main(int, char* [])
{
    test_all<double>();
    test_all<long double>();
    test_all<float>();
    test_all<int>();
    return 0;
}


#include <boost/geometry.hpp>
#include <iostream>
#include <vector>

const int dimension = 2;
using point = boost::geometry::model::point<int, dimension, boost::geometry::cs::cartesian>;
using box = boost::geometry::model::box<point>;
using boost::geometry::max_corner;
using boost::geometry::min_corner;

bool difference_correct(const box& minuend, const box& subtrahend, const std::vector<box>& result) {
	bool valid = true;
	auto total_area = boost::geometry::area(minuend);
	box intersection;
	boost::geometry::intersection(minuend,subtrahend,intersection);
	total_area -= boost::geometry::area(intersection);
	for(int i=0;i<result.size();++i) {
		const auto& r = result[i];
		total_area -= boost::geometry::area(r);
		valid = valid && boost::geometry::within(r,minuend);
		boost::geometry::intersection(r,subtrahend,intersection);
		valid = valid && (boost::geometry::area(intersection)==0);
		for(int j=i+1;j<result.size();++j) {
			const auto& r2 = result[j];
			boost::geometry::intersection(r,r2,intersection);
			valid = valid && (boost::geometry::area(intersection)==0);
		}
	}

	return valid && total_area == 0;
}

int main(int, char **)
{
	std::vector<box> result;
	box a(point(0, 0), point(10, 10)), b(point(3, 3), point(7, 7));
	boost::geometry::difference(a,b,result);
	std::cout << "BOX(0 0, 10 10)\\BOX(3 3, 7 7)\n\n";
	for(const auto &r : result)
	{
		std::cout << "BOX( " << boost::geometry::get<min_corner, 0>(r) << " " << boost::geometry::get<min_corner, 1>(r) << " , "
			<< boost::geometry::get<max_corner, 0>(r) << " " << boost::geometry::get<max_corner, 1>(r) << " )\n";
	}
	std::cout << "correct? " << (difference_correct(a,b,result)?"yes":"no") << "\n";
	return 0;
}


/*
 * Copyright (c) 2014-2015 Kartik Kumar, Dinamica Srl
 * Copyright (c) 2014-2015 Marko Jankovic, DFKI GmbH
 * Copyright (c) 2014-2015 Natalia Ortiz, University of Southampton
 * Copyright (c) 2014-2015 Juan Romero, University of Strathclyde
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#include <cmath>
#include <vector>

#include <catch.hpp>

#include <Eigen/Core>

#include "astro/centralBodyAccelerationModel.hpp"

namespace astro
{
namespace tests
{

typedef double Real;
typedef Eigen::Matrix< Real, Eigen::Dynamic, 1> Vector;

TEST_CASE( "Compute acceleration vector for a GEO spacecraft using a custom made MATLAB script",
           "[central_gravity, acceleration, models]" )
{
    // Benchmark values for test case obtained using custom MATLAB script: https://goo.gl/8BQztN

    // Set expected acceleration vector [km s^-2].
    Vector expectedAcceleration( 3 );
    expectedAcceleration[ 0 ] = -2.242096133923724e-4;
    expectedAcceleration[ 1 ] =  0.0;
    expectedAcceleration[ 2 ] =  0.0;

     // Set tolerance = error between expected value and computed value.
    const Real tolerance = 1.0e-15;

    // Set value of gravitational parameter of central body [km^3 s^-2].
    // In this case the used value is that of the planet Earth (Wertz, 1999; page 132).
    const Real gravitationalParameter = 3.986005e5;

    // Set position vector of the spacecraft relative to the origin of the reference frame [km].
    // In the following example the Earth-fixed reference frame is used ((Wertz, 1999; page 96).
    // Moreover, the spacecraft is assumed to be in positioned in Geostationary Orbit
    // (GEO) and on the Greenwich Meridian.
    Vector positionVector( 3 );
    positionVector[ 0 ] = 4.2164e4;
    positionVector[ 1 ] = 0.0;
    positionVector[ 2 ] = 0.0;

    // Compute the acceleration vector [km s^-2].
    const Vector computedAcceleration = computeCentralBodyAcceleration( gravitationalParameter,
                                                                        positionVector );

    // Check if computed acceleration matches expected values.
    REQUIRE( computedAcceleration[ 0 ]
             == Approx( expectedAcceleration[ 0 ] ).epsilon( tolerance ) );
    REQUIRE( computedAcceleration[ 1 ]
             == Approx( expectedAcceleration[ 1 ] ).epsilon( tolerance ) );
    REQUIRE( computedAcceleration[ 2 ]
             == Approx( expectedAcceleration[ 2 ] ).epsilon( tolerance ) );
}

TEST_CASE( "Compute acceleration vector for arbitrary case using Tudat library values",
           "[central_gravity, acceleration, models]" )
{
    // Test case computed using the data of the Tudat library calculated using the planet Mercury as
    // a central body. More info can be found at the following link: http://goo.gl/MyXBJE.

    // Set expected acceleration vector [m s^-2].
    Vector expectedAcceleration( 3 );
    expectedAcceleration[ 0 ] = -6.174552714649318e-2;
    expectedAcceleration[ 1 ] =  3.024510782481964e-1;
    expectedAcceleration[ 2 ] = -1.228994266291893e-1;

     // Set tolerance = error between expected value and computed value.
    const Real tolerance = 1.0e-15;

    // Set value of gravitational parameter of central body [m^3 s^-2].
    const Real gravitationalParameter = 2.2032e13;

    // Set position vector of the body relative to the origin of the reference frame [m].
    Vector positionVector( 3 );
    positionVector[ 0 ] =  1513.3e3;
    positionVector[ 1 ] = -7412.67e3;
    positionVector[ 2 ] =  3012.1e3;

    // Compute the acceleration vector [m s^-2].
    const Vector computedAcceleration = computeCentralBodyAcceleration( gravitationalParameter,
                                                                        positionVector);

    // Check if computed acceleration matches expected values.
    REQUIRE( computedAcceleration[ 0 ]
             == Approx( expectedAcceleration[ 0 ] ).epsilon( tolerance ) );
    REQUIRE( computedAcceleration[ 1 ]
             == Approx( expectedAcceleration[ 1 ] ).epsilon( tolerance ) );
    REQUIRE( computedAcceleration[ 2 ]
             == Approx( expectedAcceleration[ 2 ] ).epsilon( tolerance ) );
}

} // namespace tests
} // namespace astro

/*
 * Wertz, J.R. & Larson, W.J., Space mission analysis and design, Springer Netherlands
 */


#include "basic_functions.h"
#include "mlfe/device_context/cpu_context.h"
#include <Eigen/Dense>

namespace mlfe{ namespace math{

template <>
void squared_difference<float, CPUContext>(const int size,
                                           const float *x1_ptr,
                                           const float *x2_ptr,
                                           float *y_ptr){
    for(int n = 0; n < size; ++n){
        y_ptr[n] = std::pow(x1_ptr[n] - x2_ptr[n], 2);
    }
}

template <>
void squared_difference<double, CPUContext>(const int size,
                                            const double *x1_ptr,
                                            const double *x2_ptr,
                                            double *y_ptr){
    for(int n = 0; n < size; ++n){
        y_ptr[n] = std::pow(x1_ptr[n] - x2_ptr[n], 2);
    }
}

template <>
void rowwise_max<float, CPUContext>(const int m,
                                    const int n,
                                    const float *a_ptr,
                                    float *b_ptr
                                    )
{
    Eigen::Map<Eigen::VectorXf>(b_ptr, m) =
    Eigen::Map<const Eigen::MatrixXf>(a_ptr, n, m).colwise().maxCoeff();
}

template <>
void rowwise_max<double, CPUContext>(const int m,
                                     const int n,
                                     const double *a_ptr,
                                     double *b_ptr
                                     )
{
    Eigen::Map<Eigen::VectorXd>(b_ptr, m) =
    Eigen::Map<const Eigen::MatrixXd>(a_ptr, n, m).colwise().maxCoeff();
}

template <>
void rowwise_normalize<float, CPUContext>(const int m, const int n,
                                          const float *scaler_ptr,
                                          float *norm_dest
                                          )
{
    for(int i = 0; i < m; ++i){
        for(int j = 0; j < n; ++j){
            norm_dest[i * n + j] /= scaler_ptr[i];
        }
    }
}

template <>
void rowwise_normalize<double, CPUContext>(const int m,
                                           const int n,
                                           const double *scaler_ptr,
                                           double *norm_dest
                                           )
{
    for(int i = 0; i < m; ++i){
        for(int j = 0; j < n; ++j){
            norm_dest[i * n + j] /= scaler_ptr[i];
        }
    }
}

template<>
void exp<float, CPUContext>(const int size,
                            const float *x_ptr,
                            float *y_ptr
                           )
{
    Eigen::Map<Eigen::VectorXf>(y_ptr, size) =
    Eigen::Map<const Eigen::VectorXf>(x_ptr, size).array().exp();
}

template<>
void exp<double, CPUContext>(const int size,
                             const double *x_ptr,
                             double *y_ptr
                            )
{
    Eigen::Map<Eigen::VectorXd>(y_ptr, size) =
    Eigen::Map<const Eigen::VectorXd>(x_ptr, size).array().exp();
}

template<>
void axpy<float, CPUContext>(int size,
                             const float alpha,
                             const float *x_ptr,
                             float *y_ptr
                            )
{
    Eigen::Map<Eigen::VectorXf>(y_ptr, size) +=
    alpha * Eigen::Map<const Eigen::VectorXf>(x_ptr, size);
}

template<>
void axpy<double, CPUContext>(int size,
                              const double alpha,
                              const double *x_ptr,
                              double *y_ptr
                             )
{
    Eigen::Map<Eigen::VectorXd>(y_ptr, size) +=
    alpha * Eigen::Map<const Eigen::VectorXd>(x_ptr, size);
}

template <>
void scal<float, CPUContext>(const int size,
                             const float alpha,
                             const float *x_ptr,
                             float *y_ptr
                            )
{
    Eigen::Map<Eigen::VectorXf> y(y_ptr, size);
    if(alpha != 0.f){
        y = alpha * Eigen::Map<const Eigen::VectorXf>(x_ptr, size);
    }
    else{
        y.setZero();
    }
}

template <>
void scal<double, CPUContext>(const int size,
                              const double alpha,
                              const double *x_ptr,
                              double *y_ptr
                             )
{
    Eigen::Map<Eigen::VectorXd> y(y_ptr, size);
    if(alpha != 0.){
        y = alpha * Eigen::Map<const Eigen::VectorXd>(x_ptr, size);
    }
    else{
        y.setZero();
    }
}

template <>
void sum<float, CPUContext>(
                            const int size,
                            const float *x_ptr,
                            float *y_ptr
                           )
{
    y_ptr[0] = Eigen::Map<const Eigen::VectorXf>(x_ptr, size).sum();
}

template <>
void sum<double, CPUContext>(
                             const int size,
                             const double *x_ptr,
                             double *y_ptr
                            )
{
    y_ptr[0] = Eigen::Map<const Eigen::VectorXd>(x_ptr, size).sum();
}

template<>
void set<float, CPUContext>(
                            const int size,
                            const float val,
                            float *x_ptr
                           )
{
    Eigen::Map<Eigen::VectorXf>(x_ptr, size).setConstant(val);
}

template<>
void set<double, CPUContext>(const int size,
                             const double val,
                             double *x_ptr
                             )
{
    Eigen::Map<Eigen::VectorXd>(x_ptr, size).setConstant(val);
}

} // end namespace math
} // end namespace mlfe


#ifndef _NB_SAMPLE_HPP
#define _NB_SAMPLE_HPP

#include <vector>
#include <cmath>
#include <numeric>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/digamma.hpp>

#include <dlib/optimization.h>

#include "misc.hpp"

typedef dlib::matrix<double,0,1> column_vector;
using namespace std::placeholders;

class NBModelC {

    public:
        NBModelC (std::vector<long> lvec) : sample_vec(lvec){

        }

        double value(const column_vector &m) {

            double r_val = m(0);
            double lmean = do_mean(sample_vec);
            double lpval = lmean / (lmean + r_val);
            double total_ll = 0;

            for (int j_ind = 0; j_ind < sample_vec.size(); j_ind++) {
                double xi_val = (double)sample_vec[j_ind];

                double lval1 = lgamma(xi_val + r_val) -lgamma(xi_val + 1) -
                    lgamma(r_val) + xi_val * log (lpval) +
                    r_val * log(1 - lpval);
                total_ll += lval1;
            }

            return (total_ll);
        }

        const column_vector gradient(const column_vector &m) {

            double r_val = m(0);
            double lmean = do_mean(sample_vec);
            double lpval = lmean / (lmean + r_val);

            double total_derivative = 0;

            for (int j_ind = 0; j_ind < sample_vec.size(); j_ind++) {
                double xi_val = sample_vec[j_ind];

                double lval1 = boost::math::digamma(xi_val + r_val) -
                    boost::math::digamma(r_val) + log(1-lpval);

                total_derivative += lval1;
            }
            const column_vector final_grad = {total_derivative};
            return final_grad;

        }

        double get_r_val_0 () {
            int n = sample_vec.size();
            double lmean = do_mean(sample_vec);

            double lsum = 0;
            for (int j = 0; j < n; j++) {
                lsum += pow((sample_vec[j] - lmean), 2.0);
            }
            double lvar = lsum / (n-1);

            double r_val_0 = 0;
            if (lvar > lmean) {
                r_val_0 = pow(lmean, 2.0) / (lvar - lmean);
            } else {
                r_val_0 = 100;
            }
            return r_val_0;
        }

        double find_r_val() {

            if (r_val_ready) {
                return (final_r_val);
            }

            auto value_funct = std::bind(&NBModelC::value, this, _1);
            auto gradient_funct = std::bind(&NBModelC::gradient, this, _1);
            double r_val_0 = get_r_val_0();
            column_vector starting_point = {r_val_0};
            const column_vector x_lower1 = {0.0000001};
            const column_vector x_upper1 = {1000000};

            find_max_box_constrained(dlib::bfgs_search_strategy(),
                                     dlib::objective_delta_stop_strategy(1e-9),
                                     value_funct, gradient_funct,
                                    starting_point,
                                    x_lower1, x_upper1);

            final_r_val = starting_point(0);
            r_val_ready = true;
            return final_r_val;
        }

        double find_p_val() {

            if (p_val_ready) {
                return (final_p_val);
            }
            double lrval = find_r_val();

            double lmean = do_mean(sample_vec);
            final_p_val = lmean / (lmean + lrval);
            p_val_ready = true;
            return (final_p_val);

        }

        double find_mean_val() {

            double lmean = do_mean(sample_vec);
            return (lmean);
        }

    private:
        std::vector<long> sample_vec;
        double final_p_val;
        double final_r_val;
        bool p_val_ready = false;
        bool r_val_ready = false;

};

#endif


#define BOOST_TEST_MODULE math_test_module
#include <boost/test/unit_test.hpp>
#include <vector>
#include <iostream>

#include "math_lib.h"

using namespace std;
namespace tt = boost::test_tools;
namespace utf = boost::unit_test;

/*
template <typename T>
std::ostream& operator<<(std::ostream& out, std::vector<T> vec) {
    for (const auto& v : vec) {
        out << v << " ";
    }
    return out;
}
*/

BOOST_AUTO_TEST_SUITE(math_lib_test_suite)

    BOOST_AUTO_TEST_CASE(test_factorial) {
        // integer
        BOOST_TEST(factorial(0) == 1);
        BOOST_TEST(factorial(1) == 1);
        BOOST_TEST(factorial(2) == 2);
        BOOST_TEST(factorial(3) == 6);
        BOOST_TEST(factorial(10) == 3628800);
        BOOST_TEST(factorial(-1) == 1);
        // double
        BOOST_TEST(factorial(0.5) == 1);
        BOOST_TEST(factorial(1.5) == 1.5);
        BOOST_TEST(factorial(2.5) == 3.75);
    }

    BOOST_AUTO_TEST_CASE(test_round_to, * utf::tolerance(0.000001)) {
        BOOST_TEST(roundTo(125, 1) == 130);
        BOOST_TEST(roundTo(125, 2) == 100);
        BOOST_TEST(roundTo(125.456, 1) == 130);
        BOOST_TEST(roundTo(125.456, 0) == 125);
        BOOST_TEST(roundTo(125.456, -1) == 125.5);
        // less one
        BOOST_TEST(roundTo(0.416, 1) == 0);
        BOOST_TEST(roundTo(0.416, 0) == 0);
        BOOST_TEST(roundTo(0.416, -1) == 0.4);
    }

    BOOST_AUTO_TEST_CASE(test_odd) {
        BOOST_TEST(!odd(2));
        BOOST_TEST(odd(1));
        BOOST_TEST(odd(3));
        BOOST_TEST(!odd(0));
        BOOST_TEST(odd(-1));
    }

    BOOST_AUTO_TEST_CASE(test_average_1) {
        // contract: double average(double x, double y);
        BOOST_TEST(average(2, 4) == 3);
        BOOST_TEST(average(2, 5) == 3.5);
        BOOST_TEST(average(0, 1) == 0.5);
        BOOST_TEST(average(-1, 1) == 0);
        BOOST_TEST(average(2.5, 0.5) == 1.5);
    }

    BOOST_AUTO_TEST_CASE(test_average_2) {
        // contract: double average(const std::vector<double>& vals);
        BOOST_TEST(average({2, 4}) == 3);
        BOOST_TEST(average({2, 5}) == 3.5);
        BOOST_TEST(average({0, 1}) == 0.5);
        BOOST_TEST(average({-1, 1}) == 0);
        BOOST_TEST(average({2.5, 0.5}) == 1.5);
        BOOST_TEST(average({-2, 2, 3}) == 1);
    }

    BOOST_AUTO_TEST_CASE(test_average_3, * utf::tolerance(0.000001)) {
        // contract: void average(const std::vector<double>& vals, double &mean, double &err);
        {const vector<double> input {4, 5, 7, 2};
            double mean, err;
            average(input, mean, err);
            BOOST_TEST(mean == 4.5);
            BOOST_TEST(err == 1.04083299973307);
        }
        {const vector<double> input {2, 5};
            double mean, err;
            average(input, mean, err);
            BOOST_TEST(mean == 3.5);
            BOOST_TEST(err == 1.5);
        }
    }

    BOOST_AUTO_TEST_CASE(test_sqr_add, * utf::tolerance(0.000001)) {
        // contract: double sqrAdd(const std::vector<double>& vals);
        BOOST_TEST(sqrAdd({3, 4}) == 5);
        BOOST_TEST(sqrAdd({3, 4, 5}) == sqrt(50));
        BOOST_TEST(sqrAdd({7.1, 3.2, 78.5, 0.56}) == 78.8873475279781);
    }

    BOOST_AUTO_TEST_CASE(test_poly) {
        // f(x) = 2*x^2 + x + 3.5; f(0)
        const vector<double> coeffs = {3.5, 1, 2};
        BOOST_TEST(poly(0, coeffs) == 3.5);
        BOOST_TEST(poly(1, coeffs) == 6.5);
        BOOST_TEST(poly(-1, coeffs) == 4.5);
    }

    BOOST_AUTO_TEST_CASE(test_calculate_pdf) {
        // weights: 1, 1, 1, 1 => pdf: 0.25, 0.5, 0.75, 1 and sum = 4
        {
            vector<double> weights = {1, 1, 1, 1};
            auto sum = calculate_pdf(weights);
            BOOST_TEST(sum == 4);
            const vector<double> res = {0.25, 0.5, 0.75, 1};
            BOOST_TEST(weights == res);
        }
        {
            vector<double> weights = {1};
            auto sum = calculate_pdf(weights);
            BOOST_TEST(sum == 1);
            const vector<double> res = {1};
            BOOST_TEST(weights == res);
        }
        {
            vector<double> weights = {0.5, 2, 1};
            auto sum = calculate_pdf(weights);
            BOOST_TEST(sum == 3.5);
            const vector<double> res = {5./35, 25./35, 1};
            BOOST_TEST(weights == res);
        }
        // test with predicate: e.g. cont with struct a.weight:
        {
            struct Tmp {double weight = 0;};
            vector<Tmp> weights = {Tmp{0.5}, Tmp{2}, Tmp{1}};
            vector<double> res;
            auto sum = calculate_pdf(weights, [](const Tmp& t){ return t.weight;}, res);
            BOOST_TEST(sum == 3.5);
            const vector<double> expected = {5./35, 25./35, 1};
            BOOST_TEST(res == expected);
        }
    }

BOOST_AUTO_TEST_SUITE_END()


#include <stdexcept>

#include <boost/math/constants/constants.hpp>
#include <Eigen/Eigenvalues>
#include <Euclid/Math/Statistics.h>
#include <Euclid/Math/Vector.h>

namespace Euclid
{

template<typename Kernel>
void OBB<Kernel>::build(const std::vector<FT>& positions)
{
    if (positions.empty()) {
        throw std::invalid_argument("Input is empty.");
    }
    if (positions.size() % 3 != 0) {
        throw std::runtime_error("Size of input is not divisble by 3.");
    }

    Eigen::Map<const Eigen::
                   Matrix<FT, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
        v(positions.data(), positions.size() / 3, 3);
    _build(v);
}

template<typename Kernel>
template<typename ForwardIterator, typename VPMap>
void OBB<Kernel>::build(ForwardIterator first,
                        ForwardIterator beyond,
                        VPMap vpmap)
{
    std::vector<FT> positions;
    while (first != beyond) {
        positions.push_back(get(vpmap, *first).x());
        positions.push_back(get(vpmap, *first).y());
        positions.push_back(get(vpmap, *first).z());
        ++first;
    }
    build(positions);
}

template<typename Kernel>
template<typename Derived>
void OBB<Kernel>::build(const Eigen::MatrixBase<Derived>& v)
{
    if (v.rows() == 0) {
        throw std::invalid_argument("Input is empty.");
    }

    _build(v);
}

template<typename Kernel>
typename OBB<Kernel>::Point_3 OBB<Kernel>::center() const
{
    return _center;
}

template<typename Kernel>
typename OBB<Kernel>::Vector_3 OBB<Kernel>::axis(int n) const
{
    if (n < 0 || n >= 3) {
        throw std::invalid_argument("Invalid argument for OBB::length(int).");
    }
    return Euclid::normalized(_directions[n]);
}

template<typename Kernel>
typename OBB<Kernel>::FT OBB<Kernel>::length(int n) const
{
    if (n < 0 || n >= 3) {
        throw std::invalid_argument("Invalid argument for OBB::length(int).");
    }
    return Euclid::length(_directions[n]) * 2;
}

template<typename Kernel>
template<typename Derived>
void OBB<Kernel>::_build(const Eigen::MatrixBase<Derived>& points)
{
    using namespace boost::math::constants;

    // Conduct pca by eigen decompose the covariance matrix of points
    Eigen::Matrix<FT, 3, 3> covariance;
    Euclid::covariance_matrix(points, covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<FT, 3, 3>> eigs;
    eigs.compute(covariance);
    if (eigs.info() != Eigen::Success) {
        throw std::runtime_error("PCA no convergence.");
    }

    // Unit lengh, sorted in descending order w.r.t. eigen values
    Euclid::eigen_to_cgal(eigs.eigenvectors().col(2), _directions[0]);
    Euclid::eigen_to_cgal(eigs.eigenvectors().col(1), _directions[1]);
    Euclid::eigen_to_cgal(eigs.eigenvectors().col(0), _directions[2]);

    // Transform points to pca coordinate system
    Eigen::Matrix<FT, Eigen::Dynamic, 3> tpoints = points * eigs.eigenvectors();
    Eigen::Matrix<FT, Eigen::Dynamic, 1> tmax = tpoints.colwise().maxCoeff();
    Eigen::Matrix<FT, Eigen::Dynamic, 1> tmin = tpoints.colwise().minCoeff();

    // Record center and length of box
    _center = CGAL::ORIGIN + half<FT>() * (tmax(2) + tmin(2)) * _directions[0] +
              half<FT>() * (tmax(1) + tmin(1)) * _directions[1] +
              half<FT>() * (tmax(0) + tmin(0)) * _directions[2];
    _directions[0] *= half<FT>() * (tmax(2) - tmin(2));
    _directions[1] *= half<FT>() * (tmax(1) - tmin(1));
    _directions[2] *= half<FT>() * (tmax(0) - tmin(0));
}

} // namespace Euclid


#include <ceres/ceres.h>
#include <Eigen/Dense>
#include <fstream>
#include <string>
#include <thread>
#include <random>
#include "pose_estimator.h"
#include "re_projection_error.h"
#include "timer.h"
#include "type.h"
#include "viewer/viewer.h"

using ceres::Problem;
using ceres::AutoDiffCostFunction;
using ceres::SizedCostFunction;

Vec6d isoMat2Vector(const Eigen::Matrix4d pose) {
  Eigen::Matrix3d rotation = pose.block(0, 0, 3, 3);
  Eigen::AngleAxisd angle_axis(rotation);
  Eigen::Vector3d rv = angle_axis.angle() * angle_axis.axis();
  Vec6d v;
  v << rv.x(), rv.y(), rv.z(), pose(0, 3), pose(1, 3), pose(2, 3);
  return v;
}

Eigen::Matrix4d vector2IsoMat(const Vec6d &v) {
  double angle = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
  Eigen::Vector3d axis(v[0], v[1], v[2]);
  axis /= angle;
  Eigen::AngleAxisd angle_axis(angle, axis);

  Eigen::MatrixX4d pose = Eigen::Matrix4d::Identity();
  pose.block(0, 0, 3, 3) = angle_axis.toRotationMatrix();

  pose(0, 3) = v[3];
  pose(1, 3) = v[4];
  pose(2, 3) = v[5];
  return pose;
}

void addNoise(Vec6d &v, double rotation_noise, double translation_noise) {
  const double mean = 0.0;
  std::default_random_engine generator;
  std::normal_distribution<double> dist_rotation(mean, rotation_noise);
  std::normal_distribution<double> dist_translation(mean, translation_noise);
  for (int i = 0; i < 3; i++) {
    v[i] += dist_rotation(generator);
    v[i + 3] += dist_translation(generator);
  }
}

int main(int argc, char **argv) {
  if (argc != 3) {
    std::cout << " args error " << std::endl;
    return 1;
  }
  bool use_analytic = false;

  std::cout << kColorGreen << (use_analytic ? " Use Analytic Derivatives "
                                            : " Use Automatic Derivatives")
            << kColorReset << std::endl;

  std::string str_map_points = std::string(argv[1]);
  std::string str_observation = std::string(argv[2]);

  Mat44d ground_truth_pose;
  ground_truth_pose << -0.38773203, -0.29730779, 0.872509, -1.8075688,
      0.29847804, 0.85506284, 0.42400289, 0.0065075331, -0.87210935, 0.42482427,
      -0.24279539, 0.75284511, 0, 0, 0, 1;

  auto init_parameter = isoMat2Vector(ground_truth_pose);

  // add noise to ground_truth
  addNoise(init_parameter, 0.1, 0.2);

  // Setup Viewer

  Viewer viewer;
  viewer.LoadMappoints(str_map_points);
  viewer.SetGroundTruthPose(ground_truth_pose);
  viewer.SetInitialPose(vector2IsoMat(init_parameter));
  std::thread *viewer_thread = new std::thread(&Viewer::Run, &viewer);
  viewer_thread->detach();

  // start solving pose estimation problem

  std::shared_ptr<PoseEstimationProblem> pose_estimation =
      std::make_shared<PoseEstimationProblem>();
  pose_estimation->loadFile(str_observation);
  pose_estimation->setInitialParameter(init_parameter.data());

  ceres::Problem problem;

  // camera intrinsic parameters
  double fx = 458.65399169921875;
  double fy = 457.29598999023438;
  double cx = 367.21499633789062;
  double cy = 248.375;
  CameraInt cam_int(fx, fy, cx, cy);

  if (use_analytic) {
    problem.AddParameterBlock(pose_estimation->mutable_camera_pose(), 6,
                              new PoseSE3Parameterization());
  } else {
    problem.AddParameterBlock(pose_estimation->mutable_camera_pose(), 6);
  }
  for (int i = 0; i < pose_estimation->num_observations(); ++i) {
    auto point = pose_estimation->point_position_for_observation(i);
    auto observation = pose_estimation->observation(i);

    if (use_analytic) {
      ceres::CostFunction *cost_function =
          new ReprojectionErrorSE3(cam_int, observation.x(), observation.y(),
                                   point.x(), point.y(), point.z());
      problem.AddResidualBlock(cost_function, NULL,
                               pose_estimation->mutable_camera_pose());
    } else  // auto-diff
    {
      ceres::CostFunction *cost_function =
          ReprojectionError::create(cam_int, observation.x(), observation.y(),
                                    point[0], point[1], point[2]);
      problem.AddResidualBlock(cost_function, nullptr /* squared loss */,
                               pose_estimation->mutable_camera_pose());
    }
  }

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";

  // display optimized result
  auto final_result = pose_estimation->mutable_camera_pose();
  Eigen::Map<Vec6d> final_result_vector = Eigen::Map<Vec6d>(final_result);
  viewer.SetEstimatePose(vector2IsoMat(final_result_vector));

  while (true) {
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  }

  return 1;
}