/**
 * @file
 * @author  [Yi-Mu "Enoch" Chen](https://github.com/yimuchen)
 * @brief   Implementing measurement ostream interaction.
 */
#ifdef CMSSW_GIT_HASH
#include "UserUtils/Common/interface/Maths.hpp"
#include "UserUtils/Common/interface/STLUtils/StringUtils.hpp"
#include "UserUtils/MathUtils/interface/Measurement.hpp"
#else
#include "UserUtils/Common/Maths.hpp"
#include "UserUtils/Common/STLUtils/StringUtils.hpp"
#include "UserUtils/MathUtils/Measurement.hpp"
#endif

#include <boost/algorithm/string.hpp>
#include <regex>
#include <string>

namespace usr
{

namespace fmt {

/**
 * @brief Re-implementing a double input for common fmt interface
 *
 * This construction means that the measurement without uncertainty would
 * output the same values as if using a double.
 */
decimal::decimal( const double input, const int p ) :
  _central( input ),
  _upper( 0 ),
  _lower( 0 )
{
  precision( p );
}

/**
 * @brief normal construction for typical measurements.
 */
decimal::decimal( const Measurement& input, const int p ) :
  _central( input.CentralValue() ),
  _upper( input.AbsUpperError() ),
  _lower( input.AbsLowerError() )
{
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief Construct using a RooRealVar status.
 */
decimal::decimal( const RooRealVar& input, const int p ) :
  _central( input.getVal() ),
  _upper( fabs( input.getErrorHi() ) ),
  _lower( fabs( input.getErrorLo() ) )
{
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief Construct using a RooRealVar status.
 */
decimal::decimal( const RooRealVar* input, const int p ) :
  _central( input->getVal() ),
  _upper( fabs( input->getErrorHi() ) ),
  _lower( fabs( input->getErrorLo() ) )
{
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief main operation for creating a latex string.
 *
 * @details Note that this function essentially calls the base::decimal
 * methods for creating the string representation of a double three times,
 * meaning that if the central value and uncertainties are of wildly different
 * orders of magnitude, the output could look very weird.
 */
std::string
decimal::str() const
{
  if( _upper == _lower && _upper == 0 ){
    return base::decimal( _central ).dupsetting( *this ).str();
  } else {
    const std::string cen = base::decimal( _central ).dupsetting( *this ).str();
    const std::string up  = base::decimal( _upper ).dupsetting( *this ).str();
    const std::string lo  = base::decimal( _lower ).dupsetting( *this ).str();

    if( up == lo ){// string-wise comparison!
      return usr::fstr( "%s\\pm%s", cen, up );
    } else {
      return usr::fstr( "%s^{+%s}_{-%s}", cen, up, lo );
    }
  }
}

/**
 * @brief Base precision overloaded to allow for autoamtic precision setting
 *        with negative settings.
 */
decimal&
decimal::precision( const int p )
{
  if( p < 0 && ( _upper != 0 || _lower != 0 ) ){
    SetPrecision();
  } else {
    base::format::precision( abs( p ) );
  }
  return *this;
}

/**
 * @brief automatic precision setting for the decimal representation.
 * @details The precision is set such that the larger uncertain would display
 *          at least two significant digits (0 if larger uncertainty is greater
 *          than 1).
 */
void
decimal::SetPrecision()
{
  const double op_unc = std::max( _upper, _lower );
  const int exp       = GetExponent( op_unc );
  if( exp > 0 ){
    _precision = 0;
  } else {
    _precision = -exp+1;
  }
}

/*-----------------------------------------------------------------------------
 *  Scientific notation implementation functions
   --------------------------------------------------------------------------*/

/**
 * @brief allowing double input to avoid multiple interfaces for double input.
 *
 * This construction means that the measurement without uncertainty would
 * output the same values as if using a double.
 */
scientific::scientific( const double input, const unsigned p ) :
  _central( input ),
  _upper( 0 ),
  _lower( 0 ),
  _exp( 0 )
{
  precision( p );
  SetExponent();
}

/**
 * @brief simple construction with a measurement with uncertainties.
 */
scientific::scientific( const Measurement& input, const int p ) :
  _central( input.CentralValue() ),
  _upper( input.AbsUpperError() ),
  _lower( input.AbsLowerError() ),
  _exp( 0 )
{
  SetExponent();
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief Construct using a RooRealVar instance
 */
scientific::scientific( const RooRealVar& input, const int p ) :
  _central( input.getVal() ),
  _upper( fabs( input.getErrorHi() ) ),
  _lower( fabs( input.getErrorLo() ) ),
  _exp( 0 )
{
  SetExponent();
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief Construct using a RooRealVar instance
 */
scientific::scientific( const RooRealVar* input, const int p ) :
  _central( input->getVal() ),
  _upper( fabs( input->getErrorHi() ) ),
  _lower( fabs( input->getErrorLo() ) ),
  _exp( 0 )
{
  SetExponent();
  if( p < 0 ){
    SetPrecision();
  } else {
    precision( p );
  }
}

/**
 * @brief implementing the virtual function.
 *
 * The function attempts to use the simplest form to represent the uncertainty,
 * using as little latex symbols as possible.
 */
std::string
scientific::str() const
{
  const std::string cen  = base::decimal( _central ).dupsetting( *this ).str();
  const std::string up   = base::decimal( _upper ).dupsetting( *this ).str();
  const std::string lo   = decimal( _lower ).dupsetting( *this ).str();
  const std::string base =
    ( up == lo && _upper == 0 ) ? cen :
    ( up == lo )                ? usr::fstr( "%s\\pm%s", cen, up ) :
    usr::fstr( "%s^{+%s}_{-%s}", cen, up, lo );

  const std::string ans =
    ( _exp == 0 )               ? base :
    ( up == lo && _upper != 0 ) ? usr::fstr( "(%s)\\times10^{%d}", base, _exp ) :
    usr::fstr( "%s\\times10^{%d}", base, _exp );

  return ans;
}

/**
 * @brief overloading base implementation to allow for precision autodetection
 *        with a negative input.
 */
scientific&
scientific::precision( const int i )
{
  if( i < 0 && ( _upper != 0 || _lower != 0 ) ){
    SetPrecision();
  } else {
    base::format::precision( abs( i ) );
  }
  return *this;
}

/**
 * @brief reducing/magnifying the central value and uncertainties by a common
 *        exponent value.
 *
 * If the central value is none-zero, then extracting the exponent of the
 * central value such that \f$1 < |\mathrm{central}_\mathrm{man}| < 10\f$;
 * If the central value is zero, then the exponent is determined by the
 * larger of the uncertainties, if it is non-zero.
 * If everything is zero, then the exponent is not set.
 */
void
scientific::SetExponent()
{
  if( _central != 0 ){
    _exp      = GetExponent( _central );
    _central /= IntPower( 10, _exp );
    _upper   /= IntPower( 10, _exp );
    _lower   /= IntPower( 10, _exp );
  } else if( _upper != 0 || _lower != 0  ){
    _exp      = std::max( GetExponent( _upper ), GetExponent( _lower ) );
    _central /= IntPower( 10, _exp );
    _upper   /= IntPower( 10, _exp );
    _lower   /= IntPower( 10, _exp );
  }
}

/**
 * @brief automatically determining the precision to use.
 *
 * If the central value is smaller than the larger uncertainty, then at most
 * the precision is 1 (if the uncertainty after the exponent has been factored
 * out smaller than 10).
 * if the central value is larger than the larger uncertainty, the precision
 * is set such that two significant digits of the larger uncertainty is
 * displayed.
 */
void
scientific::SetPrecision()
{
  double op_unc = std::max( _upper, _lower );
  if( op_unc > _central ){
    if( op_unc < 10 ){
      _precision = 1;
    } else {
      _precision = 0;
    }
  } else {
    _precision = 1;

    while( op_unc < 1 && op_unc > 0 ){
      _precision++;
      op_unc *= 10;
    }
  }
}

}/* fmt */

/**
 * @brief template specialization for a json file parsing.
 *
 * reading a list of doubles with 1-3 parameters as a measurement class.
 * * If only an empty list exists at the address, then the unit measurement
 *   (1+-0), * is returned.
 * * If only the list is only a single double long, a measurement with no
 *   uncertainty is returned.
 * * If only two double exist, then a measurement with symmetric uncertainty is
 *   returned. (x[0] +- x[1])
 * * If more than three doubles exists, then the first three doubles will be
 *   used to construct a measurement with asymmetric uncertainty:
 *   (x[0] +x[1] -x[2] )
 */
template<>
void
ExceptJSONEntry<Measurement>( const JSONMap&     map,
                              const std::string& index )
{
  ExceptJSONList( map, index );
}

template<>
Measurement
JSONEntry<Measurement>( const JSONMap&     map,
                        const std::string& index )
{
  const auto array = JSONList<double>( map, index );
  if( array.size() == 0 ){
    throw std::invalid_argument( "Array for usr::Measurement cannot be empty" );
  } else if( array.size() == 1 ){
    return usr::Measurement( array.at( 0 ), 0, 0 );
  } else if( array.size() == 2 ){
    return usr::Measurement( array.at( 0 ), array.at( 1 ), array.at( 1 ) );
  } else {
    return usr::Measurement( array.at( 0 ), array.at( 1 ), array.at( 2 ) );
  }
}

template<>
Measurement
JSONEntry<Measurement>( const JSONMap&     map,
                        const std::string& index,
                        const Measurement& def  )
{
  if( map.HasMember( index.c_str() ) ){
    return JSONEntry<Measurement>( map, index );
  } else {
    return def;
  }
}

}/* usr */


#include <math.h>
#include <Eigen/Core>
#include <algorithm>
#include <functional>

#include "crf_op.h"

namespace caffe2 {

namespace {

template <typename T>
void bilinear_interpolation(const float* input, float* output,
                            const int batch_size, const int num_channels,
                            const int input_height, const int input_width,
                            const int output_height, const int output_width) {
  int channels = num_channels * batch_size;

  const float rheight = (output_height > 1)
                            ? (float)(input_height - 1) / (output_height - 1)
                            : 0.f;
  const float rwidth =
      (output_width > 1) ? (float)(input_width - 1) / (output_width - 1) : 0.f;
  for (int h2 = 0; h2 < output_height; ++h2) {
    const float h1r = rheight * h2;
    const int h1 = h1r;
    const int h1p = (h1 < input_height - 1) ? 1 : 0;
    const float h1lambda = h1r - h1;
    const float h0lambda = (float)1. - h1lambda;
    for (int w2 = 0; w2 < output_width; ++w2) {
      const float w1r = rwidth * w2;
      const int w1 = w1r;
      const int w1p = (w1 < input_width - 1) ? 1 : 0;
      const float w1lambda = w1r - w1;
      const float w0lambda = (float)1. - w1lambda;
      const float* Xdata = &input[h1 * input_width + w1];
      float* Ydata = &output[h2 * output_width + w2];
      for (int c = 0; c < channels; ++c) {
        Ydata[0] = h0lambda * (w0lambda * Xdata[0] + w1lambda * Xdata[w1p]) +
                   h1lambda * (w0lambda * Xdata[h1p * input_width] +
                               w1lambda * Xdata[h1p * input_width + w1p]);
        Xdata += input_width * input_height;
        Ydata += output_width * output_height;
      }
    }
  }
}

template <typename T>
void image_process(const float* input, unsigned char* output,
                   const int batch_size, const int height, const int width) {
  // TODO(YH): add argument
  float mean[] = {102.9801, 115.9465, 122.7717};
  for (int b = 0; b < batch_size; b++) {
    for (int c = 0; c < 3; c++) {
      for (int h = 0; h < height; h++) {
        for (int w = 0; w < width; w++) {
          int idx_i = ((b * 3 + c) * height + h) * width + w;
          int idx_o = ((b * height + h) * width + w) * 3 + c;
          output[idx_o] = (unsigned char)(input[idx_i] + mean[c]);
        }
      }
    }
  }
}

template <typename T>
void unary_process(const float* input, float* output, const int batch_size,
                   const int num_classes, const int height, const int width) {
  const float min_prob = 0.0001;
  for (int b = 0; b < batch_size; b++) {
    for (int c = 0; c < num_classes; c++) {
      for (int h = 0; h < height; h++) {
        for (int w = 0; w < width; w++) {
          int idx_i = ((b * num_classes + c) * height + h) * width + w;
          int idx_o = ((b * height + h) * width + w) * num_classes + c;
          output[idx_o] = std::max(input[idx_i], min_prob);
          // output[idx_o] = -1. * std::max(input[idx_i], min_prob);
          // output[idx_o] = -1. * input[idx_i];
        }
      }
    }
  }
}

template <typename T>
void result_process(const float* input, float* output, const int batch_size,
                    const int num_classes, const int height, const int width) {
  const float min_prob = 0.0001;

  Tensor N(caffe2::CPU);
  N.Resize(batch_size, height, width);
  float* Nmdata = N.mutable_data<float>();

  for (int b = 0; b < batch_size; b++) {
    for (int h = 0; h < height; h++) {
      for (int w = 0; w < width; w++) {
        Nmdata[0] = 0;
        for (int c = 0; c < num_classes; c++) {
          int idx_i = ((b * height + h) * width + w) * num_classes + c;
          int idx_o = ((b * num_classes + c) * height + h) * width + w;
          output[idx_o] = std::max(input[idx_i], min_prob);
          Nmdata[0] += output[idx_o];
        }
        Nmdata += 1;
      }
    }
  }

  const float* Ndata = N.data<float>();

  for (int b = 0; b < batch_size; b++) {
    for (int c = 0; c < num_classes; c++) {
      for (int h = 0; h < height; h++) {
        for (int w = 0; w < width; w++) {
          int idx_o = ((b * num_classes + c) * height + h) * width + w;
          float norm = *(Ndata + (b * height + h) * width + w);
          // output[idx_o] = log(output[idx_o] / norm);
          output[idx_o] = output[idx_o] / norm;
        }
      }
    }
  }
}

}  // namespace

template <>
int DenseCRFOp<float, CPUContext>::npixels() {
  return W * H;
}

template <>
int DenseCRFOp<float, CPUContext>::nlabels() {
  return m_nlabels;
}

template <>
void DenseCRFOp<float, CPUContext>::add_pairwise_energy(
    float w1, float theta_alpha_1, float theta_alpha_2, float theta_betta_1,
    float theta_betta_2, float theta_betta_3, float w2, float theta_gamma_1,
    float theta_gamma_2, const unsigned char* im) {
  m_crf->addPairwiseGaussian(theta_gamma_1, theta_gamma_2,
                             new PottsCompatibility(w2));
  m_crf->addPairwiseBilateral(theta_alpha_1, theta_alpha_2, theta_betta_1,
                              theta_betta_2, theta_betta_3, im,
                              new PottsCompatibility(w1));
  // m_crf->addPairwiseGaussian(3, 3, new PottsCompatibility(3));
  // m_crf->addPairwiseBilateral(80, 80, 13, 13, 13, im,
  // new PottsCompatibility(10));
}

template <>
void DenseCRFOp<float, CPUContext>::set_unary_energy(
    const float* unary_costs_ptr) {
  m_crf->setUnaryEnergy(
      Eigen::Map<const Eigen::MatrixXf>(unary_costs_ptr, m_nlabels, W * H));
}

template <>
void DenseCRFOp<float, CPUContext>::map(int n_iters, int* labels) {
  VectorXs labels_vec = m_crf->map(n_iters);
  for (int i = 0; i < (W * H); ++i) labels[i] = labels_vec(i);
}

template <>
void DenseCRFOp<float, CPUContext>::inference(int n_iters, float* probs_out) {
  MatrixXf probs = m_crf->inference(n_iters);
  for (int i = 0; i < npixels(); ++i)
    for (int j = 0; j < nlabels(); ++j)
      probs_out[i * nlabels() + j] = probs(j, i);
}

template <>
void DenseCRFOp<float, CPUContext>::dense_crf(const unsigned char* image,
                                              const float* unary,
                                              float* probs_out) {
  // set unary potentials
  set_unary_energy(unary);

  // set pairwise potentials
  // add_pairwise_energy(10, 80 / scale_factor_, 80 / scale_factor_,
  // color_factor_, color_factor_, color_factor_, 3, 3 / scale_factor_, 3 /
  // scale_factor_, image);
  add_pairwise_energy(BI_W, BI_X_STD / scale_factor_, BI_Y_STD / scale_factor_,
                      BI_R_STD, BI_G_STD, BI_B_STD, POS_W,
                      POS_X_STD / scale_factor_, POS_Y_STD / scale_factor_,
                      image);

  // run inference
  inference(max_iter_, probs_out);
}

template <>
bool DenseCRFOp<float, CPUContext>::RunOnDevice() {
  const auto& U = Input(0);
  const auto& I = Input(1);

  CAFFE_ENFORCE_EQ(U.dim(), 4);
  CAFFE_ENFORCE_EQ(I.dim(), 4);
  CAFFE_ENFORCE_EQ(U.dim32(0), I.dim32(0));
  CAFFE_ENFORCE_EQ(I.dim32(1), 3);

  const int batch_size = U.dim32(0);
  const int num_classes = U.dim32(1);
  const int height = U.dim32(2);
  const int width = U.dim32(3);
  const int height_im = I.dim32(2);
  const int width_im = I.dim32(3);
  H = height;
  W = width;
  m_nlabels = num_classes;

  m_crf = new DenseCRF2D(W, H, m_nlabels);

  auto* M = Output(0);
  M->Resize(batch_size, num_classes, height, width);

  Tensor MT(caffe2::CPU);
  MT.Resize(batch_size, height, width, num_classes);

  Tensor IT(caffe2::CPU);
  IT.Resize(batch_size, height, width, 3);

  if (height != height_im || width != width_im) {
    Tensor IB(caffe2::CPU);
    IB.Resize(batch_size, 3, height, width);
    bilinear_interpolation<float>(I.data<float>(), IB.mutable_data<float>(),
                                  batch_size, 3, height_im, width_im, height,
                                  width);

    image_process<float>(IB.data<float>(), IT.mutable_data<unsigned char>(),
                         batch_size, height, width);
  } else {
    image_process<float>(I.data<float>(), IT.mutable_data<unsigned char>(),
                         batch_size, height, width);
  }

  Tensor UT(caffe2::CPU);
  UT.Resize(batch_size, height, width, num_classes);
  unary_process<float>(U.data<float>(), UT.mutable_data<float>(), batch_size,
                       num_classes, height, width);

  for (int b = 0; b < batch_size; b++) {
    const unsigned char* image =
        IT.data<unsigned char>() + b * height * width * 3;
    const float* unary = UT.data<float>() + b * height * width * num_classes;
    float* probs_out =
        MT.mutable_data<float>() + b * height * width * num_classes;

    // auto adjust scale_factor_
    scale_factor_ = 1.0 * SIZE_STD / std::max(height, width);

    dense_crf(image, unary, probs_out);
  }

  result_process<float>(MT.data<float>(), M->mutable_data<float>(), batch_size,
                        num_classes, height, width);

  delete m_crf;
  return true;
}

REGISTER_CPU_OPERATOR(DenseCRF, DenseCRFOp<float, CPUContext>);

namespace {}  // namespace

using namespace std::placeholders;

OPERATOR_SCHEMA(DenseCRF)
    .NumInputs(2)
    .NumOutputs(1)
    .SetDoc(R"DOC(
)DOC")
    .Arg("max_iter", "(int) default to 0")
    .Arg("debug_info", "(bool) default to false")
    .Input(0, "U", "input tensor of size (BxCxH1xW1)")
    .Input(1, "I", "input tensor of size (Bx3xH2xW2)")
    .Output(0, "M", "output tensor of size (BxCxH1xW1)");

namespace {

NO_GRADIENT(DenseCRF);

}  // namespace

}  // namespace caffe2


/**
 *  sim_onlineca_multi.cpp
 *
 *  Simulate the LTL motion planning with collision avoidance
 *  with multiple moving obstacles.
 *
 *  Created by Yinan Li on Feb. 27, 2021.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <algorithm>
#include <utility>
#include <cmath>
#include <sys/stat.h>
#include <boost/numeric/odeint.hpp>
#include <cstdlib>
#include <ctime>

#include "src/grid.h"
#include "src/definitions.h"
#include "src/abstraction.hpp"
#include "src/DBAparser.h"
#include "src/bsolver.hpp"
#include "src/patcher.h"
#include "src/hdf5io.h"

#include "car.hpp"
#include "odes.hpp"

int main(int argc, char *argv[])
{
    std::string specfile, ctlrfile, cafile, graphfile;
    specfile = "dba1.txt";
    ctlrfile = "controller_dba1_0.2-0.2-0.2.h5";
    graphfile = "gwin.h5";
    const double eta[] = {0.2, 0.2, 0.2};

    cafile = "controller_safety_abst-0.8-0.9-0.1-0.1.h5";
    const double eta_r[] = {0.1, 0.1, 0.1};

    /* Input arguments:
     * ./simMulti dbafile ctlrfile cafile graphfile eta[0] eta_r[0]
     */
    if(argc!=1) {
	if(argc==7) {
	    specfile = std::string(argv[1]);
	    ctlrfile = std::string(argv[2]);
	    cafile = std::string(argv[3]);
	    graphfile = std::string(argv[4]);
	    eta[0] = std::atof(argv[5]);
	    eta[1] = std::atof(argv[5]);
	    eta[2] = std::atof(argv[5]);
	    eta_r[0] = std::atof(argv[6]);
	    eta_r[1] = std::atof(argv[6]);
	    eta_r[2] = std::atof(argv[6]);
	} else {
	    std::cout << "Improper number of arguments.\n";
	    std::exit(1);
	}
    }
    std::cout << "Simulate " << specfile << " with controller " << ctlrfile << '\n';

    /**
     * Setup the motion planning workspace
     **/
    const int x_dim = 3;
    const double theta = 3.5;
    const double xlb[] = {0, 0, -theta};
    const double xub[] = {10, 10, theta};
    /* set the control values */
    const int u_dim = 2;
    const double ulb[] = {-1.0, -1.0};
    double uub[] = {1.0, 1.0};
    /* discretization precision */
    double mu[] = {0.3, 0.3};
    /* generate grid */
    rocs::grid x_grid(x_dim,eta,xlb,xub);
    x_grid.gridding();
    rocs::grid u_grid(u_dim,mu,ulb,uub);
    std::cout << "Number of discrete states: " << x_grid._nv << "\n";
    std::cout << "Number of discrete inputs: " << u_grid._nv << "\n";

    clock_t tb, te;

    /**
     * Load specification
     **/
    std::cout << "\nReading the specification...\n";
    rocs::UintSmall nAP = 0, nNodes = 0, q0 = 0;
    std::vector<rocs::UintSmall> acc;
    std::vector<std::vector<rocs::UintSmall>> arrayM;
    if (!rocs::read_spec(specfile, nNodes, nAP, q0, arrayM, acc))
	std::exit(1);
    boost::dynamic_bitset<> isacc(nNodes, false);
    for (rocs::UintSmall i = 0; i < acc.size(); ++i)
	isacc[acc[i]] = true;

    /**
     * Load global controller
     **/
    std::cout << "\nLoading global controller...\n";
    std::vector<long long> w_x0, encode3;
    std::vector<NODE_POST> nts_ctrlr;
    std::vector<CTRL> ctrl;
    std::vector<int> q_prime;
    rocs::h5FileHandler planRdr(ctlrfile, H5F_ACC_RDONLY);
    planRdr.read_discrete_controller(w_x0, encode3, nts_ctrlr, ctrl, q_prime);

    /**
     * Load local safety controller
     **/
    std::cout << "\nLoading local safety controller...\n";
    rocs::h5FileHandler reader(cafile, H5F_ACC_RDONLY);
    std::vector<size_t> optCtlr;
    std::vector<double> value;
    boost::dynamic_bitset<> safeCtlr;
    boost::dynamic_bitset<> win;
    size_t cdims[2];
    reader.read_discrete_controller(win, safeCtlr, cdims, optCtlr, value);
    double xrlb[] = {-3, -3, -theta};
    double xrub[] = {3, 3, theta};
    rocs::grid rel_grid(3, eta_r, xrlb, xrub);
    rel_grid.gridding();

    /**
     * Launch patcher
     **/
    rocs::Patcher local;
    struct stat buffer;
    if(stat(graphfile.c_str(), &buffer) == 0) {
    	/* Read from a file */
    	std::cout << "\nReading winning graph...\n";
    	rocs::h5FileHandler graphRdr(graphfile, H5F_ACC_RDONLY);
    	tb = clock();
    	graphRdr.read_winning_graph(local);
    	te = clock();
    	std::cout << "Time of reading graph: " << (float)(te - tb)/CLOCKS_PER_SEC << '\n';
    } else {
    	std::cout << "Graph file doesn't exist.\n";
    	return 1;
    }
    int horizon = 3;  //set forward propagation horizon

    /**
     * Simulation
     **/
    const double drange[] = {3.0, 3.0};
    /* set ode solver */
    const double tsim = 0.1; //simulation time
    const double dt = 0.001; //integration step size for odeint
    boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;
    boost::dynamic_bitset<> u_safe(cdims[1]);

    /* Initial condition of the ego robot */
    rocs::Rn x{1, 1, M_PI/3.0};
    rocs::Rn u{0, 0};
    long long x_index = x_grid.val_to_id(x);
    std::vector<long long>::iterator p1;
    p1 = std::lower_bound(w_x0.begin(), w_x0.end(), x_index);
    if(*p1 != x_index) {
	std::cout << x[0] <<  " "  << x[1] << " " << x[2];
	std::cout << " is not in the winning set, change a new initial state." << std::endl;
	return 1;
    }
    /* Initial condition of 3 obstacles */
    rocs::Rn uomax{0.8, 0.8};
    rocs::Rn uomin{-0.8, -0.8};
    double v;

    /* State in local relative coordinate */
    rocs::Rn xr1{0,0,0};
    rocs::Rn xr2{0,0,0};
    rocs::Rn xr3{0,0,0};
    long long xr_id1, xr_id2, xr_id3;

    /* obstacle 1 */
    rocs::Rn xo1{3.0, 10.0, -1.5};
    rocs::Rn uo1{0.1, 0.0};
    auto obstacle_behavior1 = [&uo1, tsim](int j) {
				  double t = tsim*j;
				  if(t >= 0) {
				      uo1[0] = 0.2;
				      uo1[1] = 0.0;
				  }
			      };

    /* obstacle 2 */
    rocs::Rn xo2{5.3, 0.0, 2.0};
    rocs::Rn uo2{0.5, 0.0};
    auto obstacle_behavior2 = [&uo2, tsim](int j) {
				  double t = tsim*j;
				  if(t >= 0) {
				      if(t < 14) {
					  uo2[0] = 0.5;
					  uo2[1] = 0.0;
				      } else {
					  uo2[0] = 0.5;
					  uo2[1] = 0.6;
				      }
				  }
			      };

    /* obstacle 3 */
    rocs::Rn xo3{10.0, 6.4, M_PI};
    rocs::Rn uo3{0.15, 0.0};
    auto obstacle_behavior3 = [&uo3, tsim](int j) {
				  double t = tsim*j;
				  if(t >= 0) {
				      if(t < 31) {
					  uo3[0] = 0.2;
					  uo3[1] = 0.0;
				      } else if(t < 35) {
					  uo3[0] = 0.2;
					  uo3[1] = 0.3;
				      } else {
					  uo3[0] = 0.2;
					  uo3[1] = 0.0;
				      }
				  }
			      };

    /* Open files for writing results and logs */
    std::string simfile = "traj_closedloop_ca_4.txt";
    std::ofstream ctlrWtr(simfile);
    if(!ctlrWtr.is_open())
	ctlrWtr.open(simfile, std::ios::out);
    std::string simother1 = "traj_other_robot_4_1.txt";
    std::string simother2 = "traj_other_robot_4_2.txt";
    std::string simother3 = "traj_other_robot_4_3.txt";
    std::ofstream otherWtr1(simother1);
    std::ofstream otherWtr2(simother2);
    std::ofstream otherWtr3(simother3);
    if(!otherWtr1.is_open())
    	otherWtr1.open(simother1, std::ios::out);
    if(!otherWtr2.is_open())
    	otherWtr2.open(simother2, std::ios::out);
    if(!otherWtr3.is_open())
    	otherWtr3.open(simother3, std::ios::out);
    /********** Logging **********/
    std::ofstream logger;
    logger.open("logs_4.txt", std::ios::out);
    /********** Logging **********/

    /* Simulation loop */
    srand(time(NULL));
    float tpat = 0;
    int max_num_achieve_acc=5, max_num_iteration=1500; //3000000;
    rocs::UintSmall q;
    int i, j;
    double t0, tc; //record the time when an obstacle detected
    bool o1, o2, o3;
    o1 = false;
    o2 = false;
    o3 = false;
    std::vector<CTRL>::iterator p7;
    NODE_POST p5;
    std::cout << "\nLaunching simulation...\n";
    for(q = q0, i = j = 0; i<max_num_achieve_acc && j<max_num_iteration; ++j) {
	/* choose obstacle trajectory */
	obstacle_behavior1(j);
	obstacle_behavior2(j);
	obstacle_behavior3(j);

	/* Determine the control u by the product state (x_index, q) */
	p5 = nts_ctrlr[encode3[x_index]];
	p7 = std::lower_bound(ctrl.begin()+p5.pos, ctrl.begin()+p5.pos+p5.num_a,
			      q, [](const CTRL &item, const int val) {
				     return item.q < val;});
	if(p7->q != q) {
	    // std::cout << j << "th iteration: " << "Reach accepting state " << i << " times.\n";
	    // std::cout << "Automaton state: " << q << '\n';
	    // std::cout << "Position in ctrl list: " << p5.pos << ", number of actions: " << p5.num_a << '\n';
	    std::cout << "Error in ctrl" << std::endl;
	    break;
	}
	u_grid.id_to_val(u, p7->u);

	/* Print to screen */
	std::cout << "# " << j << ":\n";
	std::cout << "x: [" << x[0] <<  ','  << x[1] << ',' << x[2] << "]\n";
	std::cout << "Nominal control: " << p7->u << '('
		  << '[' << u[0] << ',' << u[1] << "])\n";
	std::cout << "current dba state: " << q << "\n";

	tc = j*tsim; //record the current time

	/* Get the relative state xr */
	xr1[0] = xo1[0]-x[0];
	xr1[1] = xo1[1]-x[1];
	xr1[2] = xo1[2]-x[2];
	xr2[0] = xo2[0]-x[0];
	xr2[1] = xo2[1]-x[1];
	xr2[2] = xo2[2]-x[2];
	xr3[0] = xo3[0]-x[0];
	xr3[1] = xo3[1]-x[1];
	xr3[2] = xo3[2]-x[2];
	if(xr1[2] > M_PI) //convert angle into [-pi, pi]
	    xr1[2] -= 2*M_PI;
	if(xr1[2] < -M_PI)
	    xr1[2] += 2*M_PI;
	if(xr2[2] > M_PI) //convert angle into [-pi, pi]
	    xr2[2] -= 2*M_PI;
	if(xr2[2] < -M_PI)
	    xr2[2] += 2*M_PI;
	if(xr3[2] > M_PI) //convert angle into [-pi, pi]
	    xr3[2] -= 2*M_PI;
	if(xr3[2] < -M_PI)
	    xr3[2] += 2*M_PI;
	inertia_to_body(xr1, x); //convert from the inertial to body frame
	inertia_to_body(xr2, x);
	inertia_to_body(xr3, x);

	/* Deal with obstacles */
	if(std::fabs(xr1[0])*std::fabs(xr1[0]) +
	   std::fabs(xr1[1])*std::fabs(xr1[1]) < drange[0]*drange[1]) {
	    o1 = true;
	    std::cout << "Obstacle 1 detected.\n";
	    std::cout << "Relative coordinate: ";
	    if(rel_grid._bds.isout(xr1)) { // xr is the out-of-domain node
		std::cout << rel_grid._nv;
	    } else {
		xr_id1 = rel_grid.val_to_id(xr1); //uniform grid rel_grid
		std::cout << xr_id1;
	    }
	    std::cout << "([" << xr1[0] << ',' << xr1[1] << ',' << xr1[2] << "])\n";
	} else {
	    o1 = false;
	}//end if obstacle is within range

	if(std::fabs(xr2[0])*std::fabs(xr2[0]) +
	   std::fabs(xr2[1])*std::fabs(xr2[1]) < drange[0]*drange[1]) {
	    o2 = true;
	    std::cout << "Obstacle 2 detected.\n";
	    std::cout << "Relative coordinate: ";
	    if(rel_grid._bds.isout(xr2)) { // xr is the out-of-domain node
		std::cout << rel_grid._nv;
	    } else {
		xr_id2 = rel_grid.val_to_id(xr2); //uniform grid rel_grid
		std::cout << xr_id2;
	    }
	    std::cout << "([" << xr2[0] << ',' << xr2[1] << ',' << xr2[2] << "])\n";
	} else {
	    o2 = false;
	}//end if obstacle is within range

	if(std::fabs(xr3[0])*std::fabs(xr3[0]) +
	   std::fabs(xr3[1])*std::fabs(xr3[1]) < drange[0]*drange[1]) {
	    o3 = true;
	    std::cout << "Obstacle 3 detected.\n";
	    std::cout << "Relative coordinate: ";
	    if(rel_grid._bds.isout(xr3)) { // xr is the out-of-domain node
		std::cout << rel_grid._nv;
	    } else {
		xr_id3 = rel_grid.val_to_id(xr3); //uniform grid rel_grid
		std::cout << xr_id3;
	    }
	    std::cout << "([" << xr3[0] << ',' << xr3[1] << ',' << xr3[2] << "])\n";
	} else {
	    o3 = false;
	}//end if obstacle is within range

	/* Perform RH (receding horizon) */
	if(o1||o2||o3) {
	    /********** Logging **********/
	    logger << tc << '(' << j << "): "
	       << x_index << '(' << x[0] << ',' << x[1] << ',' << x[2] << "), ";
	    if(o1) {
		logger << "o1," << xr_id1 << '(' << xr1[0] << ',' << xr1[1] << ',' << xr1[2] << "), ";
		for(size_t k = 0; k < cdims[1]; ++k)
		    logger << safeCtlr[xr_id1*cdims[1]+k];
		logger << '\n';
	    }
	    if(o2) {
		logger << "o2," << xr_id2 << '(' << xr2[0] << ',' << xr2[1] << ',' << xr2[2]<< "), ";
		for(size_t k = 0; k < cdims[1]; ++k)
		    logger << safeCtlr[xr_id2*cdims[1]+k];
		logger << '\n';
	    }
	    if(o3) {
		logger << "o3," << xr_id3 << '(' << xr3[0] << ',' << xr3[1] << ',' << xr3[2] << ")";
		for(size_t k = 0; k < cdims[1]; ++k)
		    logger << safeCtlr[xr_id3*cdims[1]+k];
		logger << '\n';
	    }
	    std::cout << "o1:"<< o1 <<", o2:"<< o2 <<", o3:" << o3 <<'\n';
	    // std::cout << "Current state id of the robot:" << xp << ", key:" << key << '\n';
	    /********** Logging **********/

	    size_t xp = x_index*nNodes+q; // (idx,idq)=idx*nNodes+idq (buchi.c)
	    size_t uid;

	    /********** Logging **********/
	    size_t row, key = local._encode[xp];
	    row = local._idmap[key];
	    // std::cout << "id_n0n1: " << xp << ',' << " id_np: " << key << '\n';
	    for(size_t k = 0; k < u_grid._nv; ++k) {
		if(local._winfts._npost[row*local._na+k]) {//outdeg>0
		    logger << k << ' ';
		}
	    }
	    logger << ", ";
	    /********** Logging **********/

	    /* Determine safe inputs for RH module */
	    std::cout << "Use RH to find safe control inputs.\n ";
	    u_safe.set();
	    int nSafeAct = 0;
	    rocs::Rn uu(u_dim);
	    double dtemp, u_dist=(uub[0]-ulb[0])*(uub[0]-ulb[0])+(uub[1]-ulb[1])*(uub[1]-ulb[1]);
	    for(size_t k = 0; k < cdims[1]; ++k) {
		if(o1)
		    u_safe[k] = u_safe[k] & safeCtlr[xr_id1*cdims[1]+k];
		if(o2)
		    u_safe[k] = u_safe[k] & safeCtlr[xr_id2*cdims[1]+k];
		if(o3)
		    u_safe[k] = u_safe[k] & safeCtlr[xr_id3*cdims[1]+k];

		if(u_safe[k]) {
		    /************* Logging **************/
		    // std::cout << k << ',';
		    // std::cout << "Outdeg: "
		    // 	      << local._winfts._npost[row*local._na+k]
		    // 	      << '\n';
		    /************* Logging **************/
		    if(local._winfts._npost[row*local._na+k]) {//outdeg>0
			++nSafeAct; //count the # of safe valid controls
			/* take the closest to the original one */
			u_grid.id_to_val(uu, k);
			dtemp = (uu[0]-u[0])*(uu[0]-u[0])+(uu[1]-u[1])*(uu[1]-u[1]);
			if(dtemp < u_dist) {
			    uid = k;
			    u_dist= dtemp;
			}
			/************* Logging **************/
			// std::cout << "Post nodes: ";
			// size_t pidstart=local._winfts._ptrpost[row*local._na+k];
			// for(size_t pid=pidstart; pid<pidstart+local._winfts._npost[row*local._na+k]; ++pid)
			//     std::cout << local._winfts._idpost[pid] << ' ';
			// std::cout << '\n';
			/************* Logging **************/
		    }
		    // /************* Logging **************/
		    // logger << k << ' ';
		    // /************* Logging **************/
		}
		/********** Logging **********/
		logger << u_safe[k];
		/********** Logging **********/
	    }
	    /********** Logging **********/
	    u_grid.id_to_val(uu, uid);
	    logger << '\n'
		   << p7->u << ' ' << u[0] << ',' << u[1] << ';'
		   << uid << ' ' << uu[0] << ',' << uu[1] << ' ' << u_dist << ';';
	    /********** Logging **********/

	    /* Get a safe control from patcher */
	    if(u_safe[p7->u]) {
		std::cout << "The static strategy is safe.\n";
		// u_grid.id_to_val(u, uid);
		logger << p7->u << ' ';
	    } else if(nSafeAct > 1) {
		tb = clock();
		int suc = local.solve_local_reachability(xp, horizon, u_safe);
		te = clock();
		tpat += (float)(te - tb)/CLOCKS_PER_SEC;
		std::cout << "Average time for RH: "
			  << tpat/(j+1) << ".\n";
		if(!suc) {//if fails in re-planning, use the closest u
		    std::cout << "Apply the safe control input closest to the original one.\n";
		    u_grid.id_to_val(u, uid);
		    logger << uid << ' ';
		} else {
		    std::cout << "Apply controls returned by local reachability.\n";
		    u_grid.id_to_val(u, local._ctlr);
		    logger << local._ctlr << ' ';
		}
	    } else if(nSafeAct > 0) {
		std::cout << "Apply the only one safe control.\n";
		u_grid.id_to_val(u, uid);
		logger << uid << ' ';
	    } else {
		std::cout << "No safe controls. Freeze.\n";
		u[0] = 0;
		u[1] = 0;
		logger << p7->u << ' ';
	    }
	    /********** Logging **********/
	    logger << u[0] << ',' << u[1] << "\n\n";
	    /********** Logging **********/
	}//end RH

	/* Write to txt file */
	// ctlrWtr << j << ':';
	for(int d = 0; d < x_dim; ++d) {
	    /* the controlled state */
	    ctlrWtr << x[d];
	    /* the state of the other robot */
	    otherWtr1 << xo1[d];
	    otherWtr2 << xo2[d];
	    otherWtr3 << xo3[d];
	    if(d<x_dim-1) {
		ctlrWtr << ',';
		otherWtr1 << ',';
		otherWtr2 << ',';
		otherWtr3 << ',';
	    }
	}
	ctlrWtr << ';';
	otherWtr1 << ';';
	otherWtr2 << ';';
	otherWtr3 << ';';
	for(int d = 0; d < u_dim; ++d) {
	    ctlrWtr << u[d];
	    otherWtr1 << uo1[d];
	    otherWtr2 << uo2[d];
	    otherWtr3 << uo3[d];
	    if(d<u_dim-1) {
		ctlrWtr << ',';
		otherWtr1 << ',';
		otherWtr2 << ',';
		otherWtr3 << ',';
	    }
	}
	ctlrWtr << '\n';
	otherWtr1 << '\n';
	otherWtr2 << '\n';
	otherWtr3 << '\n';

	/* Integrate ego robot trajectory */
	boost::numeric::odeint::integrate_const(rk45, car_dynamics(u), x, 0.0, tsim, dt);
	if(x[2] > M_PI) //convert angle into [-pi, pi]
	    x[2] -= 2*M_PI;
	if(x[2] < -M_PI)
	    x[2] += 2*M_PI;

	/* integrate obstacle trajectory after it appears */
	boost::numeric::odeint::integrate_const(rk45, car_dynamics(uo1), xo1, 0.0, tsim, dt);
	if(xo1[2] > M_PI) //convert angle into [-pi, pi]
	    xo1[2] -= 2*M_PI;
	if(xo1[2] < -M_PI)
	    xo1[2] += 2*M_PI;
	boost::numeric::odeint::integrate_const(rk45, car_dynamics(uo2), xo2, 0.0, tsim, dt);
	if(xo2[2] > M_PI) //convert angle into [-pi, pi]
	    xo2[2] -= 2*M_PI;
	if(xo2[2] < -M_PI)
	    xo2[2] += 2*M_PI;
	boost::numeric::odeint::integrate_const(rk45, car_dynamics(uo3), xo3, 0.0, tsim, dt);
	if(xo3[2] > M_PI) //convert angle into [-pi, pi]
	    xo3[2] -= 2*M_PI;
	if(xo3[2] < -M_PI)
	    xo3[2] += 2*M_PI;

	/* Update automaton state q */
	x_index = x_grid.val_to_id(x);
	q = q_prime[p5.label*nNodes + q];
	if(isacc[q])
	    std::cout << "******** achieve acc \'" << q << "\' " << ++i << " time ********" << std::endl;
    }

    ctlrWtr.close();
    otherWtr1.close();
    otherWtr2.close();
    otherWtr3.close();
    logger.close();

    return 0;
}


#include <unordered_map>
#include <iostream>
#include <string>
using std::string;

#include <boost/log/utility/setup/console.hpp>
#include <boost/log/trivial.hpp>

#include "streamTrace.hpp"
#include "navigation.hpp"
#include "constants.hpp"
#include "station.hpp"
#include "antenna.hpp"
#include "satSys.hpp"
#include "tides.hpp"
#include "rinex.hpp"
#include "gTime.hpp"

#define J2_GLO   1.0826257E-3     /* 2nd zonal harmonic of geopot   ref [2] */

#define OMGE_GLO 7.292115E-5      /* earth angular velocity (rad/s) ref [2] */
#define OMGE_GAL 7.2921151467E-5  /* earth angular velocity (rad/s) ref [7] */
#define OMGE_CMP 7.292115E-5      /* earth angular velocity (rad/s) ref [9] */

#define RTOL_KEPLER 	1E-14         /* relative tolerance for Kepler equation */
#define MAX_ITER_KEPLER 30        /* max number of iteration of Kelpler */
#define GLOSTEP   		60.0

#define SIN_5 -0.0871557427476582 /* sin(-5.0 deg) */
#define COS_5  0.9961946980917456 /* cos(-5.0 deg) */

nav_t	nav = {};
string			GNSSinp="GE";
map<SatSys,int> BS_Satel_List;
int 			BS_Start_Week		= 0;
double			BS_Start_TOW		= 0.0;
double			BS_Start_Epoc[6]	= {0};
int 			BS_Epoch_Num		= 0;
double			BS_Epoch_Inter		= 900.0;
map<E_Sys,double> BS_Max_Dtime;

void helpstring(void)
{
	fprintf(stdout, " brdc2sp3 -ant <ATX file> -inp <RINEX NAV file> [options]\n");
	fprintf(stdout, " Options [default]:\n");
	fprintf(stdout, "     -gns string   GNSS to be processed: G: GPS, R:GLONASS, E:Galileo, C: Beidou, J: QZSS, A: all [GE]\n");
	fprintf(stdout, "     -int float    Time spacing of SP3 entries [900.0]\n");
	fprintf(stdout, "     -out string   File name for SP3 output [BS_orbits.sp3]\n");
	fprintf(stdout, "     -*dt max_dt   set max_dt for constellation (*: 'G','R','E','J' or 'C') [86400.0]\n");
	fprintf(stdout, "     -clst         Select ephemeris with closest Toe (within max_dt seconds) [Latest valid ephemeris]\n");
	fprintf(stdout, "     -inav         I/NAV only for Galileo [use both F/NAV and I/NAV]\n");
	fprintf(stdout, "     -fnav         F/NAV only for Galileo [use both F/NAV and I/NAV]\n");
	fprintf(stdout, "     -frst         Filter out satellites with no/bad data in first epoc [not filtered]\n");
}

int BS_satantoff( GTime time, SatSys Sat, Vector3d& rs, Vector3d& dant, int gloind = 0)
{
	double lam1, lam2;

	char id[5];
	Sat.getId(id);

	PhaseCenterData* pcsat = findAntenna(Sat.id(), time, nav);

	if (pcsat == nullptr) return -1;
	auto& pcoMap = pcsat->pcoMap;

	E_FType j = F1;
	E_FType k = F2;
	double glonfrq1 = FREQ1_GLO + DFRQ1_GLO*gloind;
	double glonfrq2 = FREQ2_GLO + DFRQ2_GLO*gloind;
	switch (Sat.sys)
	{
		case E_Sys::GPS: lam1 = CLIGHT/FREQ1; 		lam2 = CLIGHT/FREQ2;			break;
		case E_Sys::GLO: lam1 = CLIGHT/glonfrq1; 	lam2 = CLIGHT/glonfrq2;			break;
		case E_Sys::QZS: lam1 = CLIGHT/FREQ1; 		lam2 = CLIGHT/FREQ2;			break;
		case E_Sys::GAL: lam1 = CLIGHT/FREQ1; 		lam2 = CLIGHT/FREQ5;	k=F5;	break;
		case E_Sys::BDS: lam1 = CLIGHT/FREQ1_CMP; 	lam2 = CLIGHT/FREQ2_CMP; 		break;
		default:     	 return -2;
	}

	/* sun position in ecef */
	Vector3d rsun;
	double gmst;
	ERPValues erpv;
	sunmoonpos(gpst2utc(time), erpv, &rsun, nullptr, &gmst);

	/* unit vectors of satellite fixed coordinates */
	Vector3d r = -rs;
	Vector3d ez = r.normalized();
	r = rsun - rs;
	Vector3d es = r.normalized();
	r = ez.cross(es);
	Vector3d ey = r.normalized();
	Vector3d ex = ey.cross(ez);

	double gamma	= lam2*lam2/lam1/lam1;
	double C1		= gamma	/ (gamma - 1);
	double C2		= -1	/ (gamma - 1);

	/* iono-free LC */
	for (int i = 0; i < 3; i++)
	{
		/* ENU to NEU */
		Vector3d pcoJ;
		Vector3d pcoK;
		if (pcoMap.find(j) == pcoMap.end())	pcoJ = Vector3d::Zero();
		else								pcoJ = pcoMap[j];
		if (pcoMap.find(k) == pcoMap.end())	pcoK = Vector3d::Zero();
		else								pcoK = pcoMap[k];
		double dant1	= pcoJ[1] * ex(i)
						+ pcoJ[0] * ey(i)
						+ pcoJ[2] * ez(i);	//todo aaron, matrix
		double dant2	= pcoK[1] * ex(i)
						+ pcoK[0] * ey(i)
						+ pcoK[2] * ez(i);

		fprintf( stdout,"\n  ATX: %s %11.6f %11.6f %11.6f  %11.6f %11.6f %11.6f", Sat.id().c_str(), pcoJ[0], pcoJ[1], pcoJ[2], pcoK[0], pcoK[1], pcoK[2]);

		//dant(i)	= C1 * dant1
		//		+ C2 * dant2;
		dant(i) = dant1;
	}
	return 0;
}

Eph* BS_seleph( GTime time, SatSys Sat, int iode, nav_t& nav_, int opt=0)
{
	double valid = 0;

	switch (Sat.sys)
	{
		case E_Sys::QZS:	valid = MAXDTOE_QZS	+ 1; break;
		case E_Sys::GAL:	valid = MAXDTOE_GAL	+ 1; break;
		case E_Sys::BDS:	valid = MAXDTOE_CMP	+ 1; break;
		default: 			valid = MAXDTOE		+ 1; break;
	}

	auto& ephList = nav_.ephMap[Sat];
	Eph* chosen = nullptr;
	GTime latestToe = GTime::noTime();
	double max_dtime = BS_Max_Dtime[Sat.sys];
	for (auto& [dummy, eph] : ephList)
	{
		if 	( iode >= 0 )
		{
			if(iode == eph.iode) return &eph;
			else continue;
		}
		//fprintf(stdout, "    %s %s\n",Sat.id().c_str(),eph.toe.to_string(0).c_str());
		if(opt == 0)
		{
			if (fabs(eph.toe - time) > valid)
				continue;
			if (eph.toe > latestToe)
			{
				chosen	= &eph;
				latestToe = eph.toe;
			}
		}
		if(opt==1)
		{
			double dtime=max_dtime;
			if (Sat.sys == +E_Sys::GPS)	dtime = fabs(eph.toe - (time + 3600));
			else						dtime = fabs(eph.toe - time);

			if ( max_dtime > dtime)
			{
				chosen	= &eph;
				latestToe = eph.toe;
				max_dtime = dtime;
			}
		}

	}

	return chosen;
}

Geph* BS_selgeph( GTime time, SatSys Sat, int iode, nav_t& nav_, int opt=0)
{
	double valid = MAXDTOE_GLO	+ 1;

	auto& gephList = nav_.gephMap[Sat];
	Geph* chosen = nullptr;
	GTime latestToe = GTime::noTime();
	double max_dtime = BS_Max_Dtime[E_Sys::GLO];
	for (auto& [dummy, geph] : gephList)
	{
		if 	( iode >= 0 )
		{
			if(iode == geph.iode) 	return &geph;
			else 					continue;
		}
		//fprintf(stdout, "    %s %s\n",Sat.id().c_str(),geph.toe.to_string(0).c_str());
		if(opt==0)
		{
			if (fabs(geph.toe - time) > valid)
				continue;
			if (geph.toe > latestToe)
			{
				chosen	= &geph;
				latestToe = geph.toe;
			}
		}
		if(opt==1)
		{
			double dtime = fabs(geph.toe - time);
			if ( max_dtime > dtime)
			{
				chosen	= &geph;
				latestToe = geph.toe;
				max_dtime = dtime;
			}
		}
	}

	return chosen;
}

/* glonass orbit differential equations --------------------------------------*/
void deq(const double* x, double* xdot, Vector3d& acc)
{
	double a, b, c, r2 = dot(x, x, 3), r3 = r2 * sqrt(r2), omg2 = OMGE_GLO*OMGE_GLO;

	if (r2 <= 0.0)
	{
		xdot[0] = xdot[1] = xdot[2] = xdot[3] = xdot[4] = xdot[5] = 0.0;
		return;
	}

	/* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */
	a = 1.5 * J2_GLO * MU_GLO * RE_GLO*RE_GLO / r2 / r3; /* 3/2*J2*mu*Ae^2/r^5 */
	b = 5.0 * x[2] * x[2] / r2;            /* 5*z^2/r^2 */
	c = -MU_GLO / r3 - a * (1.0 - b);      /* -mu/r^3-a(1-b) */
	xdot[0] = x[3];
	xdot[1] = x[4];
	xdot[2] = x[5];
	xdot[3] = (c + omg2) * x[0] + 2.0 * OMGE_GLO * x[4] + acc[0];
	xdot[4] = (c + omg2) * x[1] - 2.0 * OMGE_GLO * x[3] + acc[1];
	xdot[5] = (c - 2.0 * a) * x[2] + acc[2];
}

/* glonass position and velocity by numerical integration --------------------*/
void glorbit(double t, double* x, Vector3d& acc)
{
	double k1[6], k2[6], k3[6], k4[6], w[6];
	int i;

	deq(x, k1, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k1[i] * t / 2;
	deq(w, k2, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k2[i] * t / 2;
	deq(w, k3, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k3[i] * t;
	deq(w, k4, acc);

	for (i = 0; i < 6; i++)
		x[i] += (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) * t / 6;
}

int BS_geph2pos( GTime	time, Geph* geph, Vector3d& rs, double*	dts)
{
	double t = time - geph->toe;
	*dts	=  geph->taun
			+  geph->gamn * t;

	double x[6] = {0};
	for (int i = 0; i < 3; i++)
	{
		x[i  ] = geph->pos[i];
		x[i+3] = geph->vel[i];
	}

	for (double tt = t < 0 ? -GLOSTEP : GLOSTEP; fabs(t) > 1E-9; t -= tt)
	{
		if (fabs(t) < GLOSTEP) tt=t;
		glorbit(tt, x, geph->acc);
	}

	for (int i = 0; i < 3; i++) rs(i) = x[i];

	return 0;
}

int BS_eph2pos( GTime	time, Eph* eph, Vector3d& rs, double*	dts) {

	if (eph->A <= 0) return -1;

	double tk = time - eph->toe;
	int prn = eph->Sat.prn;
	int sys = eph->Sat.sys;

	double mu;
	double omge;
	switch (sys)
	{
		case E_Sys::GAL: mu = MU_GAL; omge = OMGE_GAL; break;
		case E_Sys::BDS: mu = MU_CMP; omge = OMGE_CMP; break;
		default:     	 mu = MU_GPS; omge = OMGE;     break;
	}

	double M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk;

	double E	= M;
	double Ek	= 0;
	int n;
	for (n = 0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++)
	{
		Ek = E;
		E -= (E - eph->e * sin(E) - M) / (1 - eph->e * cos(E));
	}

	if (n >= MAX_ITER_KEPLER)
	{
		fprintf(stderr,"WARNING for %s ephemeris: iteration divergent", eph->Sat.id().c_str());
		return -1;
	}

	double sinE  = sin(E);
	double cosE  = cos(E);
	double u 	 = atan2(sqrt(1 - eph->e * eph->e) * sinE, cosE - eph->e) + eph->omg;
	double r 	 = eph->A * (1 - eph->e * cosE);
	double i	 = eph->i0 + eph->idot * tk;
	double sin2u = sin(2 * u);
	double cos2u = cos(2 * u);

	u 			+= eph->cus * sin2u + eph->cuc * cos2u;
	r 			+= eph->crs * sin2u + eph->crc * cos2u;
	i 			+= eph->cis * sin2u + eph->cic * cos2u;

	double x	 = r * cos(u);
	double y	 = r * sin(u);
	double cosi  = cos(i);

	/* beidou geo satellite */
	if (sys == +E_Sys::BDS && prn <= 5)
	{
		double O	= eph->OMG0
					+ eph->OMGd * tk
					- omge * eph->toes;
		double sinO = sin(O);
		double cosO = cos(O);
		double xg = x * cosO - y * cosi * sinO;
		double yg = x * sinO + y * cosi * cosO;
		double zg = y * sin(i);
		double sino = sin(omge * tk);
		double coso = cos(omge * tk);
		rs(0) = +xg * coso + yg * sino * COS_5 + zg * sino * SIN_5;
		rs(1) = -xg * sino + yg * coso * COS_5 + zg * coso * SIN_5;
		rs(2) = -yg * SIN_5 + zg * COS_5;
	}
	else
	{
		double O	= eph->OMG0
					+ (eph->OMGd - omge) * tk
					- omge * eph->toes;
		double sinO = sin(O);
		double cosO = cos(O);
		rs(0) = x * cosO - y * cosi * sinO;
		rs(1) = x * sinO + y * cosi * cosO;
		rs(2) = y * sin(i);
	}

	tk = time - eph->toc;
	*dts	= eph->f0
			+ eph->f1 * tk
			+ eph->f2 * tk * tk;
	return 0;
}

void print_SP3header(FILE* fpout)
{
	/* line one */
	fprintf(fpout,"#dP%4.0f %2.0f %2.0f %2.0f %2.0f %11.8f ", BS_Start_Epoc[0], BS_Start_Epoc[1], BS_Start_Epoc[2], BS_Start_Epoc[3], BS_Start_Epoc[4], BS_Start_Epoc[5]);
	fprintf(fpout,"%7d ORBIT WGS84 BCT   GA", BS_Epoch_Num);

	/* Line two */
	double mjdate = 7.0*BS_Start_Week + BS_Start_TOW/86400.0 + 44244.0;
	fprintf(fpout,"\n## %4d %15.8f %5.0f %15.13f", BS_Start_Week, BS_Start_TOW, mjdate, mjdate-floor(mjdate));

	/* satellite lines */
	int NumSatLeft = 85, satind=0, satinline=0;
	if(BS_Satel_List.size()>85) NumSatLeft = BS_Satel_List.size();
	auto it = BS_Satel_List.begin();
	while(satind < NumSatLeft || satinline > 0)
	{
		if(satind == 0) fprintf(fpout, "\n+  %3d   ", (int)BS_Satel_List.size());
		else if(satinline == 0) fprintf(fpout, "\n+        ");

		if(it==BS_Satel_List.end()) fprintf(fpout, "  0");
		else
		{
			fprintf(fpout,"%s",it->first.id().c_str());
			it++;
		}

		satind++;
		if(satinline<16) satinline++;
		else satinline = 0;
	}

	/* accuracy lines */
	satind = 0;
	satinline = 0;
	while(satind<NumSatLeft)
	{
		if(satinline == 0) fprintf(fpout, "\n++       ");

		fprintf(fpout, "  0");

		satind++;
		if(satinline<16) satinline++;
		else satinline = 0;
	}

	/* char variable lines */
	if(GNSSinp.size() > 1) fprintf(fpout, "\n%%c M ");
	else fprintf(fpout, "\n%%c %s ", GNSSinp.c_str());
	fprintf(fpout, " cc GPS ccc cccc cccc cccc cccc ccccc ccccc ccccc ccccc");
	fprintf(fpout, "\n%%c cc cc GPS ccc cccc cccc cccc cccc ccccc ccccc ccccc ccccc");

	/* float variable lines */
	fprintf(fpout, "\n%%f  1.2500000  1.025000000  0.00000000000  0.000000000000000");
	fprintf(fpout, "\n%%f  0.0000000  0.000000000  0.00000000000  0.000000000000000");

	/* float variable lines */
	fprintf(fpout, "\n%%i    0    0    0    0      0      0      0      0         0");
	fprintf(fpout, "\n%%i    0    0    0    0      0      0      0      0         0");

	fprintf(fpout, "\n/* Created using Ginan at: %s. ", timeget().to_string(0).c_str());
	fprintf(fpout, "\n/* WARNING: For Geoscience Australia's internal use only");
}

void print_SP3epoch(FILE* fpout, GTime tsync, map<SatSys, vector<double>>& temp_eph)
{
	double ep[6];
	time2epoch(tsync, ep);
	fprintf(fpout,"\n*  %4.0f %2.0f %2.0f %2.0f %2.0f %11.8f ", ep[0], ep[1], ep[2], ep[3], ep[4], ep[5]);

	for(auto& [sat, neph] : BS_Satel_List){
		if(temp_eph.find(sat) == temp_eph.end())
			fprintf(fpout, "\nP%s%14.6f%14.6f%14.6f 999999.999999                    ", sat.id().c_str(), 0.0, 0.0, 0.0);
		else
		{
			auto vect = temp_eph[sat];
			fprintf(fpout, "\nP%s%14.6f%14.6f%14.6f%14.6f                    ", sat.id().c_str(), vect[0]/1000, vect[1]/1000, vect[2]/1000, vect[3]*1e6);
		}
	}
}

int main(int argc, char **argv)
{
	boost::log::core::get()->set_filter (boost::log::trivial::severity >= boost::log::trivial::info);
	string atxfile = "igs14.atx";
	vector<string> navfiles;
	int GALSelection = 7;
	/* Argument parsing */
	string inpfile;
    string outfile = "BS_orbits.sp3";
    bool fstreq = false;
    BS_Max_Dtime[E_Sys::GPS] = 86400.0;
    BS_Max_Dtime[E_Sys::GLO] = 86400.0;
    BS_Max_Dtime[E_Sys::GAL] = 86400.0;
    BS_Max_Dtime[E_Sys::QZS] = 86400.0;
    BS_Max_Dtime[E_Sys::BDS] = 86400.0;
    int ephopt = 0;
    for (int i = 1; i < argc; i++)
    {
        if      (!strcmp(argv[i], "-inp") && i+1 < argc)
        {
        	inpfile.assign(argv[++i]);
        	navfiles.push_back(inpfile);
        }
        else if (!strcmp(argv[i], "-out") && i+1<argc) outfile.assign(argv[++i]);
        else if (!strcmp(argv[i], "-ant") && i+1<argc) atxfile.assign(argv[++i]);
        else if (!strcmp(argv[i], "-gns") && i+1<argc) GNSSinp.assign(argv[++i]);
        else if (!strcmp(argv[i], "-int") && i+1<argc) BS_Epoch_Inter = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Gdt") && i+1<argc) BS_Max_Dtime[E_Sys::GPS] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Rdt") && i+1<argc) BS_Max_Dtime[E_Sys::GLO] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Edt") && i+1<argc) BS_Max_Dtime[E_Sys::GAL] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Jdt") && i+1<argc) BS_Max_Dtime[E_Sys::QZS] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Cdt") && i+1<argc) BS_Max_Dtime[E_Sys::BDS] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-clst"))			   ephopt = 1;
    	else if (!strcmp(argv[i], "-inav"))            GALSelection = 5;
        else if (!strcmp(argv[i], "-fnav"))  		   GALSelection = 2;
        else if (!strcmp(argv[i], "-frst"))			   fstreq=true;
        else if (!strcmp(argv[i], "-help"))
        {
        	helpstring();
        	exit(0);
        }
    }
	if(BS_Epoch_Inter <= 0.0) BS_Epoch_Inter = 900.0;

