/**
 * @file Cone.hpp
 * @author A. Domahidi [domahidi@embotech.com]
 * @license (new license) License BSD-3-Clause
 * @copyright Copyright (c) [2012-2015] Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
 * @date 2012-2015
 * @brief ECOS - Embedded Conic Solver
 *
 * Modified to c++ code by New York University and Max Planck Gesellschaft, 2017
 */

#pragma once

#include <memory>
#include <vector>
#include <iostream>
#include <Eigen/Sparse>
#include <solver/interface/SolverSetting.hpp>

namespace solver {

  class Cone;
  class Vector;
  class ConicVector;
  class ExtendedVector;
  class OptimizationVector;

  class ScalingOperator
  {
    public:
	  ScalingOperator(){}
	  ~ScalingOperator(){}

	  void initialize(const Cone& cone) { cone_ = &cone; }
	  ConicVector operator*(const Eigen::Ref<const Eigen::VectorXd>& rhs) const;

    private:
	  const Cone* cone_;
  };

  /**
   * This class performs appropriate scalings of the cone variables.
   * Details can be found in the paper: Domahidi, A. and Chu, E. and Boyd, S.,
   *     ECOS: An SOCP solver for embedded systems, ECC 2013, pages 3071-3076
   */
  class NesterovToddScaling
  {
    public:
	  friend class Cone;

	  NesterovToddScaling(int conesize);
	  ~NesterovToddScaling(){}

	  inline double& w() { return w_; }
	  inline double& d1() { return d1_; }
	  inline double& u0() { return u0_; }
	  inline double& u1() { return u1_; }
	  inline double& v1() { return v1_; }
	  inline double& eta() { return eta_; }
	  inline double& etaSquare() { return eta_square_; }
	  int& indexSoc(int id) { return SOC_id_(id); }
	  Eigen::VectorXd& scalingSoc() { return wbar_; }
	  double& scalingSoc(int id) { return wbar_(id); }

	  inline const double& w() const { return w_; }
	  inline const double& d1() const { return d1_; }
	  inline const double& u0() const { return u0_; }
	  inline const double& u1() const { return u1_; }
	  inline const double& v1() const { return v1_; }
	  inline const double& eta() const { return eta_; }
	  inline const double& etaSquare() const { return eta_square_; }
	  const int& indexSoc(int id) const { return SOC_id_(id); }
	  const Eigen::VectorXd& scalingSoc() const { return wbar_; }
	  const double& scalingSoc(int id) const { return wbar_(id); }

    private:
	  Eigen::VectorXd wbar_;
	  Eigen::VectorXi SOC_id_;
	  double w_, d1_, u0_, u1_, v1_, eta_, eta_square_;
  };

  /**
   * This class contains all information about the conic optimization
   * problem, and provides functionality for conic operations, such as
   * scalings, residuals and sizes.
   */
  class Cone
  {
    public:
	  // Cone class
	  Cone(){}
	  ~Cone(){}

	  // Setting up problem
	  void setupLpcone(int size);
	  void setupSocone(const Eigen::VectorXi& indices);
	  void initialize(int nvars, int nleq, int nlineq, const Eigen::VectorXi& nsoc);

	  // Getter and setter methods for problem size variables
	  inline const int numLeq() const { return neq_; }
	  inline const int numSoc() const { return nsoc_; }
	  inline const int sizeSoc() const { return ssoc_; }
	  inline const int sizeLpc() const { return nineq_; }
	  inline const int numVars() const { return nvars_; }
	  inline const int sizeCone() const { return sizecone_; }
	  inline const int extSizeSoc() const { return extssoc_; }
	  inline const int sizeSoc(int id) const { return q_(id); }
	  inline const int numCones() const { return nineq_+nsoc_; }
	  inline const int sizeProb() const { return sizeproblem_; }
	  inline const int extSizeCone() const { return extsizecone_; }
	  inline const int lpConeStart() const { return lpconestart_; }
	  inline const int soConeStart() const { return soconestart_; }
	  inline const int extSizeProb() const { return extsizeproblem_; }
	  inline const int startSoc(int id) const { return SOC_start_(id); }
	  inline const int sizeConstraints() const { return sizeconstraints_; }
	  inline const int optStartSoc(int id) const { return opt_SOC_start_(id); }
	  inline const int extStartSoc(int id) const { return ext_SOC_start_(id); }
	  static double normSoc(const Eigen::Ref<const Eigen::VectorXd>& v) { return v[0]-v.tail(v.size()-1).norm(); }

	  // Getter and setter methods for variables of linear cone
	  int& indexLpc(int id) { return LP_id_(id); }
	  Eigen::VectorXd& scalingLpc() { return LP_scaling_; }
	  double& scalingLpc(int id) { return LP_scaling_(id); }
	  Eigen::VectorXd& sqScalingLpc() { return LP_sq_scaling_; }
	  double& sqScalingLpc(int id) { return LP_sq_scaling_(id); }

	  const int& indexLpc(int id) const { return LP_id_(id); }
	  const Eigen::VectorXd& scalingLpc() const { return LP_scaling_; }
	  const double& scalingLpc(int id) const { return LP_scaling_(id); }
	  const Eigen::VectorXd& sqScalingLpc() const { return LP_sq_scaling_; }
	  const double& sqScalingLpc(int id) const { return LP_sq_scaling_(id); }

	  // Getter and setter methods for variables of second order cone
	  NesterovToddScaling& soc(int id) { return soc_[id]; }
	  const NesterovToddScaling& soc(int id) const { return soc_[id]; }
	  inline double conicResidual(const double* u, int size) const;
	  inline double conicResidual(const double* u, const double* v, int size) const;
	  double conicResidual(const Eigen::Ref<const Eigen::VectorXd>& u) const;
	  double conicResidual(const Eigen::Ref<const Eigen::VectorXd>& u, const Eigen::Ref<const Eigen::VectorXd>& v) const;

	  // Functions for cones
	  double safeDivision(double x, double y) const;
	  void conicProjection(Eigen::Ref<Eigen::VectorXd> s);
	  void conicNTScaling(const double* z, double* lambda) const;
	  void conicNTScaling(const Eigen::VectorXd& z, Eigen::Ref<Eigen::VectorXd> lambda) const;
	  void conicNTScaling2(const Eigen::VectorXd& x, Eigen::Ref<Eigen::VectorXd> y) const;
	  void conicDivision(const Eigen::Ref<const Eigen::VectorXd>& u, const Eigen::Ref<const Eigen::VectorXd>& w, Eigen::Ref<Eigen::VectorXd> v) const;
	  double conicProduct(const Eigen::Ref<const Eigen::VectorXd> u, const Eigen::Ref<const Eigen::VectorXd> v, Eigen::Ref<Eigen::VectorXd> w) const;
	  ConeStatus updateNTScalings(const Eigen::VectorXd& s, const Eigen::VectorXd& z, Eigen::VectorXd& lambda);
	  void unpermuteSolution(const Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic>& Pinv, const Eigen::VectorXd& Px, OptimizationVector& sd) const;
	  void unpermuteSolution(const Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic>& Pinv, const Eigen::VectorXd& Px, OptimizationVector& sd, Eigen::VectorXd& dz) const;

	  // Getter and setter methods for scaling operators
	  ScalingOperator& W() { return W_scaling_operator_; }
	  const ScalingOperator& W() const { return W_scaling_operator_; }

    private:
	  int nvars_;           // number variables
	  int neq_;             // number linear equality constraints
	  int nineq_;           // number linear inequality constraints
	  int nsoc_;            // number second order cone constraints
	  int ssoc_;            // size second order cone constraints
	  int extssoc_;         // extended size second order cone constraints
	  int lpconestart_;     // offset to linear cone start
	  int soconestart_;     // offset to second order cone start
	  int sizecone_;        // size of linear and second order cones
	  int extsizecone_;     // extended size of linear and second order cones
	  int sizeconstraints_; // size of the constraints
	  int sizeproblem_;     // size of number of variables plus constraints
	  int extsizeproblem_;  // size of number of variables plus constraints
	  Eigen::VectorXi q_;   // sizes of second-order cone constraints

	  // variables of LP cone
	  Eigen::VectorXi LP_id_;
	  Eigen::VectorXd LP_scaling_;
	  Eigen::VectorXd LP_sq_scaling_;

	  // variables of SO cone
	  Eigen::VectorXd skbar_, zkbar_;
      std::vector<NesterovToddScaling> soc_;
	  Eigen::VectorXi SOC_start_, opt_SOC_start_, ext_SOC_start_;

	  // Operators
	  ScalingOperator W_scaling_operator_;
  };

  /**
   * Helper class to work with optimization variables, lay down in normal order.
   */
  class Vector : public Eigen::VectorXd
  {
    public:
	  Vector() : Eigen::VectorXd() {}
	  ~Vector(){}

	  template<typename OtherDerived>
	  Vector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::VectorXd(other) {}

	  template<typename OtherDerived>
	  Vector& operator=(const Eigen::MatrixBase <OtherDerived>& other) {
	    this->Eigen::VectorXd::operator=(other);
	    return *this;
	  }

	  void initialize(const Cone& cone);

	  Eigen::Ref<Eigen::VectorXd> x() { return this->head(cone_->numVars()); }
	  Eigen::Ref<Eigen::VectorXd> y() { return this->segment(cone_->numVars(), cone_->numLeq()); }
	  Eigen::Ref<Eigen::VectorXd> z() { return this->segment(cone_->lpConeStart(), cone_->sizeCone()); }
	  Eigen::Ref<Eigen::VectorXd> yz() { return this->segment(cone_->numVars(), cone_->sizeConstraints()); }

	  Eigen::Ref<Eigen::VectorXd> zLpc() { return this->segment(cone_->lpConeStart(), cone_->sizeLpc()); }
	  Eigen::Ref<Eigen::VectorXd> zSoc() { return this->segment(cone_->soConeStart(), cone_->sizeSoc()); }
	  Eigen::Ref<Eigen::VectorXd> zSoc(int id) { return this->segment(cone_->optStartSoc(id), cone_->sizeSoc(id)); }

	  const Eigen::Ref<const Eigen::VectorXd> x() const { return this->head(cone_->numVars()); }
	  const Eigen::Ref<const Eigen::VectorXd> y() const { return this->segment(cone_->numVars(), cone_->numLeq()); }
	  const Eigen::Ref<const Eigen::VectorXd> z() const { return this->segment(cone_->lpConeStart(), cone_->sizeCone()); }
	  const Eigen::Ref<const Eigen::VectorXd> yz() const { return this->segment(cone_->numVars(), cone_->sizeConstraints()); }

	  const Eigen::Ref<const Eigen::VectorXd> zLpc() const { return this->segment(cone_->lpConeStart(), cone_->sizeLpc()); }
	  const Eigen::Ref<const Eigen::VectorXd> zSoc() const { return this->segment(cone_->soConeStart(), cone_->sizeSoc()); }
	  const Eigen::Ref<const Eigen::VectorXd> zSoc(int id) const { return this->segment(cone_->optStartSoc(id), cone_->sizeSoc(id)); }

    private:
	  const Cone* cone_;
  };

  /**
   * Helper class to work with variables, which are members of a proper convex cone.
   */
  class ConicVector : public Eigen::VectorXd
  {
    public:
	  ConicVector() : Eigen::VectorXd() {}
	  ConicVector(int size) : Eigen::VectorXd(size) {}
	  ~ConicVector(){}

	  template<typename OtherDerived>
	  ConicVector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::VectorXd(other) {}

	  template<typename OtherDerived>
	  ConicVector& operator=(const Eigen::MatrixBase <OtherDerived>& other) {
	    this->Eigen::VectorXd::operator=(other);
	    return *this;
	  }

	  ConicVector& operator=(const ConicVector& other) {
	    this->Eigen::VectorXd::operator=(other);
	    this->cone_ = other.cone_;
	    return *this;
	  }

	  void initialize(const Cone& cone);
	  ConicVector  operator/ (const ConicVector& rhs) const;
	  ConicVector  operator* (const ConicVector& rhs) const;
	  ConicVector  operator+ (const ConicVector& rhs) const;
	  ConicVector  operator+ (const double& rhs) const;
	  ConicVector  operator- (const double& rhs) const;
	  ConicVector& operator+=(const double& rhs);
	  ConicVector  operator- () const;

	  Eigen::Ref<Eigen::VectorXd> z() { return this->head(cone_->sizeCone()); }
	  Eigen::Ref<Eigen::VectorXd> zLpc() { return this->head(cone_->sizeLpc()); }
	  Eigen::Ref<Eigen::VectorXd> zSoc() { return this->segment(cone_->sizeLpc(),cone_->sizeSoc()); }
	  Eigen::Ref<Eigen::VectorXd> zSoc(int id) { return this->segment(cone_->startSoc(id),cone_->sizeSoc(id)); }