	bool pass = readantexf(atxfile, nav);
	if(!pass)
	{
		fprintf(stderr, "ERROR: invalid ANTEX file %s\n", atxfile.c_str());
		exit(0);
	}
	std::cout << "ATX file " << atxfile << std::endl;

	map<E_Sys,bool> GNSS_on;
    if (GNSSinp.find("A") != std::string::npos) GNSSinp = "GRECJ"; //All constellations
    if (GNSSinp.find("G") != std::string::npos) GNSS_on[E_Sys::GPS] = true; //GPS
    if (GNSSinp.find("R") != std::string::npos) GNSS_on[E_Sys::GLO] = true; //GLONASS
    if (GNSSinp.find("E") != std::string::npos) GNSS_on[E_Sys::GAL] = true; //Galileo
    if (GNSSinp.find("C") != std::string::npos) GNSS_on[E_Sys::BDS] = true; //Beidou
    if (GNSSinp.find("J") != std::string::npos) GNSS_on[E_Sys::QZS] = true; //QZSS
    if (GNSS_on.empty())
	{
        fprintf(stderr, "No GNSS selected\n");
        exit(0);
    }
	std::cout << "Processed GNSS " << GNSSinp << std::endl;

	char			type = 'N';
	ObsList 		obsList;
	RinexStation	sta;
	double			version = 0;
	E_Sys			sys;
	int 			tsys;
	map<E_Sys, vector<CodeType>>	sysCodeTypes;
	int 			nfil = 0;