	  const Eigen::Ref<const Eigen::VectorXd> z() const { return this->head(cone_->sizeCone()); }
	  const Eigen::Ref<const Eigen::VectorXd> zLpc() const { return this->head(cone_->sizeLpc()); }
	  const Eigen::Ref<const Eigen::VectorXd> zSoc() const { return this->segment(cone_->sizeLpc(),cone_->sizeSoc()); }
	  const Eigen::Ref<const Eigen::VectorXd> zSoc(int id) const { return this->segment(cone_->startSoc(id),cone_->sizeSoc(id)); }

    private:
	  const Cone* cone_;
  };

  /**
   * Helper class to work with optimization variables, lay down in an extended order.
   */
  class ExtendedVector : public Eigen::VectorXd
  {
    public:
	  ExtendedVector() : Eigen::VectorXd() {}
	  ~ExtendedVector(){}

	  template<typename OtherDerived>
	  ExtendedVector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::VectorXd(other) {}

	  template<typename OtherDerived>
	  ExtendedVector& operator=(const Eigen::MatrixBase <OtherDerived>& other) {
	    this->Eigen::VectorXd::operator=(other);
	    return *this;
	  }

	  void initialize(const Cone& cone);

	  void zRtoE(const Eigen::Ref<const Eigen::VectorXd>& rvec);
	  Eigen::Ref<Eigen::VectorXd> x() { return this->head(cone_->numVars()); }
	  Eigen::Ref<Eigen::VectorXd> y() { return this->segment(cone_->numVars(),cone_->numLeq()); }
	  Eigen::Ref<Eigen::VectorXd> z() { return this->segment(cone_->lpConeStart(),cone_->extSizeCone()); }