	for (auto& file : navfiles)
	{
		std::ifstream	inputStream;
		inputStream.open(file, std::ifstream::in);
		int info = readrnx(inputStream, type , obsList, nav, &sta, version, sys, tsys, sysCodeTypes);
		if (info == false)
		{
			std::cout << "ERROR: invalid BRDC file " << file.c_str() << std::endl;
		}
		else{
			std::cout << "Processed file " << file << " " << type << " " << version << " " << std::endl;
			nfil++;
			info = readrnx(inputStream, type , obsList, nav, &sta, version, sys, tsys, sysCodeTypes);
		}
	}

	if(nfil == 0)
	{
		std::cout << "No valid Nav. files" << std::endl;
		exit(0);
	}

	FILE* sp3fp=fopen(outfile.c_str(),"wt");
	if(!sp3fp)
	{
		std::cout << "Cannot open output file: " << outfile << std::endl;
		exit(0);
	}
	std::cout << "Output file: " << outfile << std::endl;

	GTime tstart = GTime::noTime();
	GTime tfinsh = GTime::noTime();

	for(auto& [satint,ephlist] : nav.ephMap)
	{
		SatSys sat;
		sat.fromHash(satint);
		if(!GNSS_on[sat.sys]) continue;
		int neph = 0;
		for(auto it = nav.ephMap[satint].begin(); it != nav.ephMap[satint].end();)
		{
			auto [dummy, eph] = *it;
			double dtoc = eph.toe - eph.toc;
			double dttr = eph.toe - eph.ttr;
			bool alert = false;

			if(dtoc !=0.0) alert = true;
			if(fabs(dttr)>10000.0) alert = true;

			if(sys == +E_Sys::GAL && !(eph.code & GALSelection)) alert=true;		/* INAVs messages, change from 5 to 2 for FNAVs (we dont want to mix the two) */

			if(alert)
			{
				it=nav.ephMap[satint].erase(it);
			}
			else
			{
				//fprintf(stdout, "%s, %s, %d, %.1f, %s\n", sat.id().c_str(), eph.toe.to_string(0).c_str(), eph.iode, dtoc, eph.ttr.to_string(0).c_str());
				neph++;
				if (tstart == GTime::noTime() || tstart > eph.toe)		tstart = eph.toe;
				if (tfinsh == GTime::noTime() || tfinsh < eph.toe)		tfinsh = eph.toe;
				it++;
			}
		}
		if(neph>0) BS_Satel_List[sat]=neph;
	}

	if(GNSS_on[E_Sys::GLO]) for(auto& [satint,gephlist] : nav.gephMap)
	{
		SatSys sat;
		sat.fromHash(satint);
		int neph = 0;
		for(auto it = nav.gephMap[satint].begin(); it != nav.gephMap[satint].end();){
			auto& [dummy, geph] = *it;
			double dtoc = geph.toe - geph.tof;
			bool alert = false;

			//if(dtoc !=0.0) alert=true;
			//if(fabs(dttr)>10000.0) alert=true;

			if(alert){
				it = nav.gephMap[satint].erase(it);
				continue;
			}
			//fprintf(stdout, "%s %s %d, %.1f, %d\n", sat.id().c_str(), geph.toe.to_string(0).c_str(), geph.iode, dtoc, geph.svh);

			neph++;
			if (tstart == GTime::noTime() || tstart > geph.toe ) 	tstart = geph.toe;
			if (tfinsh == GTime::noTime() || tfinsh < geph.toe ) 	tfinsh = geph.toe;
			it++;
		}

		if (neph > 0)
			BS_Satel_List[sat] = neph;
	}

	double StarTow	= time2gpst(tstart, &BS_Start_Week);
	//BS_Start_TOW	= BS_Epoch_Inter*floor(StarTow/BS_Epoch_Inter);
	BS_Start_TOW	= 3600.0 * floor(StarTow / 3600.0);
	GTime tsync 	= gpst2time(BS_Start_Week, BS_Start_TOW);
	BS_Epoch_Num	= (int)(floor((tfinsh - tsync)/BS_Epoch_Inter) + 1);
	time2epoch(tsync, BS_Start_Epoc);