	  Eigen::Ref<Eigen::VectorXd> zLpc() { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
	  Eigen::Ref<Eigen::VectorXd> zSoc() { return this->segment(cone_->soConeStart(),cone_->extSizeSoc()); }
	  Eigen::Ref<Eigen::VectorXd> zSoc(int id) { return this->segment(cone_->extStartSoc(id),cone_->sizeSoc(id)+2); }

	  const Eigen::Ref<const Eigen::VectorXd> x() const { return this->head(cone_->numVars()); }
	  const Eigen::Ref<const Eigen::VectorXd> y() const { return this->segment(cone_->numVars(),cone_->numLeq()); }
	  const Eigen::Ref<const Eigen::VectorXd> z() const { return this->segment(cone_->lpConeStart(),cone_->extSizeCone()); }

	  const Eigen::Ref<const Eigen::VectorXd> zLpc() const { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
	  const Eigen::Ref<const Eigen::VectorXd> zSoc() const { return this->segment(cone_->soConeStart(),cone_->extSizeSoc()); }
	  const Eigen::Ref<const Eigen::VectorXd> zSoc(int id) const { return this->segment(cone_->extStartSoc(id),cone_->sizeSoc(id)+2); }

    private:
	  const Cone* cone_;
  };

  /**
   * Helper class to define an optimization vector, including primal and dual
   * variables, and variables to render the optimization problem homogeneous.
   */
  class OptimizationVector : public Eigen::VectorXd
  {
    public:
	  OptimizationVector() : Eigen::VectorXd() {}
	  ~OptimizationVector(){}

	  template<typename OtherDerived>
	  OptimizationVector(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::VectorXd(other) {}

	  template<typename OtherDerived>
	  OptimizationVector& operator=(const Eigen::MatrixBase <OtherDerived>& other) {
	    this->Eigen::VectorXd::operator=(other);
	    this->cone_ = other.cone_;
	    return *this;
	  }

	  void initialize(const Cone& cone);

	  OptimizationVector operator+(const OptimizationVector& rhs) const;

	  Eigen::Ref<Eigen::VectorXd> x() { return this->head(cone_->numVars()); }
	  Eigen::Ref<Eigen::VectorXd> y() { return this->segment(cone_->numVars(),cone_->numLeq()); }
	  Eigen::Ref<Eigen::VectorXd> z() { return this->segment(cone_->lpConeStart(),cone_->sizeCone()); }
	  Eigen::Ref<Eigen::VectorXd> s() { return this->segment(cone_->sizeProb()+2,cone_->sizeCone()); }
	  Eigen::Ref<Eigen::VectorXd> yz() { return this->segment(cone_->numVars(),cone_->sizeConstraints()); }
	  Eigen::Ref<Eigen::VectorXd> yztau() { return this->segment(cone_->numVars(),cone_->sizeConstraints()+1); }
	  double& tau() { return this->segment<2>(cone_->sizeProb())[0]; }
	  double& kappa() { return this->segment<2>(cone_->sizeProb())[1]; }

	  const Eigen::Ref<const Eigen::VectorXd> x() const { return this->head(cone_->numVars()); }
	  const Eigen::Ref<const Eigen::VectorXd> y() const { return this->segment(cone_->numVars(),cone_->numLeq()); }
	  const Eigen::Ref<const Eigen::VectorXd> s() const { return this->segment(cone_->lpConeStart(),cone_->sizeCone()); }
	  const Eigen::Ref<const Eigen::VectorXd> z() const { return this->segment(cone_->sizeProb()+2,cone_->sizeCone()); }
	  const Eigen::Ref<const Eigen::VectorXd> yz() const { return this->segment(cone_->numVars(),cone_->sizeConstraints()); }
	  const Eigen::Ref<const Eigen::VectorXd> yztau() const { return this->segment(cone_->numVars(),cone_->sizeConstraints()+1); }
	  double tau() const { return this->segment<2>(cone_->sizeProb())[0]; }
	  double kappa() const { return this->segment<2>(cone_->sizeProb())[1]; }

	  Eigen::Ref<Eigen::VectorXd> xyz() { return this->head(cone_->sizeProb()); }
	  Eigen::Ref<Eigen::VectorXd> xyztau() { return this->head(cone_->sizeProb()+1); }
	  Eigen::Ref<Eigen::VectorXd> zLpc() { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
	  Eigen::Ref<Eigen::VectorXd> zSoc() { return this->segment(cone_->soConeStart(),cone_->sizeSoc()); }
	  Eigen::Ref<Eigen::VectorXd> zSoc(int id) { return this->segment(cone_->optStartSoc(id),cone_->sizeSoc(id)); }

	  const Eigen::Ref<const Eigen::VectorXd> xyz() const { return this->head(cone_->sizeProb()); }
	  const Eigen::Ref<const Eigen::VectorXd> xyztau() const { return this->head(cone_->sizeProb()+1); }
	  const Eigen::Ref<const Eigen::VectorXd> zLpc() const { return this->segment(cone_->lpConeStart(),cone_->sizeLpc()); }
	  const Eigen::Ref<const Eigen::VectorXd> zSoc() const { return this->segment(cone_->soConeStart(),cone_->sizeSoc()); }
	  const Eigen::Ref<const Eigen::VectorXd> zSoc(int id) const { return this->segment(cone_->optStartSoc(id),cone_->sizeSoc(id)); }

    private:
	  const Cone* cone_;
  };

  /**
   * Class that provides storage space for the optimization matrices,
   * vectors and variables.
   */
  class SolverStorage
  {
    public:
	  SolverStorage(){}
	  ~SolverStorage(){}

	  double& gTh() { return gTh_; }
	  const double& gTh() const { return gTh_; }

	  Eigen::SparseMatrix<double>& Amatrix() { return A_; }
	  Eigen::SparseMatrix<double>& Gmatrix() { return G_; }
	  Eigen::SparseMatrix<double>& Atmatrix() { return At_; }
	  Eigen::SparseMatrix<double>& Gtmatrix() { return Gt_; }
	  const Eigen::SparseMatrix<double>& Amatrix() const { return A_; }
	  const Eigen::SparseMatrix<double>& Gmatrix() const { return G_; }
	  const Eigen::SparseMatrix<double>& Atmatrix() const { return At_; }
	  const Eigen::SparseMatrix<double>& Gtmatrix() const { return Gt_; }

	  void initializeMatrices();
	  void initialize(Cone& cone, SolverSetting& stgs);
	  void cleanCoeffs() { Acoeffs_.clear(); Gcoeffs_.clear(); }
	  void addCoeff(const Eigen::Triplet<double>& coeff, bool flag_eq = false);

	  Eigen::Ref<Eigen::VectorXd> cbh() { return cbh_; }
	  Eigen::Ref<Eigen::VectorXd> c() { return cbh_.x(); }
	  Eigen::Ref<Eigen::VectorXd> b() { return cbh_.y(); }
	  Eigen::Ref<Eigen::VectorXd> h() { return cbh_.z(); }
	  Eigen::Ref<Eigen::VectorXd> bh() { return cbh_.yz(); }
	  std::vector<Eigen::Triplet<double>>& Acoeffs() { return Acoeffs_; }
	  std::vector<Eigen::Triplet<double>>& Gcoeffs() { return Gcoeffs_; }
	  const Eigen::Ref<const Eigen::VectorXd> cbh() const { return cbh_; }
	  const Eigen::Ref<const Eigen::VectorXd> c() const { return cbh_.x(); }
	  const Eigen::Ref<const Eigen::VectorXd> b() const { return cbh_.y(); }
	  const Eigen::Ref<const Eigen::VectorXd> h() const { return cbh_.z(); }
	  const Eigen::Ref<const Eigen::VectorXd> bh() const { return cbh_.yz(); }
	  const std::vector<Eigen::Triplet<double>>& Acoeffs() const { return Acoeffs_; }
	  const std::vector<Eigen::Triplet<double>>& Gcoeffs() const { return Gcoeffs_; }

	  OptimizationVector& u() { return u_opt_; }
	  OptimizationVector& v() { return v_opt_; }
	  OptimizationVector& ut() { return u_t_opt_; }
	  OptimizationVector& uprev() { return u_prev_opt_; }
	  Vector& cbh_copy() { return cbh_copy_; }
	  const OptimizationVector& u() const { return u_opt_; }
	  const OptimizationVector& v() const { return v_opt_; }
	  const OptimizationVector& ut() const { return u_t_opt_; }
	  const OptimizationVector& uprev() const { return u_prev_opt_; }
	  const Vector& cbh_copy() const { return cbh_copy_; }

    private:
	  double gTh_;
	  Cone* cone_;
	  SolverSetting* stgs_;
	  Vector cbh_, cbh_copy_;
	  Eigen::SparseMatrix<double> A_, At_, G_, Gt_;
	  std::vector<Eigen::Triplet<double>> Acoeffs_, Gcoeffs_;
	  OptimizationVector u_opt_, v_opt_, u_t_opt_, u_prev_opt_;
  };

}


/**
 * @file test_state.cpp
 * @author Miroslav Cibulka
 * @created 12/5/15
 * @copyright Copyright (c) 2015 XXX
 * @brief test_state class
 * @detail
 *
 */

#include <boost/test/unit_test.hpp>

#include "support.hh"

BOOST_AUTO_TEST_SUITE(state_tests)

      BOOST_AUTO_TEST_CASE(regression)
        {
          ExampleState state = ExampleState();
          ExampleState _surroundings = ExampleState();

          _surroundings.setValue(5);
          state << _surroundings;

          _surroundings.setValue(4);
          state << _surroundings;

          _surroundings.setValue(3);
          state << _surroundings;

          BOOST_CHECK(state.getValue() == 0);
          state.renew();
          BOOST_CHECK(state.getValue() == 4);

          // (5 + 4 + 3) / 3 = 4
        }

  BOOST_AUTO_TEST_SUITE_END()


// Header
#include "QuadProgMm.hpp"

// Boost
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

// QuadProgMm
#include "Translate.hpp"

namespace QuadProgMm {
  Solution minimize(const QuadraticExpression& q, const std::vector<Constraint>& constraints) {
    Translation t = translate(q, constraints);

    quadprogpp::Vector<double> x;
    double cost = quadprogpp::solve_quadprog(t.G, t.g0, t.CE, t.ce0, t.CI, t.ci0, x);

    std::map<Variable, double> values;
    {
      int index = 0;
      foreach(Variable v, t.variables) {
        values[v] = x[index];
        ++index;
      }
    }

    return Solution(cost + t.g00, values);
  }

  Solution maximize(const QuadraticExpression& q, const std::vector<Constraint>& constraints) {
    return minimize(-q, constraints);
  }
}


#include<iostream>
#include<vector>
#include<cstdlib>
#include<mpi.h>
#include <sys/time.h>
#include <ctime>
#include <boost/multi_array.hpp>

double getTime() {
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return double(tv.tv_sec)+double(tv.tv_usec)/1000000.0;
}

void mySum(void *in_, void *out_, int *len, MPI_Datatype *dtype) {
  double* in = reinterpret_cast<double*>(in_);
  double* out = reinterpret_cast<double*>(out_);
  MPI_Aint sz,lb;
  MPI_Type_get_extent(*dtype,&lb,&sz);
  for(int i=0, iend=sz/sizeof(double); i<iend; i++)
    out[i] += in[i];
}

using std::cout;
using std::cin;
using std::endl;
using boost::extents;

// run with 4 procs
int main(int argc,char* argv[])
{

  MPI_Init(&argc,&argv);;

  int global_rank, global_size;
  MPI_Comm_size(MPI_COMM_WORLD, &global_size);
  MPI_Comm_rank(MPI_COMM_WORLD, &global_rank);

  MPI_Info info;
  MPI_Comm MPI_COMM_NODE_LOCAL;
  MPI_Comm_split_type(MPI_COMM_WORLD,MPI_COMM_TYPE_SHARED,global_rank,info,&MPI_COMM_NODE_LOCAL);

  int local_rank,local_size;
  MPI_Comm_size(MPI_COMM_NODE_LOCAL, &local_size);
  MPI_Comm_rank(MPI_COMM_NODE_LOCAL, &local_rank);

  MPI_Comm MPI_COMM_GROUP;
  MPI_Comm_split(MPI_COMM_WORLD,local_rank,global_rank,&MPI_COMM_GROUP);

  int rank,size;
  MPI_Comm_size(MPI_COMM_GROUP, &size);
  MPI_Comm_rank(MPI_COMM_GROUP, &rank);

  if(size*local_size != global_size) {
    std::cerr<<" Proc split error. \n";
    exit(1);
  }

  int nw=16;
  int ni=10000/local_size;
  double t1,t2;
  int blk=ni;
  if(argc>1) blk=atoi(argv[1]);
  std::vector<int> nr{1,10,25,50,75};

  boost::multi_array<std::complex<double>,2> A(extents[ni*75][nw*size]);  // 2x2 matrix
  boost::multi_array<std::complex<double>,2> A_(extents[ni*75][nw*size]);  // 2x2 matrix
  boost::multi_array<std::complex<double>,2> B(extents[ni*75][nw]);  // 2x2 matrix
  boost::multi_array<std::complex<double>,2> C(extents[ni*75][nw]);  // 2x2 matrix

  for(int n=0; n<nr.size(); n++) {

  int ni_ = ni*nr[n];
  if(blk > ni_) continue;
  int nblk = ni_/blk;
  ni_ = nblk*blk;

  MPI_Allreduce(A_.data(),A.data(),2*ni_*nw*size,MPI_DOUBLE,MPI_SUM,MPI_COMM_GROUP);

  int nt=4;
  MPI_Barrier(MPI_COMM_WORLD);
  t1=getTime();
  for(int i=0; i<nt; i++)
    MPI_Allreduce(A_.data(),A.data(),2*ni_*nw*size,MPI_DOUBLE,MPI_SUM,MPI_COMM_GROUP);
  MPI_Barrier(MPI_COMM_WORLD);
  t2=getTime();
  double tv1=(t2-t1)/nt;

  int nmmax=4096;
  std::vector<MPI_Request> req(nmmax);
  std::vector<MPI_Status> st(nmmax);
  MPI_Barrier(MPI_COMM_WORLD);
  t1=getTime();
  for(int i=0; i<nt; i++) {
    int cnt=0;
    for(int b=0; b<nblk; b++) {
      for(int r=0; r<size; r++) {
        if(rank==r)
          MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_GROUP,&req[cnt++]);
        else
          MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_GROUP,&req[cnt++]);
      }
      if(cnt==nmmax) {
        MPI_Waitall(cnt,req.data(),st.data());
        cnt=0;
      }
    }
    if(cnt>0) {
      MPI_Waitall(cnt,req.data(),st.data());
      cnt=0;
    }
  }
  MPI_Barrier(MPI_COMM_WORLD);
  t2=getTime();
  double tv5=(t2-t1)/nt;

  if(global_rank==0) cout<<" n, times (Allreduce, multiple IReduce): " <<ni_*local_size <<" " <<ni_ <<" " <<tv1 <<" " <<tv5 <<endl;

  }

  MPI_Finalize();

  return 0;
}


/*  $Id: cylcutter.cpp 607 2011-01-21 18:01:36Z anders.e.e.wallin $
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/foreach.hpp>

#include "cylcutter.h"
#include "bullcutter.h" // for offsetCutter()
#include "numeric.h"

namespace ocl
{

CylCutter::CylCutter() {
    std::cout << " usage: CylCutter( double diameter, double length ) \n";
    assert(0);
}

CylCutter::CylCutter(double d, double l) {
    diameter= d;                    assert( d > 0.0);
    radius= d/2.0;
    length = l;                     assert( l > 0.0 );
    xy_normal_length = radius;
    normal_length = 0.0;
    center_height = 0.0;
}

// drop-cutter vertexDrop is handled by the base-class
// drop-cutter facetDrop is handled by the base-class

CC_CLZ_Pair CylCutter::singleEdgeDropCanonical(const Point& u1, const Point& u2) const {
    // along the x-axis the cc-point is at x-coord s:
    double s = sqrt( square( radius ) - square( u1.y ) );
    Point cc1(  s, u1.y, 0);
    Point cc2( -s, u1.y, 0);
    cc1.z_projectOntoEdge(u1,u2);
    cc2.z_projectOntoEdge(u1,u2);
    // pick the higher one
    double cc_u;
    double cl_z;
    if (cc1.z > cc2.z) {
        cc_u = cc1.x;
        cl_z = cc1.z;
    } else {
        cc_u = cc2.x;
        cl_z = cc2.z;
    }
    return CC_CLZ_Pair( cc_u, cl_z);
}

std::string CylCutter::str() const {
    std::ostringstream o;
    o << *this;
    return o.str();
}

std::ostream& operator<<(std::ostream &stream, CylCutter c) {
  stream << "CylCutter (d=" << c.diameter << ", L=" << c.length <<  ")";
  return stream;
}

} // end namespace
// end file cylcutter.cpp


#include "bipartite.h"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

using namespace boost;

typedef adjacency_list_traits<vecS, vecS, directedS> Traits;

typedef adjacency_list<
    vecS,
    vecS,
    directedS,
    no_property,
    property<edge_capacity_t,
             long,
             property<edge_residual_capacity_t,
                      long,
                      property<edge_reverse_t, Traits::edge_descriptor, property<edge_weight_t, long>>>>>
    Graph;
typedef property_map<Graph, edge_capacity_t>::type Capacity;
typedef property_map<Graph, edge_residual_capacity_t>::type ResidualCapacity;
typedef property_map<Graph, edge_weight_t>::type Weight;
typedef property_map<Graph, edge_reverse_t>::type Reversed;
typedef boost::graph_traits<Graph>::vertices_size_type size_type;
typedef Traits::vertex_descriptor vertex_descriptor;

template <class Graph, class Weight, class Capacity, class Reversed, class ResidualCapacity>
class EdgeAdder {
public:
    EdgeAdder(Graph& g, Weight& w, Capacity& c, Reversed& rev, ResidualCapacity&)
        : m_g(g), m_w(w), m_cap(c), m_rev(rev) {}
    void addEdge(vertex_descriptor v, vertex_descriptor w, long weight, long capacity) {
        Traits::edge_descriptor e, f;
        e = add(v, w, weight, capacity);
        f = add(w, v, -weight, 0);
        m_rev[e] = f;
        m_rev[f] = e;
    }

private:
    Traits::edge_descriptor add(vertex_descriptor v, vertex_descriptor w, long weight, long capacity) {
        bool b;
        Traits::edge_descriptor e;
        boost::tie(e, b) = add_edge(vertex(v, m_g), vertex(w, m_g), m_g);
        if (!b) {
            std::cerr << "Edge between " << v << " and " << w << " already exists." << std::endl;
            std::abort();
        }
        m_cap[e] = capacity;
        m_w[e] = weight;
        return e;
    }
    Graph& m_g;
    Weight& m_w;
    Capacity& m_cap;
    Reversed& m_rev;
};

void MinCostBipartiteMatching(const vector<vector<pair<int, long>>>& bigraph,
                              size_t num1,
                              size_t num2,
                              vector<pair<int, long>>& res,
                              long& cost) {
    assert(num1 == bigraph.size());
    int cid2vid = 2;
    int sid2vid = cid2vid + num1;
    int numVer = sid2vid + num2 + 2;

    Graph g;
    Capacity capacity = get(edge_capacity, g);
    Reversed rev = get(edge_reverse, g);
    ResidualCapacity residual_capacity = get(edge_residual_capacity, g);
    Weight weight = get(edge_weight, g);

    size_type N(numVer);
    for (size_type i = 0; i < N; ++i) {
        add_vertex(g);
    }

    vertex_descriptor s = 0;
    vertex_descriptor t = 1;

    EdgeAdder<Graph, Weight, Capacity, Reversed, ResidualCapacity> ea(g, weight, capacity, rev, residual_capacity);

    // source to cells
    for (int vi = cid2vid; vi < sid2vid; ++vi) ea.addEdge(0, vi, 0, 1);

    // cells to sites
    for (size_t cid = 0; cid < num1; ++cid) {
        for (auto& edge : bigraph[cid]) {
            ea.addEdge(cid + cid2vid, edge.first + sid2vid, edge.second, 1);
        }
    }

    // sites to destination
    for (int vi = sid2vid; vi < numVer; ++vi) ea.addEdge(vi, 1, 0, 1);

    successive_shortest_path_nonnegative_weights(g, s, t);
    cost = find_flow_cost(g);

    res.resize(num1, {-1, -1});
    for (int vi = cid2vid; vi < sid2vid; ++vi) {
        BGL_FORALL_OUTEDGES_T(vi, e, g, Graph) {
            if (get(capacity, e) > 0 && get(residual_capacity, e) == 0) {
                // cout <<  get(capacity, e) << " " << get(residual_capacity, e) << " " << get(weight, e) << endl;
                auto vi2 = target(e, g);
                int c = vi - cid2vid, s = vi2 - sid2vid;
                res[c].first = s;
                res[c].second = get(weight, e);
                // cout << c << " goes to " << s << endl;
            }
        }
    }

    // BGL_FORALL_EDGES_T(e, g, Graph) {
    //    if(get(capacity, e) > Cost(0)) {
    //     	cout    <<  "(" << source(e,g) << "," << target(e,g) << ") "
    //                 << get(capacity, e) << " " << get(residual_capacity, e)
    //                 << " " << get(weight, e) << endl;
    //    }
    // }
}

void LiteMinCostMaxFlow(const vector<vector<pair<int, int>>>& supplyToDemand,
                        const vector<int>& demandCap,
                        size_t num1,
                        size_t num2,
                        vector<pair<int, int>>& res,
                        int& cost) {
    unsigned nVirVertex = 2;
    unsigned nSupplyVertex = num1;
    unsigned nDemandVertex = num2;
    unsigned nVertex = nVirVertex + nSupplyVertex + nDemandVertex;

    Graph g;
    Capacity capacity = get(edge_capacity, g);
    Reversed rev = get(edge_reverse, g);
    ResidualCapacity residual_capacity = get(edge_residual_capacity, g);
    Weight weight = get(edge_weight, g);

    size_type N(nVertex);
    for (size_type i = 0; i < N; ++i) {
        add_vertex(g);
    }

    vertex_descriptor s = 0;
    vertex_descriptor t = 1;

    EdgeAdder<Graph, Weight, Capacity, Reversed, ResidualCapacity> ea(g, weight, capacity, rev, residual_capacity);

    // source to cells
    for (unsigned c = 0; c < nSupplyVertex; c++) ea.addEdge(0, c + nVirVertex, 0, 1);

    // cells to sites
    for (unsigned c = 0; c < nSupplyVertex; c++) {
        for (auto pair : supplyToDemand[c]) {
            int s = pair.first;
            int w = pair.second;
            ea.addEdge(c + nVirVertex, s + nVirVertex + nSupplyVertex, w, 1);
            // cout << c << " goes to " << s << endl;
        }
    }

    // sites to destination
    for (unsigned s = 0; s < nDemandVertex; s++) {
        if (demandCap[s] != 0) ea.addEdge(s + nVirVertex + nSupplyVertex, 1, 0, demandCap[s]);
    }

    successive_shortest_path_nonnegative_weights(g, s, t);
    cost = find_flow_cost(g);
    res.resize(nSupplyVertex, {-1, -1});

    for (unsigned c = 0; c < nSupplyVertex; ++c) {
        BGL_FORALL_OUTEDGES_T(c + nVirVertex, e, g, Graph) {
            if (get(capacity, e) > 0 && get(residual_capacity, e) == 0) {
                // cout <<  get(capacity, e) << " " << get(residual_capacity, e) << " " << get(weight, e) << endl;
                auto s = target(e, g) - nVirVertex - nSupplyVertex;
                res[c].first = s;
                res[c].second = get(weight, e);
                // cout << c << " goes to " << s << endl;
            }
        }
    }
}


#include "dbdet_load_edg.h"
#include <vcl_cstring.h>
#include <vcl_locale.h>

#include <vul/vul_file.h>
#include <vsol/vsol_line_2d.h>
#include <vsol/vsol_point_2d.h>
#include <vsol/vsol_line_2d_sptr.h>
#include "dbdet_edgemap.h"
//#include "dbdet_config.h"
#include "dbdet_sel_utils.h"

#ifdef HAS_BOOST
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#endif

bool dbdet_load_edg(vcl_string input_file, bool bSubPixel, bool blines, double scale,
                    vcl_vector< vsol_spatial_object_2d_sptr > &edgels)
{
  double x, y;
  char lineBuffer[1024];
  int numGeometry, w=0, h=0;
  int ix, iy;
  double idir, iconf, dir, conf;

  //1)If file open fails, return.
  vcl_ifstream infp(input_file.c_str(), vcl_ios::in);

  if (!infp){
    vcl_cout << " Error opening file  " << input_file.c_str() << vcl_endl;
    return false;
  }

  bool file_valid = false;

  //2)Read in the header block
  for (int j=0; j<9; j++){

    //read next line
    infp.getline(lineBuffer,1024);

    //read the line with the file type
    if (!vcl_strncmp(lineBuffer, "# EDGE_MAP", sizeof("# EDGE_MAP")-1)){
      file_valid = true;
      continue;
    }

    //ignore all other comment lines and empty lines
    if (vcl_strlen(lineBuffer)<2 || lineBuffer[0]=='#')
      continue;

    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, " WIDTH=", sizeof(" WIDTH=")-1)){
      sscanf(lineBuffer," WIDTH=%d",&(w));
      continue;
    }
    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, "WIDTH=", sizeof("WIDTH=")-1)){
      sscanf(lineBuffer,"WIDTH=%d",&(w));
      continue;
    }

    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, " HEIGHT=", sizeof(" HEIGHT=")-1)){
      sscanf(lineBuffer," HEIGHT=%d",&(h));
      continue;
    }
    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, "HEIGHT=", sizeof("HEIGHT=")-1)){
      sscanf(lineBuffer,"HEIGHT=%d",&(h));
      continue;
    }

    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, " EDGE_COUNT=", sizeof(" EDGE_COUNT=")-1)){
      sscanf(lineBuffer," EDGE_COUNT=%d",&(numGeometry));
      continue;
    }
    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, "EDGE_COUNT=", sizeof("EDGE_COUNT=")-1)){
      sscanf(lineBuffer,"EDGE_COUNT=%d",&(numGeometry));
      continue;
    }
  }

  //make sure the file is valid
  if (!file_valid){
    vcl_cout << "Invalid .edg file (vsol/ascii)" << vcl_endl;
    return false;
  }

  // make sure w and h are reasonable
  if (w==0 && h==0) { w=1000; h=1000; }

  //read the data block
  while (infp.getline(lineBuffer,1024)) {

    //there are two variations of this file in existence
    if (!vcl_strncmp(lineBuffer, "EDGE : ", sizeof("EDGE : ")-1))
      sscanf(lineBuffer,"EDGE :  [%d, %d]    %lf %lf   [%lf, %lf]   %lf %lf",&(ix), &(iy),
        &(idir), &(iconf), &(x), &(y), &(dir), &(conf));
    else
      sscanf(lineBuffer," [%d, %d]   %lf %lf  [%lf, %lf]  %lf %lf",&(ix), &(iy),
        &(idir), &(iconf), &(x), &(y), &(dir), &(conf));

    //scale the edges
    x *= scale;
    y *= scale;

    ix = (int)(ix*scale);
    iy = (int)(iy*scale);

    if (bSubPixel){
      if ( blines){
        vsol_line_2d_sptr newLine = new vsol_line_2d(vgl_vector_2d<double>(vcl_cos(dir)/2.0, vcl_sin(dir)/2.0), vgl_point_2d<double>(x,y));
        edgels.push_back(newLine->cast_to_spatial_object());
      }
      else {
        vsol_point_2d_sptr newPt = new vsol_point_2d(x,y);
        edgels.push_back(newPt->cast_to_spatial_object());
      }
    }
    else { //pixel edges
      if (blines){
        vsol_line_2d_sptr newLine = new vsol_line_2d(vgl_vector_2d<double>(vcl_cos(idir), vcl_sin(idir)), vgl_point_2d<double>((double)ix, (double)iy));
        edgels.push_back(newLine->cast_to_spatial_object());
      }
      else {
        vsol_point_2d_sptr newPt = new vsol_point_2d((double)ix, (double)iy);
        edgels.push_back(newPt->cast_to_spatial_object());
      }
    }
  }
  infp.close();

  return true;

}

#ifdef HAS_BOOST
static bool dbdet_load_edg_gzip(vcl_string input_file, bool bSubPixel, double scale, dbdet_edgemap_sptr &edge_map);
static bool dbdet_save_edg_gzip(vcl_string filename, dbdet_edgemap_sptr edgemap);
#endif

static bool dbdet_load_edg_ascii(vcl_string input_file, bool bSubPixel, double scale, dbdet_edgemap_sptr &edge_map);

static bool dbdet_save_edg_ascii(vcl_string filename, dbdet_edgemap_sptr edgemap);

bool dbdet_load_edg(vcl_string input_file, bool bSubPixel, double scale, dbdet_edgemap_sptr &edge_map)
{
  vcl_string ext = vul_file::extension(input_file);
  bool ret;

  char * cur_locale, * dup_locale;
  cur_locale = setlocale(LC_NUMERIC, NULL);
  dup_locale = strdup(cur_locale);
  setlocale(LC_NUMERIC, "C");

  if (ext == ".gz") {
#ifdef HAS_BOOST
    ret = dbdet_load_edg_gzip(input_file, bSubPixel, scale, edge_map);
#else
    vcl_cerr << "Error: .gz compressed edg file was provided, but boost wasn't found\n";
#endif
  } else
    ret = dbdet_load_edg_ascii(input_file, bSubPixel, scale, edge_map);

  setlocale(LC_NUMERIC, dup_locale);
  free(dup_locale);

  return ret;
}

bool dbdet_save_edg(vcl_string filename, dbdet_edgemap_sptr edgemap)
{
  vcl_string ext = vul_file::extension(filename);
  bool ret;

  char * cur_locale, * dup_locale;
  cur_locale = setlocale(LC_NUMERIC, NULL);
  dup_locale = strdup(cur_locale);
  setlocale(LC_NUMERIC, "C");

  if (ext == ".gz") {
#ifdef HAS_BOOST
    ret = dbdet_save_edg_gzip(filename, edgemap);
#else
    vcl_cerr << "Error: .gz compressed output edg file requested, but boost wasn't found\n";
#endif
  } else
    ret = dbdet_save_edg_ascii(filename, edgemap);

  setlocale(LC_NUMERIC, dup_locale);
  free(dup_locale);

  return ret;
}

bool dbdet_load_edg_ascii(vcl_string input_file, bool bSubPixel, double scale, dbdet_edgemap_sptr &edge_map)
{
  double x, y;
  char lineBuffer[1024];
  int numGeometry, w=0, h=0;
  int ix, iy;
  double idir, iconf, dir, conf, uncer=0;

  //1)If file open fails, return.
  vcl_ifstream infp(input_file.c_str(), vcl_ios::in);

  if (!infp){
    vcl_cout << " Error opening file  " << input_file.c_str() << vcl_endl;
    return false;
  }

  bool file_valid = false;
  int ver = 0;

  //2)Read in the header block
  for (int j=0; j<9; j++){

    //read next line
    infp.getline(lineBuffer,1024);

    //read the line with the file type
    if (!vcl_strncmp(lineBuffer, "# EDGE_MAP", sizeof("# EDGE_MAP")-1)){
      file_valid = true;

      if (!vcl_strncmp(lineBuffer, "# EDGE_MAP v2.0", sizeof("# EDGE_MAP v2.0")-1))
        ver = 2;
      else if (!vcl_strncmp(lineBuffer, "# EDGE_MAP v3.0", sizeof("# EDGE_MAP v3.0")-1))
        ver = 3;
      else
        ver = 1;

      continue;
    }

    //ignore all other comment lines and empty lines
    if (vcl_strlen(lineBuffer)<2 || lineBuffer[0]=='#')
      continue;

    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, " WIDTH=", sizeof(" WIDTH=")-1)){
      sscanf(lineBuffer," WIDTH=%d",&(w));
      continue;
    }
    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, "WIDTH=", sizeof("WIDTH=")-1)){
      sscanf(lineBuffer,"WIDTH=%d",&(w));
      continue;
    }

    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, " HEIGHT=", sizeof(" HEIGHT=")-1)){
      sscanf(lineBuffer," HEIGHT=%d",&(h));
      continue;
    }
    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, "HEIGHT=", sizeof("HEIGHT=")-1)){
      sscanf(lineBuffer,"HEIGHT=%d",&(h));
      continue;
    }

    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, " EDGE_COUNT=", sizeof(" EDGE_COUNT=")-1)){
      sscanf(lineBuffer," EDGE_COUNT=%d",&(numGeometry));
      continue;
    }
    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, "EDGE_COUNT=", sizeof("EDGE_COUNT=")-1)){
      sscanf(lineBuffer,"EDGE_COUNT=%d",&(numGeometry));
      continue;
    }
  }

  //make sure the file is valid
  if (!file_valid){
    vcl_cout << "Invalid .edg file (ascii)" << vcl_endl;
    return false;
  }

  // make sure w and h are reasonable
  if (w==0 && h==0) { w=1000; h=1000; }

  // edge_map
  edge_map = new dbdet_edgemap(static_cast<int>(w*scale), static_cast<int>(h*scale));

  edge_map->edgels.reserve(numGeometry);

  //read the data block
  while (infp.getline(lineBuffer,1024)) {

    //there are two variations of this file in existence
    if (ver<3){
      int a = 0;
      if (!vcl_strncmp(lineBuffer, "EDGE : ", sizeof("EDGE : ")-1)){
        a=sscanf(lineBuffer,"EDGE :  [%d, %d]    %lf %lf   [%lf, %lf]   %lf %lf",&(ix), &(iy),
          &(idir), &(iconf), &(x), &(y), &(dir), &(conf));
      }else{
        a=sscanf(lineBuffer," [%d, %d]   %lf %lf  [%lf, %lf]  %lf %lf",&(ix), &(iy),
          &(idir), &(iconf), &(x), &(y), &(dir), &(conf));}
      if(a!=8) {
         vcl_cout << "dbdet_load_edg_ascii: input error argument" << a << " from line: \"" << lineBuffer << "\"" << vcl_endl;
      }
    }
    else {
      int a=sscanf(lineBuffer," [%d, %d]   %lf %lf  [%lf, %lf]  %lf %lf %lf",&(ix), &(iy),
          &(idir), &(iconf), &(x), &(y), &(dir), &(conf), &(uncer));

      if (a!=9) {
        vcl_cout << "dbdet_load_edg_ascii: input error argument" << a << " from line: \"" << lineBuffer << "\"" << vcl_endl;
      }
    }

    //scale the edges
    x *= scale;
    y *= scale;

    ix = (int)(ix*scale);
    iy = (int)(iy*scale);

    if (bSubPixel){
      //create an edgel token
//      if (ix !=0 && iy !=0) //trustworthy coordinates
//        edge_map->insert(new dbdet_edgel(vgl_point_2d<double>(x, y), dir, conf, 0.0, uncer), ix, iy);
//      else
        edge_map->insert(new dbdet_edgel(vgl_point_2d<double>(x, y), dir, conf, 0.0, uncer));
    }
    else { //pixel edges
      //create an edgel token
      edge_map->insert(new dbdet_edgel(vgl_point_2d<double>(ix, iy), idir, iconf, 0.0, uncer));
    }
  }
  infp.close();

  return true;
}

#ifdef HAS_BOOST
//: Load .edg.gz file compressed with zlib, gzip style.
// TODO: use templating and/or istream inheritance to avoid duplicating almost
// identical code to bsold_load_cem_ascii
bool dbdet_load_edg_gzip(vcl_string input_file, bool bSubPixel, double scale, dbdet_edgemap_sptr &edge_map)
{
  double x, y;
  char lineBuffer[1024];
  int numGeometry, w=0, h=0;
  int ix, iy;
  double idir, iconf, dir, conf, uncer=0;

  //1)If file open fails, return.
  vcl_ifstream infp_orig(input_file.c_str(), vcl_ios::in  | vcl_ios::binary);

  if (!infp_orig){
    vcl_cout << " Error opening file  " << input_file.c_str() << vcl_endl;
    return false;
  }

  boost::iostreams::filtering_istream infp;
  infp.push(boost::iostreams::gzip_decompressor());
  infp.push(infp_orig);

  bool file_valid = false;
  int ver = 0;

  //2)Read in the header block
  for (int j=0; j<9; j++){

    //read next line
    infp.getline(lineBuffer,1024);

    //read the line with the file type
    if (!vcl_strncmp(lineBuffer, "# EDGE_MAP", sizeof("# EDGE_MAP")-1)){
      file_valid = true;

      if (!vcl_strncmp(lineBuffer, "# EDGE_MAP v2.0", sizeof("# EDGE_MAP v2.0")-1))
        ver = 2;
      else if (!vcl_strncmp(lineBuffer, "# EDGE_MAP v3.0", sizeof("# EDGE_MAP v3.0")-1))
        ver = 3;
      else
        ver = 1;

      continue;
    }

    //ignore all other comment lines and empty lines
    if (vcl_strlen(lineBuffer)<2 || lineBuffer[0]=='#')
      continue;

    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, " WIDTH=", sizeof(" WIDTH=")-1)){
      sscanf(lineBuffer," WIDTH=%d",&(w));
      continue;
    }
    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, "WIDTH=", sizeof("WIDTH=")-1)){
      sscanf(lineBuffer,"WIDTH=%d",&(w));
      continue;
    }

    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, " HEIGHT=", sizeof(" HEIGHT=")-1)){
      sscanf(lineBuffer," HEIGHT=%d",&(h));
      continue;
    }
    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, "HEIGHT=", sizeof("HEIGHT=")-1)){
      sscanf(lineBuffer,"HEIGHT=%d",&(h));
      continue;
    }

    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, " EDGE_COUNT=", sizeof(" EDGE_COUNT=")-1)){
      sscanf(lineBuffer," EDGE_COUNT=%d",&(numGeometry));
      continue;
    }
    //read the line with the edge count info
    if (!vcl_strncmp(lineBuffer, "EDGE_COUNT=", sizeof("EDGE_COUNT=")-1)){
      sscanf(lineBuffer,"EDGE_COUNT=%d",&(numGeometry));
      continue;
    }
  }

  //make sure the file is valid
  if (!file_valid){
    vcl_cout << "Invalid .edg file (gzip)" << vcl_endl;
    return false;
  }

  // make sure w and h are reasonable
  if (w==0 && h==0) { w=1000; h=1000; }

  // edge_map
  edge_map = new dbdet_edgemap(static_cast<int>(w*scale), static_cast<int>(h*scale));

  edge_map->edgels.reserve(numGeometry);

  //read the data block
  while (infp.getline(lineBuffer,1024)) {

    //there are two variations of this file in existence
    if (ver<3){
      int a = 0;
      if (!vcl_strncmp(lineBuffer, "EDGE : ", sizeof("EDGE : ")-1))
      {
        a = sscanf(lineBuffer,"EDGE :  [%d, %d]    %lf %lf   [%lf, %lf]   %lf %lf",&(ix), &(iy),
          &(idir), &(iconf), &(x), &(y), &(dir), &(conf));
      }
      else
      {
        a = sscanf(lineBuffer," [%d, %d]   %lf %lf  [%lf, %lf]  %lf %lf",&(ix), &(iy),
          &(idir), &(iconf), &(x), &(y), &(dir), &(conf));
      }
      if (a!=8) {
        vcl_cout << "dbdet_load_edg_ascii: input error argument" << a << " from line: \"" << lineBuffer << "\"" << vcl_endl;
      }
    }
    else
    {
      int a = sscanf(lineBuffer," [%d, %d]   %lf %lf  [%lf, %lf]  %lf %lf %lf",&(ix), &(iy),
          &(idir), &(iconf), &(x), &(y), &(dir), &(conf), &(uncer));
      if (a!=9) {
        vcl_cout << "dbdet_load_edg_ascii: input error argument" << a << " from line: \"" << lineBuffer << "\"" << vcl_endl;
      }
    }

    //scale the edges
    x *= scale;
    y *= scale;

    //XXX hack: recompute assuming pixel buckets, avoiding roundoff
    //inconsistencies, because we will need later on in other times go back from
    //x,y to buckets anyways, and if we read off from file we have roundoff
    //inconsistency.
    //
//    ix = dbdet_round(x);
//    iy = dbdet_round(y);

    if (bSubPixel){
      //create an edgel token
//      if (ix !=0 && iy !=0) //trustworthy coordinates
//        edge_map->insert(new dbdet_edgel(vgl_point_2d<double>(x, y), dir, conf, 0.0, uncer), ix, iy);
//      else
        edge_map->insert(new dbdet_edgel(vgl_point_2d<double>(x, y), dir, conf, 0.0, uncer));
    }
    else { //pixel edges
      //create an edgel token
      edge_map->insert(new dbdet_edgel(vgl_point_2d<double>(ix, iy), idir, iconf, 0.0, uncer));
    }
  }
  return true;
}
#endif

bool dbdet_save_edg_ascii(vcl_string filename, dbdet_edgemap_sptr edgemap)
{
  //1) If file open fails, return.
  vcl_ofstream outfp(filename.c_str(), vcl_ios::out);

  if (!outfp){
    vcl_cout << " Error opening file  " << filename.c_str() << vcl_endl;
    return false;
  }

  //2) write out the header block
  outfp << "# EDGE_MAP v3.0" << vcl_endl << vcl_endl;
  outfp << "# Format :  [Pixel_Pos]  Pixel_Dir Pixel_Conf  [Sub_Pixel_Pos] Sub_Pixel_Dir Strength Uncer" << vcl_endl;
  outfp << vcl_endl;
  outfp << "WIDTH=" << edgemap->width() << vcl_endl;
  outfp << "HEIGHT=" << edgemap->height() << vcl_endl;
  outfp << "EDGE_COUNT=" << edgemap->num_edgels()  << vcl_endl;
  outfp << vcl_endl << vcl_endl;

  //save the edgel tokens
  for (unsigned row=0; row<edgemap->edge_cells.rows(); row++){
    for (unsigned col=0; col<edgemap->edge_cells.cols(); col++){
      for (unsigned k=0; k<edgemap->edge_cells(row, col).size(); k++){

        dbdet_edgel* edgel = edgemap->edge_cells(row, col)[k];

        double x = edgel->pt.x();
        double y = edgel->pt.y();

        unsigned ix = col; //preserve the original pixel assignment
        unsigned iy = row;

        double idir = edgel->tangent, iconf = edgel->strength;
        double dir= edgel->tangent, conf= edgel->strength,
//               d2f=edgel->deriv,
               uncer=edgel->uncertainty;

        outfp << "[" << ix << ", " << iy << "]    " << idir << " " << iconf << "   [" << x << ", " << y << "]   " << dir << " " << conf << " " << uncer << vcl_endl;
      }
    }
  }

  outfp.close();

  return true;
}

#ifdef HAS_BOOST
bool dbdet_save_edg_gzip(vcl_string filename, dbdet_edgemap_sptr edgemap)
{
  //1) If file open fails, return.
  vcl_ofstream outfp_orig(filename.c_str(), vcl_ios::out  | vcl_ios::binary);

  if (!outfp_orig){
    vcl_cout << " Error opening file  " << filename.c_str() << vcl_endl;
    return false;
  }

  boost::iostreams::filtering_ostream outfp;
  outfp.push(boost::iostreams::gzip_compressor());
  outfp.push(outfp_orig);

  //2) write out the header block
  outfp << "# EDGE_MAP v3.0" << vcl_endl << vcl_endl;
  outfp << "# Format :  [Pixel_Pos]  Pixel_Dir Pixel_Conf  [Sub_Pixel_Pos] Sub_Pixel_Dir Strength Uncer" << vcl_endl;
  outfp << vcl_endl;
  outfp << "WIDTH=" << edgemap->width() << vcl_endl;
  outfp << "HEIGHT=" << edgemap->height() << vcl_endl;
  outfp << "EDGE_COUNT=" << edgemap->num_edgels()  << vcl_endl;
  outfp << vcl_endl << vcl_endl;

  //save the edgel tokens
  for (unsigned row=0; row<edgemap->edge_cells.rows(); row++){
    for (unsigned col=0; col<edgemap->edge_cells.cols(); col++){
      for (unsigned k=0; k<edgemap->edge_cells(row, col).size(); k++){

        dbdet_edgel* edgel = edgemap->edge_cells(row, col)[k];

        double x = edgel->pt.x();
        double y = edgel->pt.y();

        unsigned ix = col; //preserve the original pixel assignment
        unsigned iy = row;

        double idir = edgel->tangent, iconf = edgel->strength;
        double dir= edgel->tangent, conf= edgel->strength,
//               d2f=edgel->deriv,
               uncer=edgel->uncertainty;

        outfp << "[" << ix << ", " << iy << "]    " << idir << " " << iconf << "   [" << x << ", " << y << "]   " << dir << " " << conf << " " << uncer << vcl_endl;
      }
    }
  }

  return true;
}
#endif


// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "fiber/piecewise_linear_continuous_scalar_basis.hpp"
#include "fiber/scalar_traits.hpp"
#include "../type_template.hpp"
#include "../check_arrays_are_close.hpp"

#include <algorithm>
#include "common/armadillo_fwd.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/version.hpp>
#include <complex>

// Tests

BOOST_AUTO_TEST_SUITE(PiecewiseLinearContinuousScalarBasis_triangle)

BOOST_AUTO_TEST_CASE_TEMPLATE(size_is_3, ValueType, basis_function_types)
{
    Fiber::PiecewiseLinearContinuousScalarBasis<3, ValueType> basis;
    BOOST_CHECK_EQUAL(basis.size(), 3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(order_is_1, ValueType, basis_function_types)
{
    Fiber::PiecewiseLinearContinuousScalarBasis<3, ValueType> basis;
    BOOST_CHECK_EQUAL(basis.order(), 1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_works_for_all_dofs,
                              ValueType, basis_function_types)
{
    const int vertexCount = 3;
    const int elementDim = 2;
    const int pointCount = vertexCount;
    typedef Fiber::PiecewiseLinearContinuousScalarBasis<vertexCount, ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES, points, Fiber::ALL_DOFS, data);

    Fiber::_3dArray<ValueType> expected(1, // component count
                                        vertexCount,
                                        pointCount);
    std::fill(expected.begin(), expected.end(), 0.);
    expected(0, 0, 0) = 1.;
    expected(0, 1, 1) = 1.;
    expected(0, 2, 2) = 1.;

    BOOST_CHECK(check_arrays_are_close<ValueType>(data.values, expected, 1e-10));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_works_for_second_dof,
                              ValueType, basis_function_types)
{
    const int vertexCount = 3;
    const int elementDim = 2;
    const int pointCount = 3;
    const int dofIndex = 1;
    typedef Fiber::PiecewiseLinearContinuousScalarBasis<vertexCount, ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES, points, dofIndex, data);

    Fiber::_3dArray<ValueType> expected(1, // component count
                                        1, // number of DOFs
                                        pointCount);
    std::fill(expected.begin(), expected.end(), 0.);
    expected(0, 0, 1) = 1.;

    BOOST_CHECK(check_arrays_are_close<ValueType>(data.values, expected, 1e-10));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_derivatives_works_for_all_dofs,
                              ValueType, basis_function_types)
{
    const int vertexCount = 3;
    const int elementDim = 2;
    const int pointCount = vertexCount;
    typedef Fiber::PiecewiseLinearContinuousScalarBasis<vertexCount, ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::DERIVATIVES, points, Fiber::ALL_DOFS, data);

    Fiber::_4dArray<ValueType> expected(1, // component count
                                        elementDim,
                                        vertexCount,
                                        pointCount);
    std::fill(expected.begin(), expected.end(), 0.);
    expected(0, 0, 0, 0) = -1.;
    expected(0, 1, 0, 0) = -1.;
    expected(0, 0, 1, 0) = 1.;
    expected(0, 1, 2, 0) = 1.;
    expected(0, 0, 0, 1) = -1.;
    expected(0, 1, 0, 1) = -1.;
    expected(0, 0, 1, 1) = 1.;
    expected(0, 1, 2, 1) = 1.;
    expected(0, 0, 0, 2) = -1.;
    expected(0, 1, 0, 2) = -1.;
    expected(0, 0, 1, 2) = 1.;
    expected(0, 1, 2, 2) = 1.;

    BOOST_CHECK(check_arrays_are_close<ValueType>(data.derivatives, expected, 1e-10));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_and_derivatives_works_for_all_dofs,
                              ValueType, basis_function_types)
{
    const int vertexCount = 3;
    const int elementDim = 2;
    const int pointCount = 3;
    typedef Fiber::PiecewiseLinearContinuousScalarBasis<vertexCount, ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES | Fiber::DERIVATIVES, points, Fiber::ALL_DOFS, data);

    {
        Fiber::_3dArray<ValueType> expected(1, // component count
                                            vertexCount,
                                            pointCount);
        std::fill(expected.begin(), expected.end(), 0.);
        expected(0, 0, 0) = 1.;
        expected(0, 1, 1) = 1.;
        expected(0, 2, 2) = 1.;

        BOOST_CHECK(check_arrays_are_close<ValueType>(data.values, expected, 1e-10));
    }
    {
        Fiber::_4dArray<ValueType> expected(1, // component count
                                            elementDim,
                                            vertexCount,
                                            pointCount);
        std::fill(expected.begin(), expected.end(), 0.);
        expected(0, 0, 0, 0) = -1.;
        expected(0, 1, 0, 0) = -1.;
        expected(0, 0, 1, 0) = 1.;
        expected(0, 1, 2, 0) = 1.;
        expected(0, 0, 0, 1) = -1.;
        expected(0, 1, 0, 1) = -1.;
        expected(0, 0, 1, 1) = 1.;
        expected(0, 1, 2, 1) = 1.;
        expected(0, 0, 0, 2) = -1.;
        expected(0, 1, 0, 2) = -1.;
        expected(0, 0, 1, 2) = 1.;
        expected(0, 1, 2, 2) = 1.;
        BOOST_CHECK(check_arrays_are_close<ValueType>(data.derivatives, expected, 1e-10));
    }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(PiecewiseLinearContinuousScalarBasis_square)

BOOST_AUTO_TEST_CASE_TEMPLATE(size_is_4, ValueType, basis_function_types)
{
    Fiber::PiecewiseLinearContinuousScalarBasis<4, ValueType> basis;
    BOOST_CHECK_EQUAL(basis.size(), 4);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(order_is_1, ValueType, basis_function_types)
{
    Fiber::PiecewiseLinearContinuousScalarBasis<4, ValueType> basis;
    BOOST_CHECK_EQUAL(basis.order(), 1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_works_for_all_dofs,
                              ValueType, basis_function_types)
{
    const int vertexCount = 4;
    const int elementDim = 2;
    const int pointCount = vertexCount;
    typedef Fiber::PiecewiseLinearContinuousScalarBasis<vertexCount, ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    points(0, 3) = 1.;
    points(1, 3) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES, points, Fiber::ALL_DOFS, data);

    Fiber::_3dArray<ValueType> expected(1, // component count
                                        vertexCount,
                                        pointCount);
    std::fill(expected.begin(), expected.end(), 0.);
    expected(0, 0, 0) = 1.;
    expected(0, 1, 1) = 1.;
    expected(0, 2, 2) = 1.;
    expected(0, 3, 3) = 1.;

    BOOST_CHECK(check_arrays_are_close<ValueType>(data.values, expected, 1e-10));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_works_for_second_dof,
                              ValueType, basis_function_types)
{
    const int vertexCount = 4;
    const int elementDim = 2;
    const int pointCount = vertexCount;
    const int dofIndex = 1;
    typedef Fiber::PiecewiseLinearContinuousScalarBasis<vertexCount, ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    points(0, 3) = 1.;
    points(1, 3) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES, points, dofIndex, data);

    Fiber::_3dArray<ValueType> expected(1, // component count
                                        1, // number of DOFs
                                        pointCount);
    std::fill(expected.begin(), expected.end(), 0.);
    expected(0, 0, 1) = 1.;

    BOOST_CHECK(check_arrays_are_close<ValueType>(data.values, expected, 1e-10));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_derivatives_works_for_all_dofs,
                              ValueType, basis_function_types)
{
    const int vertexCount = 4;
    const int elementDim = 2;
    const int pointCount = vertexCount;
    typedef Fiber::PiecewiseLinearContinuousScalarBasis<vertexCount, ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    points(0, 3) = 1.;
    points(1, 3) = 1.;

    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::DERIVATIVES, points, Fiber::ALL_DOFS, data);

    Fiber::_4dArray<ValueType> expected(1, // component count
                                        elementDim,
                                        vertexCount,
                                        pointCount);
    std::fill(expected.begin(), expected.end(), 0.);
    for (int point = 0; point < pointCount; ++point) {
        expected(0, 0, 0, point) = -(1. - points(1, point));
        expected(0, 1, 0, point) = -(1. - points(0, point));
        expected(0, 0, 1, point) = 1. - points(1, point);
        expected(0, 1, 1, point) = -points(0, point);
        expected(0, 0, 2, point) = -points(1, point);
        expected(0, 1, 2, point) = 1. - points(0, point);
        expected(0, 0, 3, point) = points(1, point);
        expected(0, 1, 3, point) = points(0, point);
    }

    BOOST_CHECK(check_arrays_are_close<ValueType>(data.derivatives, expected, 1e-10));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_and_derivatives_works_for_all_dofs,
                              ValueType, basis_function_types)
{
    const int vertexCount = 4;
    const int elementDim = 2;
    const int pointCount = vertexCount;
    typedef Fiber::PiecewiseLinearContinuousScalarBasis<vertexCount, ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(elementDim, pointCount);
    points.fill(0.);
    points(0, 1) = 1.;
    points(1, 2) = 1.;
    points(0, 3) = 1.;
    points(1, 3) = 1.;
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES | Fiber::DERIVATIVES, points, Fiber::ALL_DOFS, data);

    {
        Fiber::_3dArray<ValueType> expected(1, // component count
                                            vertexCount,
                                            pointCount);
        std::fill(expected.begin(), expected.end(), 0.);
        expected(0, 0, 0) = 1.;
        expected(0, 1, 1) = 1.;
        expected(0, 2, 2) = 1.;
        expected(0, 3, 3) = 1.;

        BOOST_CHECK(check_arrays_are_close<ValueType>(data.values, expected, 1e-10));
    }
    {
        Fiber::_4dArray<ValueType> expected(1, // component count
                                            elementDim,
                                            vertexCount,
                                            pointCount);
        std::fill(expected.begin(), expected.end(), 0.);
        for (int point = 0; point < pointCount; ++point) {
            expected(0, 0, 0, point) = -(1. - points(1, point));
            expected(0, 1, 0, point) = -(1. - points(0, point));
            expected(0, 0, 1, point) = 1. - points(1, point);
            expected(0, 1, 1, point) = -points(0, point);
            expected(0, 0, 2, point) = -points(1, point);
            expected(0, 1, 2, point) = 1. - points(0, point);
            expected(0, 0, 3, point) = points(1, point);
            expected(0, 1, 3, point) = points(0, point);
        }

        BOOST_CHECK(check_arrays_are_close<ValueType>(data.derivatives, expected, 1e-10));
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * ----------------- BEGIN LICENSE BLOCK ---------------------------------
 *
 * Copyright (C) 2019 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 * ----------------- END LICENSE BLOCK -----------------------------------
 */

#include "op_planner/opendrive/geometry/CenterLine.hpp"

#include <boost/array.hpp>
#include <boost/math/tools/rational.hpp>
#include <cmath>
#include <iostream>

namespace opendrive {
namespace geometry {

geometry::DirectedPoint CenterLine::eval(double s, bool applyLateralOffset) const
{
  if (s < 0.)
  {
    // as the curve is only defined betwen the range [0 , length], we return the line evaluated at s = 0
    return eval(0.);
  }

  for (auto it = geometry.rbegin(); it != geometry.rend(); it++)
  {
    if (s >= (*it)->GetStartOffset())
    {
      auto directedPoint = (*it)->PosFromDist(s - (*it)->GetStartOffset());

      // This is parametrized as the landmarks positions do not take into account the lateral offset
      if (applyLateralOffset)
      {
        directedPoint.ApplyLateralOffset(calculateOffset(s));
      }
      return directedPoint;
    }
  }

  auto &lastGeometry = geometry.back();
  return lastGeometry->PosFromDist(s - lastGeometry->GetStartOffset());
}

std::vector<double> CenterLine::samplingPoints() const
{
  std::vector<double> points;

  points.push_back(length);
  double const maxError = 1e-2; // max curve sampling error expected

  for (auto it = geometry.rbegin(); it != geometry.rend(); it++)
  {
    if ((*it)->GetType() == ::opendrive::GeometryType::LINE)
    {
      // only start and end point
    }
    else if ((*it)->GetType() == ::opendrive::GeometryType::ARC)
    {
      auto arc = static_cast<::opendrive::geometry::GeometryArc *>((*it).get());
      double theta = fabs(arc->GetCurvature()) * (*it)->GetLength();

      if (theta < 1e-2)
      {
        points.insert(points.begin(), (*it)->GetStartOffset() + 0.5 * (*it)->GetLength());
      }
      else
      {
        double c = fabs(arc->GetCurvature());
        double dsmax = 2.0 / c * acos(1.0 - c * maxError);
        int segments = static_cast<int>((*it)->GetLength() / dsmax);
        for (auto k = segments - 1; k > 0; --k)
        {
          double r = static_cast<double>(k) / static_cast<double>(segments);
          points.insert(points.begin(), (*it)->GetStartOffset() + r * (*it)->GetLength());
        }
      }
    }
    else if ((*it)->GetType() == ::opendrive::GeometryType::POLY3)
    {
      int segments = 10;
      for (auto k = segments - 1; k > 0; --k)
      {
        double r = static_cast<double>(k) / static_cast<double>(segments);
        points.insert(points.begin(), (*it)->GetStartOffset() + r * (*it)->GetLength());
      }
    }
    else if ((*it)->GetType() == ::opendrive::GeometryType::PARAMPOLY3)
    {
      int segments = 10;
      for (auto k = segments - 1; k > 0; --k)
      {
        double r = static_cast<double>(k) / static_cast<double>(segments);
        points.insert(points.begin(), (*it)->GetStartOffset() + r * (*it)->GetLength());
      }
    }
    else
    {
      // nothing to do
    }

    points.insert(points.begin(), (*it)->GetStartOffset());
  }

  return points;
}

double CenterLine::calculateOffset(double s) const
{
  if (s < 0.)
  {
    return calculateOffset(0.);
  }

  for (auto it = offsetVector.rbegin(); it != offsetVector.rend(); it++)
  {
    if (s >= it->s)
    {
      auto poly = boost::array<double, 4>{{it->a, it->b, it->c, it->d}};
      return boost::math::tools::evaluate_polynomial(poly, s - it->s);
    }
  }

  return 0.0;
}

bool generateCenterLine(RoadInformation &roadInfo, CenterLine &centerLine)
{
  bool ok = true;

  centerLine.geometry.clear();
  centerLine.length = roadInfo.attributes.length;
  centerLine.offsetVector = roadInfo.lanes.lane_offset;

  // Add geometry information
  for (auto &geometry_attribute : roadInfo.geometry_attributes)
  {
    Point start(geometry_attribute->start_position_x, geometry_attribute->start_position_y);

    try
    {
      switch (geometry_attribute->type)
      {
        case ::opendrive::GeometryType::ARC:
        {
          auto arc = static_cast<GeometryAttributesArc *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(std::make_unique<geometry::GeometryArc>(
            arc->start_position, arc->length, arc->heading, start, arc->curvature));
          break;
        }
        case ::opendrive::GeometryType::LINE:
        {
          auto line = static_cast<GeometryAttributesLine *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(
            std::make_unique<geometry::GeometryLine>(line->start_position, line->length, line->heading, start));
          break;
        }
        break;
        case ::opendrive::GeometryType::SPIRAL:
        {
          // currently not yet supported
          std::cerr << "generateCenterLine() spirals are currently not supported yet\n";
          ok = false;
          break;
        }
        break;
        case ::opendrive::GeometryType::POLY3:
        {
          auto poly3 = static_cast<GeometryAttributesPoly3 *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(std::make_unique<geometry::GeometryPoly3>(
            poly3->start_position, poly3->length, poly3->heading, start, poly3->a, poly3->b, poly3->c, poly3->d));
          break;
        }
        break;
        case ::opendrive::GeometryType::PARAMPOLY3:
        {
          auto paramPoly3 = static_cast<GeometryAttributesParamPoly3 *>(geometry_attribute.get());
          centerLine.geometry.emplace_back(std::make_unique<geometry::GeometryParamPoly3>(paramPoly3->start_position,
                                                                                paramPoly3->length,
                                                                                paramPoly3->heading,
                                                                                start,
                                                                                paramPoly3->aU,
                                                                                paramPoly3->bU,
                                                                                paramPoly3->cU,
                                                                                paramPoly3->dU,
                                                                                paramPoly3->aV,
                                                                                paramPoly3->bV,
                                                                                paramPoly3->cV,
                                                                                paramPoly3->dV));
          break;
        }
        break;
        default:
          break;
      }
    }
    catch (...)
    {
      std::cerr << "generateCenterLine() Invalid geometry definition\n";
      ok = false;
    }
  }

  return ok;
}
}
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-bitwise
    This function object returns \f$2^n\f$ (or 0 if n is less than zero)

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

    @par Note:

    This function is not defined for floating entries

    @see  ilog2

    @par Example:

      @snippet twopower.cpp twopower

    @par Possible output:

      @snippet twopower.txt twopower

  **/
  IntegerValue twopower(IntegerValue const& x);
} }
#endif

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

#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing iround2even capabilities

    Computes the integer conversion of the round2even of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer_t<T> r = iround2even(x);
    @endcode

    is similar to:

    @code
    as_integer_t<T> r = toints(round2even(x));
    @endcode

    @par Note:
    Speed can be gained using iround2even(x, fast_) that uses @ref
    toint in place of @ref toints,  but be aware that large values can be not correctly converted
    and that invalid entries lead to undefined results

  **/
  const boost::dispatch::functor<tag::iround2even_> iround2even = {};
} }
#endif

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

#endif


// Copyright John Maddock 2013.

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

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/multiprecision/cpp_bin_float.hpp>
#ifdef TEST_MPFR
#include <boost/multiprecision/mpfr.hpp>
#endif
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include "libs/multiprecision/test/test.hpp"
#include <iostream>
#include <iomanip>

template <class T>
T generate_random()
{
   typedef int e_type;
   static boost::random::mt19937 gen;
   T val = gen();
   T prev_val = -1;
   while(val != prev_val)
   {
      val *= (gen.max)();
      prev_val = val;
      val += gen();
   }
   e_type e;
   val = frexp(val, &e);

   static boost::random::uniform_int_distribution<e_type> ui(-20, 20);
   return ldexp(val, ui(gen));
}