	fprintf(stdout, "\nTstart= %s, Tini= %s, Nepoc=%d\n", tstart.to_string(0).c_str(), tsync.to_string(0).c_str(), BS_Epoch_Num);

	if(fstreq)for(auto it = BS_Satel_List.begin(); it != BS_Satel_List.end();)
	{
		SatSys sat = it->first;
		bool nofst = false;
		Vector3d rs(0, 0, 0);
		Vector3d dant(0, 0, 0);
		double dts;
		if(sat.sys == +E_Sys::GLO)
		{
			Geph* gephp=BS_selgeph( tsync, sat, -1, nav);
			if(!gephp) nofst = true;
			else if(BS_geph2pos( tsync, gephp, rs, &dts) < 0) nofst = true;
			else if(rs.norm() < RE_WGS84) nofst = true;
			else if(BS_satantoff( tsync, sat, rs, dant, gephp->frq ) < 0) nofst = true;
			else nofst = false;
		}
		else
		{
			Eph* ephp=BS_seleph( tsync, sat, -1, nav);
			if(!ephp) nofst = true;
			else if(BS_eph2pos( tsync, ephp, rs, &dts) < 0) nofst = true;
			else if(rs.norm() < RE_WGS84) nofst = true;
			else if(BS_satantoff( tsync, sat, rs, dant, 0 ) < 0) nofst = true;
			else nofst = false;
		}

		if(nofst){
			it = BS_Satel_List.erase(it);
		}
		else{
			++it;
		}
	}