using namespace boost::multiprecision;
#ifdef TEST_MPFR
typedef number<mpfr_float_backend<35> > good_type;
#else
typedef double good_type;
#endif
typedef number<cpp_bin_float<std::numeric_limits<good_type>::digits, digit_base_2>, et_off> test_type;

void test_special_cases()
{
   test_type max_val = (std::numeric_limits<test_type>::max)();
   test_type min_val = (std::numeric_limits<test_type>::min)();
   test_type eps = std::numeric_limits<test_type>::epsilon();
   test_type inf_val = (std::numeric_limits<test_type>::infinity)();
   test_type nan_val = (std::numeric_limits<test_type>::quiet_NaN)();
   test_type half = 0.5;
   test_type one_point_5 = 1.5;

   BOOST_CHECK((boost::math::isnormal)(max_val));
   BOOST_CHECK((boost::math::isnormal)(-max_val));
   BOOST_CHECK((boost::math::isnormal)(min_val));
   BOOST_CHECK((boost::math::isnormal)(-min_val));
   BOOST_CHECK((boost::math::isinf)(inf_val));
   BOOST_CHECK((boost::math::isinf)(-inf_val));
   BOOST_CHECK((boost::math::isnan)(nan_val));
   BOOST_CHECK((boost::math::isnan)(-nan_val));

   if(std::numeric_limits<test_type>::has_denorm)
      min_val = std::numeric_limits<test_type>::denorm_min();

   // Adding epsilon will increment 1.0:
   BOOST_CHECK(test_type(1) + eps != test_type(1));
   BOOST_CHECK(test_type(1) + eps / 2 == test_type(1));
   // But it's not the smallest value that will do that:
   test_type small = 1 + eps;
   small = ldexp(small, -std::numeric_limits<test_type>::digits);
   BOOST_CHECK(test_type(1) + small != test_type(1));
   // And if we increment 1.0 first, then an even smaller
   // addition will round up:
   test_type one_next = test_type(1) + eps;
   BOOST_CHECK(one_next + eps / 2 != one_next);

   // Overflow:
   BOOST_CHECK_EQUAL(max_val + max_val * eps, inf_val);
   BOOST_CHECK_EQUAL(-max_val - max_val * eps, -inf_val);
   BOOST_CHECK_EQUAL(max_val * 2, inf_val);
   BOOST_CHECK_EQUAL(max_val * -2, -inf_val);
   BOOST_CHECK_EQUAL(max_val / half, inf_val);
   BOOST_CHECK_EQUAL(max_val / -half, -inf_val);
   BOOST_CHECK_EQUAL(max_val / min_val, inf_val);
   BOOST_CHECK_EQUAL(max_val / -min_val, -inf_val);
   // Underflow:
   BOOST_CHECK_EQUAL(min_val * 2 - one_point_5 * min_val, 0);
   BOOST_CHECK_EQUAL(-min_val * 2 + one_point_5 * min_val, 0);
   BOOST_CHECK_EQUAL(min_val / 2, 0);
   BOOST_CHECK_EQUAL(min_val / max_val, 0);
   BOOST_CHECK_EQUAL(min_val * half, 0);
   BOOST_CHECK_EQUAL(min_val - min_val, 0);
   BOOST_CHECK_EQUAL(max_val - max_val, 0);
   BOOST_CHECK_EQUAL(-min_val + min_val, 0);
   BOOST_CHECK_EQUAL(-max_val + max_val, 0);
   // Things which should not over/underflow:
   BOOST_CHECK_EQUAL((min_val * 2) / 2, min_val);
   BOOST_CHECK_EQUAL((max_val / 2) * 2, max_val);
   BOOST_CHECK_GE((min_val * 2.0000001) / 1.9999999999999999, min_val);
   BOOST_CHECK_LE((max_val / 2.0000001) * 1.9999999999999999, max_val);
   BOOST_CHECK_EQUAL(min_val * 2 - min_val, min_val);
   BOOST_CHECK_EQUAL(max_val / 2 + max_val / 2, max_val);
   // Things involving zero:
   BOOST_CHECK_EQUAL(max_val + 0, max_val);
   BOOST_CHECK_EQUAL(max_val - 0, max_val);
   BOOST_CHECK_EQUAL(0 + max_val, max_val);
   BOOST_CHECK_EQUAL(0 - max_val, -max_val);
   BOOST_CHECK_EQUAL(max_val * 0, 0);
   BOOST_CHECK_EQUAL(0 * max_val, 0);
   BOOST_CHECK_EQUAL(max_val / 0, inf_val);
   BOOST_CHECK_EQUAL(0 / max_val, 0);
   BOOST_CHECK_EQUAL(-max_val / 0, -inf_val);
   BOOST_CHECK_EQUAL(0 / -max_val, 0);
   // Things involving infinity:
   BOOST_CHECK_EQUAL(inf_val + 2, inf_val);
   BOOST_CHECK_EQUAL(inf_val - 2, inf_val);
   BOOST_CHECK_EQUAL(inf_val + -2, inf_val);
   BOOST_CHECK_EQUAL(inf_val - -2, inf_val);
   BOOST_CHECK_EQUAL(-inf_val + 2, -inf_val);
   BOOST_CHECK_EQUAL(-inf_val - 2, -inf_val);
   BOOST_CHECK_EQUAL(-inf_val + -2, -inf_val);
   BOOST_CHECK_EQUAL(-inf_val - -2, -inf_val);

   BOOST_CHECK_EQUAL(2 + inf_val, inf_val);
   BOOST_CHECK_EQUAL(2 - inf_val, -inf_val);
   BOOST_CHECK_EQUAL(-2 + inf_val, inf_val);
   BOOST_CHECK_EQUAL(-2 - inf_val, -inf_val);
   BOOST_CHECK_EQUAL(2 + (-inf_val), -inf_val);
   BOOST_CHECK_EQUAL(2 - (-inf_val), inf_val);
   BOOST_CHECK_EQUAL(-2 + (-inf_val), -inf_val);
   BOOST_CHECK_EQUAL(-2 - (-inf_val), inf_val);

   BOOST_CHECK_EQUAL(sqrt(inf_val), inf_val);
   BOOST_CHECK(boost::math::isnan(sqrt(-inf_val)));

   BOOST_CHECK_EQUAL(inf_val + test_type(2), inf_val);
   BOOST_CHECK_EQUAL(inf_val - test_type(2), inf_val);
   BOOST_CHECK_EQUAL(inf_val + test_type(-2), inf_val);
   BOOST_CHECK_EQUAL(inf_val - test_type(-2), inf_val);
   BOOST_CHECK_EQUAL(-inf_val + test_type(2), -inf_val);
   BOOST_CHECK_EQUAL(-inf_val - test_type(2), -inf_val);
   BOOST_CHECK_EQUAL(-inf_val + test_type(-2), -inf_val);
   BOOST_CHECK_EQUAL(-inf_val - test_type(-2), -inf_val);

   BOOST_CHECK_EQUAL(test_type(2) + inf_val, inf_val);
   BOOST_CHECK_EQUAL(test_type(2) - inf_val, -inf_val);
   BOOST_CHECK_EQUAL(test_type(-2) + inf_val, inf_val);
   BOOST_CHECK_EQUAL(test_type(-2) - inf_val, -inf_val);
   BOOST_CHECK_EQUAL(test_type(2) + (-inf_val), -inf_val);
   BOOST_CHECK_EQUAL(test_type(2) - (-inf_val), inf_val);
   BOOST_CHECK_EQUAL(test_type(-2) + (-inf_val), -inf_val);
   BOOST_CHECK_EQUAL(test_type(-2) - (-inf_val), inf_val);

   BOOST_CHECK((boost::math::isnan)(inf_val - inf_val));
   BOOST_CHECK_EQUAL(inf_val * 2, inf_val);
   BOOST_CHECK_EQUAL(-inf_val * 2, -inf_val);
   BOOST_CHECK_EQUAL(inf_val * -2, -inf_val);
   BOOST_CHECK_EQUAL(-inf_val * -2, inf_val);
   BOOST_CHECK_EQUAL(inf_val * test_type(-2), -inf_val);
   BOOST_CHECK_EQUAL(-inf_val * test_type(-2), inf_val);
   BOOST_CHECK((boost::math::isnan)(inf_val * 0));
   BOOST_CHECK((boost::math::isnan)(-inf_val * 0));
   BOOST_CHECK_EQUAL(inf_val / 2, inf_val);
   BOOST_CHECK_EQUAL(-inf_val / 2, -inf_val);
   BOOST_CHECK_EQUAL(inf_val / -2, -inf_val);
   BOOST_CHECK_EQUAL(-inf_val / -2, inf_val);
   BOOST_CHECK_EQUAL(inf_val / test_type(-2), -inf_val);
   BOOST_CHECK_EQUAL(-inf_val / test_type(-2), inf_val);
   BOOST_CHECK_EQUAL(inf_val / 0, inf_val);
   BOOST_CHECK_EQUAL(-inf_val / 0, -inf_val);
   BOOST_CHECK((boost::math::isnan)(inf_val / inf_val));
   BOOST_CHECK((boost::math::isnan)(-inf_val / inf_val));
   // Things involving nan:
   BOOST_CHECK((boost::math::isnan)(nan_val + 2));
   BOOST_CHECK((boost::math::isnan)(nan_val - 2));
   BOOST_CHECK((boost::math::isnan)(nan_val + 0));
   BOOST_CHECK((boost::math::isnan)(nan_val - 0));
   BOOST_CHECK((boost::math::isnan)(nan_val + inf_val));
   BOOST_CHECK((boost::math::isnan)(nan_val - inf_val));
   BOOST_CHECK((boost::math::isnan)(nan_val + nan_val));
   BOOST_CHECK((boost::math::isnan)(nan_val - nan_val));
   BOOST_CHECK((boost::math::isnan)(2 + nan_val));
   BOOST_CHECK((boost::math::isnan)(2 - nan_val));
   BOOST_CHECK((boost::math::isnan)(0 - nan_val));
   BOOST_CHECK((boost::math::isnan)(0 - nan_val));
   BOOST_CHECK((boost::math::isnan)(inf_val + nan_val));
   BOOST_CHECK((boost::math::isnan)(inf_val - nan_val));
   BOOST_CHECK((boost::math::isnan)(nan_val * 2));
   BOOST_CHECK((boost::math::isnan)(nan_val / 2));
   BOOST_CHECK((boost::math::isnan)(nan_val * 0));
   BOOST_CHECK((boost::math::isnan)(nan_val / 0));
   BOOST_CHECK((boost::math::isnan)(nan_val * inf_val));
   BOOST_CHECK((boost::math::isnan)(nan_val / inf_val));
   BOOST_CHECK((boost::math::isnan)(nan_val * nan_val));
   BOOST_CHECK((boost::math::isnan)(nan_val / nan_val));
   BOOST_CHECK((boost::math::isnan)(2 * nan_val));
   BOOST_CHECK((boost::math::isnan)(2 / nan_val));
   BOOST_CHECK((boost::math::isnan)(0 / nan_val));
   BOOST_CHECK((boost::math::isnan)(0 / nan_val));
   BOOST_CHECK((boost::math::isnan)(inf_val * nan_val));
   BOOST_CHECK((boost::math::isnan)(inf_val / nan_val));
   // Corner cases:
   BOOST_CHECK_EQUAL((max_val * half) / half, max_val);
   BOOST_CHECK_EQUAL((max_val / 2) * 2, max_val);
   BOOST_CHECK_EQUAL((min_val / half) * half, min_val);
   BOOST_CHECK_EQUAL((min_val * 2) / 2, min_val);
   BOOST_CHECK_EQUAL(max_val + min_val, max_val);
   BOOST_CHECK_EQUAL(min_val + max_val, max_val);
   BOOST_CHECK_EQUAL(max_val - min_val, max_val);
   BOOST_CHECK_EQUAL(min_val - max_val, -max_val);
   // Signed zeros:
   BOOST_CHECK(boost::math::signbit(min_val * -min_val));
   BOOST_CHECK(boost::math::signbit(min_val * min_val) == 0);
   BOOST_CHECK(boost::math::signbit(-min_val * -min_val) == 0);
   BOOST_CHECK(boost::math::signbit(-min_val * min_val));
   BOOST_CHECK(boost::math::signbit(min_val / max_val) == 0);
   BOOST_CHECK(boost::math::signbit(min_val / -max_val));
   BOOST_CHECK(boost::math::signbit(-min_val / -max_val) == 0);
   BOOST_CHECK(boost::math::signbit(-min_val / max_val));
   BOOST_CHECK(boost::math::signbit(min_val / 2) == 0);
   BOOST_CHECK(boost::math::signbit(min_val / -2));
   BOOST_CHECK(boost::math::signbit(-min_val / -2) == 0);
   BOOST_CHECK(boost::math::signbit(-min_val / 2));
   test_type neg_zero = min_val * -min_val;
   // Arithmetic involving signed zero:
   BOOST_CHECK_EQUAL(-neg_zero, 0);
   BOOST_CHECK(!boost::math::signbit(-neg_zero));
   BOOST_CHECK_EQUAL(neg_zero + 2, 2);
   BOOST_CHECK_EQUAL(neg_zero + test_type(2), 2);
   BOOST_CHECK_EQUAL(2 + neg_zero, 2);
   BOOST_CHECK_EQUAL(test_type(2) + neg_zero, 2);
   BOOST_CHECK_EQUAL(neg_zero + -2, -2);
   BOOST_CHECK_EQUAL(neg_zero + test_type(-2), -2);
   BOOST_CHECK_EQUAL(-2 + neg_zero, -2);
   BOOST_CHECK_EQUAL(test_type(-2) + neg_zero, -2);
   BOOST_CHECK_EQUAL(neg_zero - 2, -2);
   BOOST_CHECK_EQUAL(neg_zero - test_type(2), -2);
   BOOST_CHECK_EQUAL(2 - neg_zero, 2);
   BOOST_CHECK_EQUAL(test_type(2) - neg_zero, 2);
   BOOST_CHECK_EQUAL(neg_zero - -2, 2);
   BOOST_CHECK_EQUAL(neg_zero - test_type(-2), 2);
   BOOST_CHECK_EQUAL(-2 - neg_zero, -2);
   BOOST_CHECK_EQUAL(test_type(-2) - neg_zero, -2);

   BOOST_CHECK_EQUAL(neg_zero * 2, 0);
   BOOST_CHECK_EQUAL(neg_zero * test_type(2), 0);
   BOOST_CHECK_EQUAL(2 * neg_zero, 0);
   BOOST_CHECK_EQUAL(test_type(2) * neg_zero, 0);
   BOOST_CHECK_EQUAL(neg_zero * -2, 0);
   BOOST_CHECK_EQUAL(neg_zero * test_type(-2), 0);
   BOOST_CHECK_EQUAL(-2 * neg_zero, 0);
   BOOST_CHECK_EQUAL(test_type(-2) * neg_zero, 0);
   BOOST_CHECK(boost::math::signbit(neg_zero * 2));
   BOOST_CHECK(boost::math::signbit(neg_zero * test_type(2)));
   BOOST_CHECK(boost::math::signbit(2 * neg_zero));
   BOOST_CHECK(boost::math::signbit(test_type(2) * neg_zero));
   BOOST_CHECK(!boost::math::signbit(neg_zero * -2));
   BOOST_CHECK(!boost::math::signbit(neg_zero * test_type(-2)));
   BOOST_CHECK(!boost::math::signbit(-2 * neg_zero));
   BOOST_CHECK(!boost::math::signbit(test_type(-2) * neg_zero));

   BOOST_CHECK_EQUAL(neg_zero / 2, 0);
   BOOST_CHECK_EQUAL(neg_zero / test_type(2), 0);
   BOOST_CHECK_EQUAL(2 / neg_zero, -inf_val);
   BOOST_CHECK_EQUAL(test_type(2) / neg_zero, -inf_val);
   BOOST_CHECK_EQUAL(neg_zero / -2, 0);
   BOOST_CHECK_EQUAL(neg_zero / test_type(-2), 0);
   BOOST_CHECK_EQUAL(-2 / neg_zero, inf_val);
   BOOST_CHECK_EQUAL(test_type(-2) / neg_zero, inf_val);
   BOOST_CHECK(boost::math::signbit(neg_zero / 2));
   BOOST_CHECK(boost::math::signbit(neg_zero / test_type(2)));
   BOOST_CHECK(boost::math::signbit(2 / neg_zero));
   BOOST_CHECK(boost::math::signbit(test_type(2) / neg_zero));
   BOOST_CHECK(!boost::math::signbit(neg_zero / -2));
   BOOST_CHECK(!boost::math::signbit(neg_zero / test_type(-2)));
   BOOST_CHECK(!boost::math::signbit(-2 / neg_zero));
   BOOST_CHECK(!boost::math::signbit(test_type(-2) / neg_zero));

   BOOST_CHECK(boost::math::signbit(neg_zero.convert_to<double>()));
   BOOST_CHECK(boost::math::signbit(neg_zero.convert_to<float>()));
   BOOST_CHECK(boost::math::signbit(neg_zero.convert_to<long double>()));
   test_type zero(0);
   BOOST_CHECK(!boost::math::signbit(zero.convert_to<double>()));
   BOOST_CHECK(!boost::math::signbit(zero.convert_to<float>()));
   BOOST_CHECK(!boost::math::signbit(zero.convert_to<long double>()));

   // Conversions to other types of special values:
   if(std::numeric_limits<float>::has_infinity)
   {
      BOOST_CHECK_EQUAL(inf_val.convert_to<float>(), std::numeric_limits<float>::infinity());
      BOOST_CHECK_EQUAL((-inf_val).convert_to<float>(), -std::numeric_limits<float>::infinity());
   }
   if(std::numeric_limits<float>::has_quiet_NaN)
   {
      BOOST_CHECK((boost::math::isnan)(nan_val.convert_to<float>()));
   }
   if(std::numeric_limits<double>::has_infinity)
   {
      BOOST_CHECK_EQUAL(inf_val.convert_to<double>(), std::numeric_limits<double>::infinity());
      BOOST_CHECK_EQUAL((-inf_val).convert_to<double>(), -std::numeric_limits<double>::infinity());
   }
   if(std::numeric_limits<double>::has_quiet_NaN)
   {
      BOOST_CHECK((boost::math::isnan)(nan_val.convert_to<double>()));
   }
   if(std::numeric_limits<long double>::has_infinity)
   {
      BOOST_CHECK_EQUAL(inf_val.convert_to<long double>(), std::numeric_limits<long double>::infinity());
      BOOST_CHECK_EQUAL((-inf_val).convert_to<long double>(), -std::numeric_limits<long double>::infinity());
   }
   if(std::numeric_limits<long double>::has_quiet_NaN)
   {
      BOOST_CHECK((boost::math::isnan)(nan_val.convert_to<long double>()));
   }
}