	print_SP3header(sp3fp);

	for(int epc=0; epc<BS_Epoch_Num; epc++){
		fprintf(stdout, "\n%s", tsync.to_string(0).c_str());
		map<SatSys,vector<double>> temp_eph;
		for(auto& [sat,neph] : BS_Satel_List){
			Vector3d rs(0, 0, 0);
			Vector3d dant(0, 0, 0);
			double dts;

			if(sat.sys == +E_Sys::GLO){
				Geph* gephp=BS_selgeph( tsync, sat, -1, nav, ephopt);
				if(!gephp) continue;
				if(BS_geph2pos( tsync, gephp, rs, &dts) < 0) continue;
				if(rs.norm() < RE_WGS84) continue;
				if(BS_satantoff( tsync, sat, rs, dant, gephp->frq ) < 0) continue;
				fprintf(stdout, " %s ", sat.id().c_str());
			}
			else{
				Eph* ephp=BS_seleph( tsync, sat, -1, nav, ephopt);
				if(!ephp) continue;
				if(BS_eph2pos( tsync, ephp, rs, &dts) < 0) continue;
				if(rs.norm() < RE_WGS84) continue;
				if(BS_satantoff( tsync, sat, rs, dant, 0 ) < 0) continue;
				fprintf(stdout, " %s ", sat.id().c_str());
			}

			vector<double> sateph;
			sateph.push_back(rs(0) - dant(0));
			sateph.push_back(rs(1) - dant(1));
			sateph.push_back(rs(2) - dant(2));
			sateph.push_back(dts);

			//fprintf(stdout, "\n%s %11.6f %11.6f %11.6f", sat.id().c_str(), dant(0),dant(1),dant(2));

			temp_eph[sat] = sateph;
		}

		if (temp_eph.size() > 0)
			print_SP3epoch(sp3fp, tsync, temp_eph);

		tsync = tsync + BS_Epoch_Inter;
	}

	fprintf(sp3fp, "\nEOF");

	return(EXIT_SUCCESS);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_HYPOT_HPP_INCLUDED

#include <boost/simd/toolbox/arithmetic/functions/hypot.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/simd/include/functions/scalar/sqrt.hpp>
#include <boost/simd/include/functions/scalar/sqr.hpp>
#include <boost/simd/include/functions/scalar/max.hpp>
#include <boost/simd/include/functions/scalar/min.hpp>
#include <boost/simd/include/functions/scalar/logical_or.hpp>
#include <boost/simd/include/functions/scalar/logical_and.hpp>
#include <boost/simd/include/functions/scalar/exponent.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/simd/include/functions/scalar/is_inf.hpp>
#include <boost/simd/include/functions/scalar/ldexp.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  template < class T, class I = typename dispatch::meta::as_integer<T, signed>::type>
  struct hypot_constants;
  //TODO make proper constants

  template <class I> struct hypot_constants<float, I>
  {
    typedef I  int_type;
    static inline int_type C0() { return (30);};
    static inline int_type C1() { return (50);};
    static inline int_type C2() { return (60);};
    static inline int_type MC1(){ return (-50);};
    static inline int_type MC2(){ return (-60);};
    static inline int_type C3() { return (0x00800000);};
    static inline int_type M1() { return (0xfffff000);};
  };

  template <class I> struct hypot_constants<double, I>
  {
    typedef I  int_type;
    static inline int_type C0() { return (60);};
    static inline int_type C1() { return (500);};
    static inline int_type C2() { return (600);};
    static inline int_type MC1(){ return (-500);};
    static inline int_type MC2(){ return (-600);};
    static inline int_type C3() { return (0x0010000000000000ll);}
    static inline int_type M1() { return (0xffffffff00000000ll);};
  };

 BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::hypot_, tag::cpu_,
                       (A0),
                       (scalar_<arithmetic_<A0> >)(scalar_<arithmetic_<A0> >)
                      )
 {
   typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return internal(result_type(a0), result_type(a1));
    }
  private:

    static inline float internal(const float& a0, const  float& a1)
      {
      // flibc does that in ::hypotf(a0, a1) in asm with no more speed!
      // proper internal is 30% slower
        return static_cast<float>(::sqrt(boost::simd::sqr(static_cast<typename meta::make_dependent<double, A0>::type>(a0))+
                                         boost::simd::sqr(static_cast<typename meta::make_dependent<double, A0>::type>(a1))));
      }

    template < class AA0>
    static inline AA0  internal(const AA0& a0, const  AA0& a1)
    {
      // in double ::hypot is very slow and is 4 times slower than internal
      // this routine in float (with float constants) is 30% slower than
      // the straightforward preceding overload for floats
      // The float constants are provided in order to modify
      // the algorithm if a architecture gived different speed results
      if ( logical_or(logical_and(is_nan(a0), is_inf(a1)),
                      logical_and(is_nan(a1), is_inf(a0)))) return Inf<result_type>();
      typedef typename dispatch::meta::as_integer<AA0, signed>::type  int_type;
      AA0 x =  boost::simd::abs(a0);
      AA0 y =  boost::simd::abs(a1);
      if (boost::simd::is_inf(x+y)) return Inf<AA0>();
      if (boost::simd::is_nan(x+y)) return Nan<AA0>();
      AA0 a =  boost::simd::max(x, y);
      AA0 b =  boost::simd::min(x, y);
      int_type ea =   boost::simd::exponent(a);
      int_type eb  =  boost::simd::exponent(b);
      if (ea-eb > hypot_constants<AA0>::C0()) return a+b;
      int_type e = Zero<int_type>();
      if (ea > hypot_constants<AA0>::C1())
      {
        e = hypot_constants<AA0>::MC2();
      }
      if (eb < hypot_constants<AA0>::MC1())
      {
        e = hypot_constants<AA0>::C1();
      }
      if (e)
      {
        a =  boost::simd::ldexp(a, e);
        b =  boost::simd::ldexp(b, e);
      }
      AA0 w = a-b;
      if (w > b)
      {
        AA0 t1 = b_and(a, hypot_constants<AA0>::M1());
        AA0 t2 = a-t1;
        w  = (t1*t1-(b*(-b)-t2*(a+t1)));
      }
      else
      {
        AA0 y1 = b_and(b, hypot_constants<AA0>::M1());
        AA0 y2 = b - y1;
        typedef typename dispatch::meta::as_integer<AA0, unsigned>::type type;
        AA0 t1 =  bitwise_cast<AA0>(bitwise_cast<type>(a)+hypot_constants<AA0>::C3());
        AA0 t2 = (a+a) - t1;
        w  = (t1*y1-(w*(-w)-(t1*y2+t2*b)));
      }
      w = boost::simd::sqrt(w);
      if (e) w = boost::simd::ldexp(w, -e);
      return w;
    }
  };
} } }

#endif


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef ITERATE_GRID_HH
#define ITERATE_GRID_HH

#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/sequence.hpp>

#include "dune/grid/common/grid.hh"
#include "dune/geometry/quadraturerules.hh"

#include "fem/functionspace.hh"
#include "fem/quadrature.hh"
#include "fem/variables.hh"

#include "utilities/threading.hh"

namespace Kaskade
{
  namespace GridIterateDetail
  {
    template <class VariableDescriptions, class Functor, class Functions, class Spaces, class CellRange>
    void gridIterateRange(VariableDescriptions const& varDesc, Functions const& functions, Spaces const& spaces, Functor& f, CellRange cells)
    {
      using namespace boost::fusion;
      using namespace Dune;

      typedef typename SpaceType<Spaces,0>::type::Grid Grid;
      typedef typename SpaceType<Spaces,0>::type::GridView GridView;
      typedef typename SpaceType<Spaces,0>::type::Scalar Scalar;
      typedef typename Grid::ctype CoordType;
      int const dim = Grid::dimension;

      GridView const& gridView = at_c<0>(spaces)->gridView();

      // Shape function cache. Remember that every thread has to use its own cache!
      typedef ShapeFunctionCache<Grid,Scalar> SfCache;
      SfCache sfCache;

      // Quadrature rule cache
      typedef Dune::QuadratureRule<CoordType,dim> QuadRule;
      QuadratureTraits<QuadRule> quadratureCache;

      // Evaluators for shape functions of all FE spaces. Remember that
      // every thread has to use its own collection of evaluators!
      auto evaluators = getEvaluators(spaces,&sfCache);

      // Iterate over all cells.
      for (auto ci=cells.begin(); ci!=cells.end(); ++ci) {

        // for all spaces involved, compute the shape functions and
        // their global indices, which are needed for evaluating the
        // functional's derivative.
        auto idx = gridView.indexSet().index(*ci);
        moveEvaluatorsToCell(evaluators,*ci,idx);

        // loop over all quadrature points and evaluate variables
        int const p = f.integrationOrder(*ci,maxOrder(evaluators));

        QuadRule const& qr = quadratureCache.rule(ci->type(),p);
        useQuadratureRuleInEvaluators(evaluators,qr,0);

        size_t nQuadPos = qr.size();
        for (size_t g=0; g<nQuadPos; ++g) {
          // pos of integration point
          auto const& quadPos = qr[g].position();

          // for all spaces involved, update the evaluators associated
          // to this quadrature point
          moveEvaluatorsToIntegrationPoint(evaluators,quadPos,qr,g,0);

          // evaluate functions and call functor
          f(ci,idx,quadPos,evaluateFunctions<VariableDescriptions>(functions,evaluators,valueMethod),
            ci->geometry().integrationElement(quadPos)*qr[g].weight());
        }
      }
    };

  } // end of namespace GridIterateDetail

  /**
   * \brief A function that supports general iterations over a spatial domain and
   * supports efficient evaluation of FE functions at the iterated
   * quadrature points.
   *
   * \param varDesc a heterogeneous container of VariableDescription structures
   *
   * \param functions a heterogeneous container of FE functions or function
   * views that can be evaluated given an Evaluator of the associated space.
   *
   * \param spaces a heterogeneous container of pointers to FE function spaces.
   * VariableDescription classes that gives the mapping of functions in
   * data to their associated spaces.
   *
   * \param f the functor that does the work. On each quadrature point,
   * is operator() is called with a heterogeneous container of the
   * values of the functions in data.
   */
  template <class VariableDescriptions, class Functor, class Functions, class Spaces>
  void gridIterate(VariableDescriptions const& varDesc, Functions const& functions, Spaces const& spaces, Functor& f)
  {
    using namespace boost::fusion;
    auto const& cellRanges = deref(begin(functions)).space().gridManager().leafCellRanges(); // WARNING: what if the grid view is not leaf?

    int n = std::min(NumaThreadPool::instance().cpus(),cellRanges.maxRanges());
    std::vector<Functor> fs(n,f);

    parallelFor([&varDesc,&functions,&spaces,&cellRanges,&fs](int i, int n)
    {
      GridIterateDetail::gridIterateRange(varDesc,functions,spaces,fs[i],cellRanges.range(n,i));
    },n);

    for (auto& g: fs)
      f.join(g);
  }

  //---------------------------------------------------------------------

  /**
   * \brief A trivial Scaling.
   *
   * The identity scaling which leaves its operand simply as it is. This
   * is a convenience class to evaluate plain, unscaled norms.
   */
  struct IdentityScaling
  {
    template <class Cell, class F>
    void scale(Cell const&,
               Dune::FieldVector<typename Cell::Geometry::ctype,Cell::dimension> const&,
               F&) const {}
  };

  /**
   * \brief A \ref Collector that sums up the weighted contributions.
   *
   * This realizes a plain integration.
   */
  struct SummationCollector
  {
    template <class Cell>
    int integrationOrder(Cell const& /* ci */, int shapeFunctionOrder) const
    {
      return shapeFunctionOrder;
    }

    template <class CellIterator, class Index, class Sequence>
    void operator()(CellIterator const&, Index, double weight, Sequence const& x)
    {
      if (sum.empty())
        sum.resize(boost::fusion::size(x),0);
      auto i = sum.begin();                           // provide a reference to the iterator
      boost::fusion::for_each(x,Add(weight,i));       // because that is incremented on each call of Add
    }                                                 // (hack: for_each requires an immutable functor)

    void join(SummationCollector const& c)
    {
      if (sum.empty())
        sum = c.sum;
      else
      {
        assert(sum.size()==c.sum.size());
        for (int i=0; i<sum.size(); ++i)
          sum[i] += c.sum[i];
      }
    }

    std::vector<double> sum;

  private:
    struct Add
    {
      Add(double w_, std::vector<double>::iterator& i_): w(w_), i(i_) {}
      template <class T>
      void operator()(T const& t) const { *i += w*t; ++i; }

    private:
      double w;
      std::vector<double>::iterator& i;
    };
  };

  /**
   * \brief A functor for computing scaled \f$L^2\f$-norms of a set of (possibly vector-valued) functions.
   *
   * This is intended to be used with gridIterate. The plain \f$L^2\f$-norm is computed if the
   * IdentityScaling is provided.
   */
  template <class Functions, class Scaling, class Collector>
  class ScaledTwoNorm2Collector
  {
  public:

    /**
     * \brief Constructor.
     *
     * \param scaling
     * \param collector
     */
    ScaledTwoNorm2Collector(Scaling const& scaling_, Collector& collector_): scaling(scaling_), collector(collector_) {    }

    template <class Cell>
    int integrationOrder(Cell const& cell, int shapeFunctionOrder) const
    {
      return 2*collector.integrationOrder(cell,shapeFunctionOrder);
    }

    template <class CellIterator, class Index, class Sequence>
    void operator()(CellIterator const& ci, Index idx,
                    Dune::FieldVector<typename CellIterator::Entity::Geometry::ctype,CellIterator::Entity::dimension> const& pos,
                    Sequence const& seq, double weight)
    {
      using namespace boost::fusion;
      typename result_of::as_vector<Sequence>::type values(seq);
      scaling.scale(*ci,pos,values);
      collector(ci,idx,weight,transform(values,TwoNorm2()));
    }

    void join(ScaledTwoNorm2Collector const& c)
    {
      collector.join(c.collector);
    }

    Collector const& data() const { return collector; }

  private:
    // a boost::fusion functor computing the squared euclidean norm of given vectors
    struct TwoNorm2
    {
      template <class T> struct result {};

      template <class Vec>
      struct result<TwoNorm2(Vec)> { typedef  Dune::FieldVector<double,1> type; };

      template <class Vec>
      typename result<TwoNorm2(Vec)>::type operator()(Vec const& v) const { return v.two_norm2(); }
    };

    Scaling const& scaling;
    Collector collector; // keep collector by value as we write concurrently in different objects.
  };

  /**
   * \brief Evaluates the square of the scaled \f$L^2\f$-norms of a set of functions.
   * \tparam Variables a boost::fusion sequence of VariableDescription types
   * \tparam Functions a boost::fusion sequence of FE function types (or types providing the required interface subset)
   * \tparam Spaces
   * \tparam Scaling
   * \tparam Collector
   *
   * \param varDesc a boost::fusion sequence of VariableDescription entries, e.g. obtained from VariableSetDescription::Variables()
   *
   * The required interface subset of FE functions includes the value() and the space() methods.
   */
  template <class Variables, class Functions, class Spaces, class Scaling, class Collector>
  void scaledTwoNormSquared(Variables const& varDesc, Functions const& f, Spaces const& spaces,
                            Scaling const& scaling, Collector& sum)
  {
    ScaledTwoNorm2Collector<Functions,Scaling,Collector> collector(scaling,sum);
    gridIterate(varDesc,f,spaces,collector);
    sum = collector.data();
  }

  //---------------------------------------------------------------------

  /**
   * \cond internals
   */
  namespace relativeErrorDetail {

    template <class F>
    struct Difference
    {
      typedef typename F::Scalar RT;
      typedef RT Scalar;

      static int const Components = F::components;

      typedef typename F::Space     Space;
      typedef typename F::ValueType ValueType;

      Difference(F const& f1_, F const& f2_): f1(f1_), f2(f2_) { }

      ValueType value(typename Space::Evaluator const& evaluator) const
      {
        return f1.value(evaluator)-f2.value(evaluator);
      }

      Space const& space() const { return f1.space(); }

    private:
      F const& f1;
      F const& f2;
    };

    struct MakeDifference
    {
      template <class T> struct result {};

      template <class Pair>
      struct result<MakeDifference(Pair)>
      {
        typedef typename boost::fusion::result_of::value_at_c<Pair,0>::type T;

        typedef Difference<typename boost::remove_const<typename boost::remove_reference<T>::type>::type> type;
      };

      template <class Pair>
      typename result<MakeDifference(Pair)>::type operator()(Pair const& pair) const
      {
        return typename result<MakeDifference(Pair)>::type(boost::fusion::at_c<0>(pair),boost::fusion::at_c<1>(pair));
      }
    };

  } // End of namespace relativeErrorDetail
  /**
   * \endcond
   */

  /**
   * For each variable, this function computes the following pair of values:
   * \f[ (\|f_1-f_2\|,\|f_3\|) \f]
   * \tparam Variables a boost::fusion sequence of variable descriptions
   * \tparam OutIter an output iterator with value type std::pair(double,double)
   * \tparam Functions a boost::fusion sequence of finite element functions, referenced by the variable descriptions
   * \tparam Spaces a boost::fusion sequence of pointers to spaces, referenced by the variable descriptions
   * \tparam Scaling
   */
  template <class Variables, class OutIter, class Functions, class Spaces, class Scaling>
  void relativeError(Variables const& varDesc, Functions const& f1, Functions const& f2, Functions const& f3,
                     Spaces const& spaces, Scaling const& scaling, OutIter out)
  {
    using namespace boost::fusion;

    int const s = size(f1);
    SummationCollector sum;
    //   scaledTwoNormSquared(join(varDesc,varDesc),
    //                        join( f3, as_vector(transform(zip(f1,f2),relativeErrorDetail::MakeDifference())) ),
    //                        spaces,scaling,sum);
    scaledTwoNormSquared(join(varDesc,varDesc),
                         join(as_vector(transform(zip(f1,f2),relativeErrorDetail::MakeDifference())),f3),
                         spaces,scaling,sum);
    // To my understanding, the as_vector call in the statement above
    // should not be necessary. However, it prevents a (rare but
    // reproducible) segfault with gcc 4.3.4 -O2. Funny thing is, some
    // output statements to cerr have the same effect. The cause may be
    // either a wild pointer bug in the code exposed by -O2 or a code
    // generation bug in gcc.  Sigh. At least, since Difference objects
    // are lightweight, the explicit vector construction should not be a
    // performance penalty. ws 2009-12-03

    for (int i=0; i<s; ++i) {
      *out = std::make_pair(std::sqrt(sum.sum[i]),std::sqrt(sum.sum[i+s]));
      ++out;
    }
  }
} // end of namespace Kaskade

#endif


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_AFFINE_HPP
#define ODE_AFFINE_HPP

// ODE using Affine

#include <iostream>
#include <cmath>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/make-candidate.hpp>
#include <kv/psa.hpp>
#include <kv/affine.hpp>
#include <kv/ode-param.hpp>
#include <kv/ode-callback.hpp>

#ifndef ODE_FAST
#define ODE_FAST 1
#endif

#ifndef ODE_RESTART_RATIO
#define ODE_RESTART_RATIO 1
#endif

#ifndef ODE_COEF_MID
#define ODE_CORF_MID 0
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
int
ode_affine(F f, ub::vector< affine<T> >& init, const interval<T>& start, interval<T>& end, ode_param<T> p = ode_param<T>(), ub::vector< psa< interval<T> > >* result_psa = NULL)
{
	int n = init.size();
	int i, j;

	ub::vector< psa< affine<T> > > x, y;
	psa< affine<T> > torg;
	psa< affine<T> > t;

	ub::vector< psa< affine<T> > > z, w;

	psa< affine<T> > temp;
	T m;
	ub::vector<T> newton_step;

	bool flag, resized;

	affine<T> s1, s2;
	interval<T> s2i;

	ub::vector< affine<T> > s1_save;
	ub::vector< interval<T> > s2i_save;

	interval<T> deltat;
	ub::vector< affine<T> > result;

	T radius, radius_tmp;
	T tolerance;
	int n_rad;
	#if ODE_RESTART_RATIO == 1
	T max_ratio;
	#endif

	int ret_val;
	interval<T> end2;
	int restart;

	int maxnum_save = affine<T>::maxnum();

	bool save_mode, save_uh, save_rh;

	m = 1.;
	for (i=0; i<n; i++) {
		m = std::max(m, norm(to_interval(init(i))));
	}
	tolerance = m * p.epsilon;

	x = init;
	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa< affine<T> >::mode();
	save_uh = psa< affine<T> >::use_history();
	save_rh = psa< affine<T> >::record_history();
	psa< affine<T> >::mode() = 1;
	psa< affine<T> >::use_history() = false;
	psa< affine<T> >::record_history() = false;
	#if ODE_FAST == 1
	psa< affine<T> >::record_history() = true;
	psa< affine<T> >::history().clear();
	#endif
	for (j=0; j<p.order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa< affine<T> >::use_history() = true;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) {
			y(i) = integrate(y(i));
			y(i) = setorder(y(i), j+1);
		}
		x = init + y;
	}

	if (p.autostep) {
		radius = 0.;
		n_rad = 0;
		for (j = p.order; j>=1; j--) {
			m = 0.;
			for (i=0; i<n; i++) {
				#if ODE_COEF_MID == 1
				using std::abs;
				m = std::max(m, abs(x(i).v(j).get_mid()));
				#else
				m = std::max(m, norm(to_interval(x(i).v(j))));
				#endif
			}
			if (m == 0.) continue;
			radius = std::max(radius, (T)std::pow((double)m, 1./j));
			n_rad++;
			if (n_rad == 2) break;
		}
		radius = std::pow((double)tolerance, 1./p.order) / radius;
	}

	psa< affine<T> >::mode() = 2;

	restart = 0;
	// disable resize because resize algorithm is not suitable
	//  for ode-affine
	// resized = false;
	resized = true;

	while (true) {
		if (p.autostep) {
			end2 = mid(start + radius);
			if (end2 >= end.lower()) {
				end2 = end;
				radius = mid(end2 - start);
				ret_val = 2;
			} else {
				ret_val = 1;
			}
		} else {
			end2 = end;
			ret_val = 2;
		}
		deltat = end2 - start;

		psa< affine<T> >::domain() = interval<T>(0., deltat.upper());

		z = x;
		t = setorder(torg, p.order);

		try {
			w = f(z, t);
		}
		catch (std::domain_error& e) {
			 if (p.autostep && restart < p.restart_max) {
				psa< affine<T> >::use_history() = false;
				if (p.verbose == 1) {
					std::cout << "ode: radius changed: " << radius;
				}
				radius *= 0.5;
				if (p.verbose == 1) {
					std::cout << " -> " << radius << "\n";
				}
				restart++;
				continue;
			} else {
				throw std::domain_error("ode_affine: evaluation error");
			}
		}

		for (i=0; i<n; i++) {
			temp = integrate(w(i));
			w(i) = setorder(temp, p.order);
		}
		w = init + w;

		newton_step.resize(n);
		for (i=0; i<n; i++) {
			newton_step(i) = norm(to_interval(w(i).v(p.order) - z(i).v(p.order)));
		}
		make_candidate(newton_step);

		s1_save.resize(n);
		s2i_save.resize(n);

		for (i=0; i<n; i++) {
			z(i).v(p.order) += (affine<T>)(newton_step(i) * interval<T>(-1., 1.));
			#ifdef ODE_AFFINE_SIMPLE
			s2i = to_interval(z(i).v(p.order));
			s2i_save(i) = s2i;
			z(i).v(p.order) = (affine<T>)s2i;
			#else
			split(z(i).v(p.order), maxnum_save, s1, s2);
			s2i = to_interval(s2);
			s1_save(i) = s1;
			s2i_save(i) = s2i;
			z(i).v(p.order) = append(s1, (affine<T>)s2i);
			#endif
		}

		if (p.autostep && ret_val != 2 && resized == false) {
			resized = true;
			m = (std::numeric_limits<T>::min)();
			for (i=0; i<n; i++) {
				m = std::max(m, rad(eval(z(i), (affine<T>)deltat)) - rad(init(i)));
			}
			m = m / tolerance;
			radius_tmp = radius / std::pow((double)m, 1. / p.order);
			if (radius_tmp >= radius && restart > 0) {
				// do nothing, not continue
			} else {
				radius = radius_tmp;
				continue;
			}
		}

		w = f(z, t);
		for (i=0; i<n; i++) {
			temp = integrate(w(i));
			w(i) = setorder(temp, p.order);
		}
		w = init + w;

		flag = true;
		#if ODE_RESTART_RATIO == 1
		max_ratio = 0.;
		#endif
		for (i=0; i<n; i++) {
			#ifdef ODE_AFFINE_SIMPLE
			s2i = to_interval(w(i).v(p.order));
			#if ODE_RESTART_RATIO == 1
			max_ratio = std::max(max_ratio, width(s2i) / width(s2i_save(i)));
			#endif
			flag = flag && subset(s2i, s2i_save(i));
			s2i_save(i) = s2i;
			w(i).v(p.order) = (affine<T>)s2i;
			#else
			split(w(i).v(p.order), maxnum_save, s1, s2);
			s2i = to_interval(s2);
			#if ODE_RESTART_RATIO == 1
			max_ratio = std::max(max_ratio, width(to_interval(s1 - s1_save(i)) + s2i) / width(s2i_save(i)));
			#endif
			flag = flag && subset(to_interval(s1 - s1_save(i)) + s2i, s2i_save(i));
			s1_save(i) = s1;
			s2i_save(i) = s2i;
			w(i).v(p.order) = append(s1, (affine<T>)s2i);
			#endif
		}
		if (flag) break;

		if (!p.autostep || restart >= p.restart_max) {
			ret_val = 0;
			break;
		}
		if (p.verbose == 1) {
			std::cout << "ode: radius changed: " << radius;
		}
		#if ODE_RESTART_RATIO == 1
		radius *= std::max(std::min((T)0.5, (T)0.5 / max_ratio), (T)0.125);
		#else
		radius *= 0.5;
		#endif
		if (p.verbose == 1) {
			std::cout << " -> " << radius << "\n";
		}
		restart++;
	}

	if (ret_val != 0) {
		for (j=0; j<p.iteration; j++) {
			w = f(w, t);
			for (i=0; i<n; i++) {
				temp = integrate(w(i));
				w(i) = setorder(temp, p.order);
			}
			w = init + w;

			for (i=0; i<n; i++) {
				#ifdef ODE_AFFINE_SIMPLE
				s2i = to_interval(w(i).v(p.order));
				s2i = intersect(s2i, s2i_save(i));
				s2i_save(i) = s2i;
				w(i).v(p.order) = (affine<T>)s2i;
				#else
				split(w(i).v(p.order), maxnum_save, s1, s2);
				s2i = to_interval(s2);
				// s2i = intersect(to_interval(s1 - s1_save(i)) + s2i, s2i_save(i));
				s2i = intersect(to_interval(s1 - s1_save(i)) + s2i_save(i), s2i);
				s1_save(i) = s1;
				s2i_save(i) = s2i;
				w(i).v(p.order) = append(s1, (affine<T>)s2i);
				#endif
			}
		}

		if (result_psa != NULL) {
			// convert w to interval and store it to *result_psa
			(*result_psa).resize(n);
			for (i=0; i<n; i++) {
				(*result_psa)(i).v.resize(w(i).v.size());
				for (j=0; j<w(i).v.size(); j++) {
					(*result_psa)(i).v(j) = to_interval(w(i).v(j));
				}
			}
		}

		result.resize(n);
		for (i=0; i<n; i++) {
			result(i) = eval(w(i), (affine<T>)deltat);
			if (p.ep_reduce == 0) {
				split(result(i), maxnum_save, s1, s2);
				s2i = to_interval(s2);
				s1_save(i) = s1;
				s2i_save(i) = s2i;
			}
		}

		if (p.ep_reduce == 0) {
			affine<T>::maxnum() = maxnum_save;
			for (i=0; i<n; i++) {
				s1_save(i).resize();
				result(i) = append(s1_save(i), (affine<T>)s2i_save(i));
			}
		} else {
			epsilon_reduce(result, p.ep_reduce, p.ep_reduce_limit);
		}

		init = result;
		if (ret_val == 1) end = end2;
	}

	psa< affine<T> >::mode() = save_mode;
	psa< affine<T> >::use_history() = save_uh;
	psa< affine<T> >::record_history() = save_rh;

	return ret_val;
}