int main()
{
   test_special_cases();
   unsigned error_count = 0;
   for(unsigned i = 0; i < 100000; ++i)
   {
      good_type a = generate_random<good_type>();
      good_type b = generate_random<good_type>();
      test_type ta(a);
      test_type tb(b);

      BOOST_CHECK_EQUAL(test_type(a * b), ta * tb);
      BOOST_CHECK_EQUAL(test_type(-a * b), -ta * tb);
      BOOST_CHECK_EQUAL(test_type(a * -b), ta * -tb);
      BOOST_CHECK_EQUAL(test_type(-a * -b), -ta * -tb);

      BOOST_CHECK_EQUAL(test_type(a + b), ta + tb);
      BOOST_CHECK_EQUAL(test_type(-a + b), -ta + tb);
      BOOST_CHECK_EQUAL(test_type(a + -b), ta + -tb);
      BOOST_CHECK_EQUAL(test_type(-a + -b), -ta + -tb);

      BOOST_CHECK_EQUAL(test_type(a - b), ta - tb);
      BOOST_CHECK_EQUAL(test_type(-a - b), -ta - tb);
      BOOST_CHECK_EQUAL(test_type(a - -b), ta - -tb);
      BOOST_CHECK_EQUAL(test_type(-a - -b), -ta - -tb);

      BOOST_CHECK_EQUAL(test_type(a / b), ta / tb);
      BOOST_CHECK_EQUAL(test_type(-a / b), -ta / tb);
      BOOST_CHECK_EQUAL(test_type(a / -b), ta / -tb);
      BOOST_CHECK_EQUAL(test_type(-a / -b), -ta / -tb);

      BOOST_CHECK_EQUAL(test_type(sqrt(a)), sqrt(ta));
      BOOST_CHECK_EQUAL(test_type(floor(a)), floor(ta));
      BOOST_CHECK_EQUAL(test_type(floor(-a)), floor(-ta));
      BOOST_CHECK_EQUAL(test_type(ceil(a)), ceil(ta));
      BOOST_CHECK_EQUAL(test_type(ceil(-a)), ceil(-ta));

#ifdef TEST_MPFR
      //
      // Conversions:
      //
      BOOST_CHECK_EQUAL(a.convert_to<double>(), ta.convert_to<double>());
      BOOST_CHECK_EQUAL(a.convert_to<float>(), ta.convert_to<float>());
      BOOST_CHECK_EQUAL(b.convert_to<double>(), tb.convert_to<double>());
      BOOST_CHECK_EQUAL(b.convert_to<float>(), tb.convert_to<float>());
#else
      BOOST_CHECK_EQUAL(a, ta.convert_to<double>());
      BOOST_CHECK_EQUAL(static_cast<float>(a), ta.convert_to<float>());
      BOOST_CHECK_EQUAL(b, tb.convert_to<double>());
      BOOST_CHECK_EQUAL(static_cast<float>(b), tb.convert_to<float>());
#endif

      static boost::random::mt19937 i_gen;

      int si = i_gen();
      BOOST_CHECK_EQUAL(test_type(a * si), ta * si);
      BOOST_CHECK_EQUAL(test_type(-a * si), -ta * si);
      BOOST_CHECK_EQUAL(test_type(-a * -si), -ta * -si);
      BOOST_CHECK_EQUAL(test_type(a * -si), ta * -si);
      unsigned ui = std::abs(si);
      BOOST_CHECK_EQUAL(test_type(a * ui), ta * ui);
      BOOST_CHECK_EQUAL(test_type(-a * ui), -ta * ui);

      // Divide:
      BOOST_CHECK_EQUAL(test_type(a / si), ta / si);
      BOOST_CHECK_EQUAL(test_type(-a / si), -ta / si);
      BOOST_CHECK_EQUAL(test_type(-a / -si), -ta / -si);
      BOOST_CHECK_EQUAL(test_type(a / -si), ta / -si);
      BOOST_CHECK_EQUAL(test_type(a / ui), ta / ui);
      BOOST_CHECK_EQUAL(test_type(-a / ui), -ta / ui);
      // Error reporting:
      if(boost::detail::test_errors() != error_count)
      {
         error_count = boost::detail::test_errors();
         std::cout << std::setprecision(std::numeric_limits<test_type>::max_digits10) << std::scientific;
         std::cout << "a (mpfr) = " << a << std::endl;
         std::cout << "a (test) = " << ta << std::endl;
         std::cout << "b (mpfr) = " << b << std::endl;
         std::cout << "b (test) = " << tb << std::endl;
         std::cout << "si       = " << si << std::endl;
         std::cout << "ui       = " << ui << std::endl;
      }
   }
   return boost::report_errors();
}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_POINT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_POINT_HPP

#include <cstddef>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/views/detail/indexed_point_view.hpp>

#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/algorithms/detail/normalize.hpp>

#include <boost/geometry/algorithms/detail/envelope/transform_units.hpp>

#include <boost/geometry/algorithms/dispatch/envelope.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace envelope
{

template <std::size_t Dimension, std::size_t DimensionCount>
struct envelope_one_point
{
    template <std::size_t Index, typename Point, typename Box>
    static inline void apply(Point const& point, Box& mbr)
    {
        detail::indexed_point_view<Box, Index> box_corner(mbr);
        detail::conversion::point_to_point
            <
                Point,
                detail::indexed_point_view<Box, Index>,
                Dimension,
                DimensionCount
            >::apply(point, box_corner);
    }

    template <typename Point, typename Box>
    static inline void apply(Point const& point, Box& mbr)
    {
        apply<min_corner>(point, mbr);
        apply<max_corner>(point, mbr);
    }
};

struct envelope_point_on_spheroid
{
    template<typename Point, typename Box>
    static inline void apply(Point const& point, Box& mbr)
    {
        Point normalized_point = detail::return_normalized<Point>(point);

        typename point_type<Box>::type box_point;

        // transform units of input point to units of a box point
        transform_units(normalized_point, box_point);

        geometry::set<min_corner, 0>(mbr, geometry::get<0>(box_point));
        geometry::set<min_corner, 1>(mbr, geometry::get<1>(box_point));

        geometry::set<max_corner, 0>(mbr, geometry::get<0>(box_point));
        geometry::set<max_corner, 1>(mbr, geometry::get<1>(box_point));

        envelope_one_point
            <
                2, dimension<Point>::value
            >::apply(normalized_point, mbr);
    }
};

}} // namespace detail::envelope
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Point, typename CS_Tag>
struct envelope<Point, point_tag, CS_Tag>
    : detail::envelope::envelope_one_point<0, dimension<Point>::value>
{};

template <typename Point>
struct envelope<Point, point_tag, spherical_equatorial_tag>
    : detail::envelope::envelope_point_on_spheroid
{};

template <typename Point>
struct envelope<Point, point_tag, geographic_tag>
    : detail::envelope::envelope_point_on_spheroid
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_POINT_HPP