template <class T, class F>
int
odelong_affine(
	F f,
	ub::vector< affine<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	ub::vector< affine<T> > x, x1;
	interval<T> t, t1;
	int ret_ode;
	int ret_val = 0;
	bool ret_callback;

	ub::vector< psa< interval<T> > > result_tmp;

	x = init;
	t = start;
	p.set_autostep(true);

	while (1) {
		x1 = x;
		t1 = end;

		ret_ode = ode_affine(f, x1, t, t1, p, &result_tmp);
		if (ret_ode == 0) {
			if (ret_val == 1) {
				init = x1;
				end = t;
			}
			return ret_val;
		}
		ret_val = 1;
		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << to_interval(x1) << "\n";
		}

		callback(t, t1, to_interval(x), to_interval(x1), result_tmp);

		if (ret_callback == false) {
			init = x1;
			end = t1;
			return 3;
		}

		if (ret_ode == 2) {
			init = x1;
			return 2;
		}

		t = t1;
		x = x1;
	}
}

template <class T, class F>
int
odelong_affine(
	F f,
	ub::vector< interval<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	int i;
	ub::vector< affine<T> > x;
	int maxnum_save;
	int r;

	maxnum_save = affine<T>::maxnum();
	affine<T>::maxnum() = 0;

	x = init;

	r = odelong_affine(f, x, start, end, p, callback);

	affine<T>::maxnum() = maxnum_save;

	if (r == 0) return 0;

	for (i=0; i<s; i++) init(i) = to_interval(x(i));

	return r;
}

} // namespace kv

#endif // ODE_AFFINE_HPP


#include "Proof.hpp"

#include "crypto/common.h"

#include <boost/static_assert.hpp>
#include <libsnark/common/default_types/r1cs_ppzksnark_pp.hpp>
#include <libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp>
#include <mutex>

using namespace libsnark;

typedef alt_bn128_pp curve_pp;
typedef alt_bn128_pp::G1_type curve_G1;
typedef alt_bn128_pp::G2_type curve_G2;
typedef alt_bn128_pp::GT_type curve_GT;
typedef alt_bn128_pp::Fp_type curve_Fr;
typedef alt_bn128_pp::Fq_type curve_Fq;
typedef alt_bn128_pp::Fqe_type curve_Fq2;

BOOST_STATIC_ASSERT(sizeof(mp_limb_t) == 8);

namespace libvcoin {

// FE2IP as defined in the protocol spec and IEEE Std 1363a-2004.
bigint<8> fq2_to_bigint(const curve_Fq2 &e)
{
    auto modq = curve_Fq::field_char();
    auto c0 = e.c0.as_bigint();
    auto c1 = e.c1.as_bigint();

    bigint<8> temp = c1 * modq;
    temp += c0;
    return temp;
}

// Writes a bigint in big endian
template<mp_size_t LIMBS>
void write_bigint(base_blob<8 * LIMBS * sizeof(mp_limb_t)> &blob, const bigint<LIMBS> &val)
{
    auto ptr = blob.begin();
    for (ssize_t i = LIMBS-1; i >= 0; i--, ptr += 8) {
        WriteBE64(ptr, val.data[i]);
    }
}

// Reads a bigint from big endian
template<mp_size_t LIMBS>
bigint<LIMBS> read_bigint(const base_blob<8 * LIMBS * sizeof(mp_limb_t)> &blob)
{
    bigint<LIMBS> ret;

    auto ptr = blob.begin();

    for (ssize_t i = LIMBS-1; i >= 0; i--, ptr += 8) {
        ret.data[i] = ReadBE64(ptr);
    }

    return ret;
}

template<>
Fq::Fq(curve_Fq element) : data()
{
    write_bigint<4>(data, element.as_bigint());
}

template<>
curve_Fq Fq::to_libsnark_fq() const
{
    auto element_bigint = read_bigint<4>(data);

    // Check that the integer is smaller than the modulus
    auto modq = curve_Fq::field_char();
    element_bigint.limit(modq, "element is not in Fq");

    return curve_Fq(element_bigint);
}

template<>
Fq2::Fq2(curve_Fq2 element) : data()
{
    write_bigint<8>(data, fq2_to_bigint(element));
}

template<>
curve_Fq2 Fq2::to_libsnark_fq2() const
{
    bigint<4> modq = curve_Fq::field_char();
    bigint<8> combined = read_bigint<8>(data);
    bigint<5> res;
    bigint<4> c0;
    bigint<8>::div_qr(res, c0, combined, modq);
    bigint<4> c1 = res.shorten(modq, "element is not in Fq2");

    return curve_Fq2(curve_Fq(c0), curve_Fq(c1));
}

template<>
CompressedG1::CompressedG1(curve_G1 point)
{
    if (point.is_zero()) {
        throw std::domain_error("curve point is zero");
    }

    point.to_affine_coordinates();

    x = Fq(point.X);
    y_lsb = point.Y.as_bigint().data[0] & 1;
}

template<>
curve_G1 CompressedG1::to_libsnark_g1() const
{
    curve_Fq x_coordinate = x.to_libsnark_fq<curve_Fq>();

    // y = +/- sqrt(x^3 + b)
    auto y_coordinate = ((x_coordinate.squared() * x_coordinate) + alt_bn128_coeff_b).sqrt();

    if ((y_coordinate.as_bigint().data[0] & 1) != y_lsb) {
        y_coordinate = -y_coordinate;
    }

    curve_G1 r = curve_G1::one();
    r.X = x_coordinate;
    r.Y = y_coordinate;
    r.Z = curve_Fq::one();

    assert(r.is_well_formed());

    return r;
}

template<>
CompressedG2::CompressedG2(curve_G2 point)
{
    if (point.is_zero()) {
        throw std::domain_error("curve point is zero");
    }

    point.to_affine_coordinates();

    x = Fq2(point.X);
    y_gt = fq2_to_bigint(point.Y) > fq2_to_bigint(-(point.Y));
}

template<>
curve_G2 CompressedG2::to_libsnark_g2() const
{
    auto x_coordinate = x.to_libsnark_fq2<curve_Fq2>();

    // y = +/- sqrt(x^3 + b)
    auto y_coordinate = ((x_coordinate.squared() * x_coordinate) + alt_bn128_twist_coeff_b).sqrt();
    auto y_coordinate_neg = -y_coordinate;

    if ((fq2_to_bigint(y_coordinate) > fq2_to_bigint(y_coordinate_neg)) != y_gt) {
        y_coordinate = y_coordinate_neg;
    }

    curve_G2 r = curve_G2::one();
    r.X = x_coordinate;
    r.Y = y_coordinate;
    r.Z = curve_Fq2::one();

    assert(r.is_well_formed());

    if (alt_bn128_modulus_r * r != curve_G2::zero()) {
        throw std::runtime_error("point is not in G2");
    }

    return r;
}

template<>
PHGRProof::PHGRProof(const r1cs_ppzksnark_proof<curve_pp> &proof)
{
    g_A = CompressedG1(proof.g_A.g);
    g_A_prime = CompressedG1(proof.g_A.h);
    g_B = CompressedG2(proof.g_B.g);
    g_B_prime = CompressedG1(proof.g_B.h);
    g_C = CompressedG1(proof.g_C.g);
    g_C_prime = CompressedG1(proof.g_C.h);
    g_K = CompressedG1(proof.g_K);
    g_H = CompressedG1(proof.g_H);
}

template<>
r1cs_ppzksnark_proof<curve_pp> PHGRProof::to_libsnark_proof() const
{
    r1cs_ppzksnark_proof<curve_pp> proof;

    proof.g_A.g = g_A.to_libsnark_g1<curve_G1>();
    proof.g_A.h = g_A_prime.to_libsnark_g1<curve_G1>();
    proof.g_B.g = g_B.to_libsnark_g2<curve_G2>();
    proof.g_B.h = g_B_prime.to_libsnark_g1<curve_G1>();
    proof.g_C.g = g_C.to_libsnark_g1<curve_G1>();
    proof.g_C.h = g_C_prime.to_libsnark_g1<curve_G1>();
    proof.g_K = g_K.to_libsnark_g1<curve_G1>();
    proof.g_H = g_H.to_libsnark_g1<curve_G1>();

    return proof;
}

PHGRProof PHGRProof::random_invalid()
{
    PHGRProof p;
    p.g_A = curve_G1::random_element();
    p.g_A_prime = curve_G1::random_element();
    p.g_B = curve_G2::random_element();
    p.g_B_prime = curve_G1::random_element();
    p.g_C = curve_G1::random_element();
    p.g_C_prime = curve_G1::random_element();

    p.g_K = curve_G1::random_element();
    p.g_H = curve_G1::random_element();

    return p;
}

static std::once_flag init_public_params_once_flag;

void initialize_curve_params()
{
    std::call_once (init_public_params_once_flag, curve_pp::init_public_params);
}

ProofVerifier ProofVerifier::Strict() {
    initialize_curve_params();
    return ProofVerifier(true);
}

ProofVerifier ProofVerifier::Disabled() {
    initialize_curve_params();
    return ProofVerifier(false);
}

template<>
bool ProofVerifier::check(
    const r1cs_ppzksnark_verification_key<curve_pp>& vk,
    const r1cs_ppzksnark_processed_verification_key<curve_pp>& pvk,
    const r1cs_primary_input<curve_Fr>& primary_input,
    const r1cs_ppzksnark_proof<curve_pp>& proof
)
{
    if (perform_verification) {
        return r1cs_ppzksnark_online_verifier_strong_IC<curve_pp>(pvk, primary_input, proof);
    } else {
        return true;
    }
}

}