#include <cmath>
#include <exception>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>

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

#include <frovedis.hpp>
#include <frovedis/core/dvector.hpp>
#include <frovedis/core/zipped_dvectors.hpp>
#include <frovedis/matrix/rowmajor_matrix.hpp>
#include <frovedis/ml/tree/tree_model.hpp>

namespace po = boost::program_options;
using namespace frovedis;

template <typename T>
void show_classification_error_rate(
  zipped_dvectors<T, T>& zipped, const T num_records
) {
  std::cout << "Error rate: " << zipped.map(
    +[] (const T predict, const T label) -> T {
      return static_cast<T>(label != predict);
    }
  ).reduce(add<T>) / num_records * 100 << "%" << std::endl;
}

template <typename T>
void show_regression_errors(
  zipped_dvectors<T, T>& zipped, const T num_records
) {
  const T mae = zipped.map(
    +[] (const T predict, const T label) -> T {
      return std::abs(label - predict);
    }
  ).reduce(add<T>) / num_records;

  const T mse = zipped.map(
    +[] (const T predict, const T label) -> T {
      const T error = label - predict;
      return error * error;
    }
  ).reduce(add<T>) / num_records;

  std::cout << "MAE:  " << mae << std::endl;
  std::cout << "MSE:  " << mse << std::endl;
  std::cout << "RMSE: " << std::sqrt(mse) << std::endl;
}

template <typename T>
void do_validate(
  const std::vector<T>& predicts, dvector<T>& labels,
  const tree::algorithm algo
) {
  const T num_records = static_cast<T>(predicts.size());
  auto dpredicts = make_dvector_scatter(predicts);
  auto zipped = zip(dpredicts, labels);

  switch (algo) {
  case tree::algorithm::Classification:
    show_classification_error_rate(zipped, num_records);
    return;
  case tree::algorithm::Regression:
    show_regression_errors(zipped, num_records);
    return;
  default:
    throw std::logic_error("invalid tree algorithm");
  }
}

template <typename T>
void do_predict(
  const std::string& data_path,
  const std::string& model_path,
  const std::string& output_path,
  const std::string& label_path,
  const bool binary_mode
) {
  time_spent timer(DEBUG);
  decision_tree_model<T> model;

  if (binary_mode) {
    timer.reset();
    auto dataset = make_rowmajor_matrix_local_loadbinary<T>(data_path);
    timer.show("load matrix: ");
    model.loadbinary(model_path);
    timer.show("load model:  ");
    auto results = model.predict(dataset);
    timer.show("predict:     ");
    make_dvector_scatter(results).savebinary(output_path);
    timer.show("save result: ");

    if (!label_path.empty()) {
      timer.reset();
      auto labels = make_dvector_loadbinary<T>(label_path);
      timer.show("load labels: ");
      do_validate(results, labels, model.get_algo());
      timer.show("validate:    ");
    }
  } else {
    timer.reset();
    auto dataset = make_rowmajor_matrix_local_load<T>(data_path);
    timer.show("load matrix: ");
    model.load(model_path);
    timer.show("load model:  ");
    auto results = model.predict(dataset);
    timer.show("predict:     ");
    make_dvector_scatter(results).saveline(output_path);
    timer.show("save result: ");

    if (!label_path.empty()) {
      timer.reset();
      auto labels = make_dvector_loadline(label_path).map(
        +[] (const std::string& line) -> T {
          return boost::lexical_cast<T>(line);
        }
      );
      timer.show("load labels: ");
      do_validate(results, labels, model.get_algo());
      timer.show("validate:    ");
    }
  }
}

template <typename T>
void do_predict(const po::variables_map& argmap) {
  do_predict<T>(
    argmap["input"].as<std::string>(),
    argmap["model"].as<std::string>(),
    argmap["output"].as<std::string>(),
    argmap["label"].as<std::string>(),
    argmap.count("binary")
  );
}

po::variables_map parse(int argc, char** argv) {
  po::options_description opt_desc("");
  opt_desc.add_options()
    ("help,h", "show this help message and exit");

  po::options_description reqarg_desc("required arguments");
  reqarg_desc.add_options()
    ("input,i", po::value<std::string>(), "an input matrix")
    ("model,m", po::value<std::string>(), "an input model")
    ("output,o", po::value<std::string>(), "an output prediction result");

  po::options_description optarg_desc("optional arguments");
  optarg_desc.add_options()
    ("label,l", po::value<std::string>()->default_value(""),
     "a correct label for validation")
    ("binary", "use binary input/output")
    ("double", "use double precision")
    ("verbose", "set log-level to DEBUG")
    ("trace", "set log-level to TRACE");

  opt_desc.add(reqarg_desc).add(optarg_desc);
  po::variables_map argmap;
  try {
    po::store(po::command_line_parser(argc, argv)
                .options(opt_desc)
//              .allow_unregistered()
                .run(),
              argmap);
    po::notify(argmap);
  } catch (const po::error_with_option_name& e) {
    std::cerr << e.what() << std::endl;
    finalizefrovedis(1);
  }

  // help message
  if (argmap.count("help")) {
    std::cerr << opt_desc;
    finalizefrovedis(0);
  }

  // check required arguments
  bool missing = false;
  for (const auto opt: reqarg_desc.options()) {
    const std::string& name = opt->long_name();
    if (!argmap.count(name)) {
      std::cerr << "option '--" << name << "' is required" << std::endl;
      missing = true;
    }
  }
  if (missing) { finalizefrovedis(1); }

  return argmap;
}

int main(int argc, char** argv) {
  use_frovedis use(argc, argv);

  const auto argmap = parse(argc, argv);
  if (argmap.count("verbose")) { set_loglevel(DEBUG); }
  if (argmap.count("trace")) { set_loglevel(TRACE); }

  try {
    if (argmap.count("double")) {
      do_predict<double>(argmap);
    } else {
      do_predict<float>(argmap);
    }
  } catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
    finalizefrovedis(1);
  }

  return 0;
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/
//#define DEBUG_NOCOLOR
//#define DEBUG_BEGIN_END_ONLY
// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "RotationQuaternion.hpp"

#include "siconos_debug.h"
#include "SiconosVector.hpp"
#include "SimpleMatrix.hpp"
#include <boost/math/quaternion.hpp>

void computeRotationMatrix(double q0, double q1, double q2, double q3,
                           SP::SimpleMatrix rotationMatrix)
{

  /* Brute force version by multiplication of quaternion
   */
  // ::boost::math::quaternion<double>    quatQ(q0, q1, q2, q3);
  // ::boost::math::quaternion<double>    quatcQ(q0, -q1, -q2, -q3);
  // ::boost::math::quaternion<double>    quatx(0, 1, 0, 0);
  // ::boost::math::quaternion<double>    quaty(0, 0, 1, 0);
  // ::boost::math::quaternion<double>    quatz(0, 0, 0, 1);
  // ::boost::math::quaternion<double>    quatBuff;
  // quatBuff = quatQ * quatx * quatcQ;
  // rotationMatrix->setValue(0, 0, quatBuff.R_component_2());
  // rotationMatrix->setValue(1, 0, quatBuff.R_component_3());
  // rotationMatrix->setValue(2, 0, quatBuff.R_component_4());
  // quatBuff = quatQ * quaty * quatcQ;
  // rotationMatrix->setValue(0, 1, quatBuff.R_component_2());
  // rotationMatrix->setValue(1, 1, quatBuff.R_component_3());
  // rotationMatrix->setValue(2, 1, quatBuff.R_component_4());
  // quatBuff = quatQ * quatz * quatcQ;
  // rotationMatrix->setValue(0, 2, quatBuff.R_component_2());
  // rotationMatrix->setValue(1, 2, quatBuff.R_component_3());
  // rotationMatrix->setValue(2, 2, quatBuff.R_component_4());

  /* direct computation https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation */
  rotationMatrix->setValue(0, 0,     q0*q0 +q1*q1 -q2*q2 -q3*q3);
  rotationMatrix->setValue(0, 1, 2.0*(q1*q2        - q0*q3));
  rotationMatrix->setValue(0, 2, 2.0*(q1*q3        + q0*q2));

  rotationMatrix->setValue(1, 0, 2.0*(q1*q2        + q0*q3));
  rotationMatrix->setValue(1, 1,     q0*q0 -q1*q1 +q2*q2 -q3*q3);
  rotationMatrix->setValue(1, 2, 2.0*(q2*q3        - q0*q1));

  rotationMatrix->setValue(2, 0, 2.0*(q1*q3        - q0*q2));
  rotationMatrix->setValue(2, 1, 2.0*(q2*q3         + q0*q1));
  rotationMatrix->setValue(2, 2,     q0*q0 -q1*q1 -q2*q2 +q3*q3);
}

void quaternionRotate(double q0, double q1, double q2, double q3, SiconosVector& v)
{
  DEBUG_BEGIN("::quaternionRotate(double q0, double q1, double q2, double q3, SiconosVector& v )\n");
  DEBUG_EXPR(v.display(););
  DEBUG_PRINTF("( q0 = %16.12e,  q1 = %16.12e,  q2= %16.12e,  q3= %16.12e )\n", q0,q1,q2,q3);
  assert(v.size()==3);

  // First way. Using the rotation matrix
  // SP::SimpleMatrix rotationMatrix(new SimpleMatrix(3,3));
  // SiconosVector tmp(3);
  // ::computeRotationMatrix(q0,q1,q2,q3, rotationMatrix);
  // prod(*rotationMatrix, v, tmp);
  // v = tmp;
  // return;

  // Second way. Using the transpose of the rotation matrix
  // SP::SimpleMatrix rotationMatrix(new SimpleMatrix(3,3));
  // SiconosVector tmp(3);
  // ::computeRotationMatrix(q0,-q1,-q2,-q3, rotationMatrix);
  // prod(v, *rotationMatrix, tmp);
  // v = tmp;

  // Third way. cross product and axis angle
  // see http://www.geometrictools.com/Documentation/RotationIssues.pdf
  // SP::SiconosVector axis(new SiconosVector(3));
  // double angle = ::axisAngleFromQuaternion(q0,q1,q2,q3, axis);
  // SiconosVector t(3), tmp(3);
  // cross_product(*axis,v,t);
  // cross_product(*axis,t,tmp);
  // v += sin(angle)*t + (1.0-cos(angle))*tmp;

  // Direct computation with cross product
  // Works only with unit quaternion
  SiconosVector t(3), tmp(3);
  SiconosVector qvect(3);
  qvect(0)=q1;
  qvect(1)=q2;
  qvect(2)=q3;
  cross_product(qvect,v,t);
  t *= 2.0;
  cross_product(qvect,t,tmp);
  v += tmp;
  v += q0*t;
  DEBUG_EXPR(v.display(););
  DEBUG_END("::quaternionRotate(double q0, double q1, double q2, double q3, SP::SiconosVector v )\n");
}

void quaternionRotate(double q0, double q1, double q2, double q3, SP::SiconosVector v)
{
  ::quaternionRotate(q0, q1, q2, q3, *v);
}

void quaternionRotate(double q0, double q1, double q2, double q3, SP::SimpleMatrix m)
{
  DEBUG_BEGIN("::quaternionRotate(double q0, double q1, double q2, double q3, SP::SimpleMatrix m )\n");
  DEBUG_EXPR(m->display(););
  DEBUG_PRINTF("( q0 = %16.12e,  q1 = %16.12e,  q2= %16.12e,  q3= %16.12e )\n", q0,q1,q2,q3);

  // Direct computation with cross product for each column
  assert(m->size(0) == 3 && "::quaternionRotate(double q0, double q1, double q2, double q3, SP::SimpleMatrix m ) m must have 3 rows");
  SiconosVector v(3);
  SiconosVector t(3), tmp(3);
  SiconosVector qvect(3);
  qvect(0)=q1;
  qvect(1)=q2;
  qvect(2)=q3;
  for(unsigned int j = 0; j < m->size(1); j++)
  {
    v(0) = m->getValue(0,j);
    v(1) = m->getValue(1,j);
    v(2) = m->getValue(2,j);
    cross_product(qvect,v,t);
    t *= 2.0;
    cross_product(qvect,t,tmp);
    v += tmp;
    v += q0*t;
    m->setValue(0,j,v(0));
    m->setValue(1,j,v(1));
    m->setValue(2,j,v(2));
  }
  DEBUG_EXPR(m->display(););
  DEBUG_END("::quaternionRotate(double q0, double q1, double q2, double q3, SP::SimpleMatrix m )\n");
}

void quaternionRotate(SP::SiconosVector q, SP::SiconosVector v)
{
  DEBUG_BEGIN("::quaternionRotate(SP::SiconosVector q, SP::SiconosVector v )\n");
  ::quaternionRotate(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6), v);
  DEBUG_END("::quaternionRotate(SP::SiconosVector q, SP::SiconosVector v )\n");
}

void quaternionRotate(SP::SiconosVector q, SP::SimpleMatrix m)
{
  DEBUG_BEGIN("::quaternionRotate(SP::SiconosVector q, SP::SimpleMatrix m )\n");
  ::quaternionRotate(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6),m);
  DEBUG_END("::quaternionRotate(SP::SiconosVector q, SP::SimpleMatrix m)\n");
}

void changeFrameAbsToBody(const SiconosVector& q, SiconosVector& v)
{
  DEBUG_BEGIN("::changeFrameAbsToBody(const SiconosVector& q, SiconosVector& v )\n");
  ::quaternionRotate(q.getValue(3),-q.getValue(4),-q.getValue(5),-q.getValue(6), v);
  DEBUG_END("::changeFrameAbsToBody(const SiconosVector& q, SiconosVector& v )\n");
}
void changeFrameAbsToBody(SP::SiconosVector q, SP::SiconosVector v)
{
  DEBUG_BEGIN("::changeFrameAbsToBody(SP::SiconosVector q, SP::SiconosVector v )\n");
  ::quaternionRotate(q->getValue(3),-q->getValue(4),-q->getValue(5),-q->getValue(6), v);
  DEBUG_END("::changeFrameAbsToBody(SP::SiconosVector q, SP::SiconosVector v )\n");
}
void changeFrameAbsToBody(SP::SiconosVector q, SP::SimpleMatrix m)
{
  DEBUG_BEGIN("::changeFrameAbsToBody(SP::SiconosVector q, SP::SimpleMatrix m )\n");
  ::quaternionRotate(q->getValue(3),-q->getValue(4),-q->getValue(5),-q->getValue(6), m);
  DEBUG_END("::changeFrameAbsToBody(SP::SiconosVector q, SP::SimpleMatrix m )\n");
}

void changeFrameBodyToAbs(const SiconosVector& q, SiconosVector& v)
{
  DEBUG_BEGIN("::changeFrameBodyToAbs(const SiconosVector& q, SiconosVector& v )\n");
  ::quaternionRotate(q.getValue(3),q.getValue(4),q.getValue(5),q.getValue(6), v);
  DEBUG_END("::changeFrameBodyToAbs(const SiconosVector& q, SiconosVector& v )\n");
}
void changeFrameBodyToAbs(SP::SiconosVector q, SP::SiconosVector v)
{
  DEBUG_BEGIN("::changeFrameBodyToAbs(SP::SiconosVector q, SP::SiconosVector v )\n");
  ::quaternionRotate(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6), *v);
  DEBUG_END("::changeFrameBodyToAbs(SP::SiconosVector q, SP::SiconosVector v )\n");
}
void changeFrameBodyToAbs(SP::SiconosVector q, SP::SimpleMatrix m)
{
  DEBUG_BEGIN("::changeFrameBodyToAbs(SP::SiconosVector q, SP::SimpleMatrix m )\n");
  ::quaternionRotate(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6), m);
  DEBUG_END("::changeFrameBodyToAbs(SP::SiconosVector q, SP::SimpleMatrix m )\n");
}

void computeRotationMatrix(SP::SiconosVector q, SP::SimpleMatrix rotationMatrix)
{
  ::computeRotationMatrix(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6),
                          rotationMatrix);
}
void computeRotationMatrixTransposed(SP::SiconosVector q, SP::SimpleMatrix rotationMatrix)
{
  ::computeRotationMatrix(q->getValue(3),-q->getValue(4),-q->getValue(5),-q->getValue(6),
                          rotationMatrix);
}

double axisAngleFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector axis)
{
  DEBUG_BEGIN("axisAngleFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector axis )\n");
  double angle = acos(q0) *2.0;
  //double f = sin( angle *0.5);
  double f = sqrt(1-q0*q0); // cheaper than sin ?
  if(f !=0.0)
  {
    axis->setValue(0, q1/f);
    axis->setValue(1, q2/f);
    axis->setValue(2, q3/f);
  }
  else
  {
    axis->zero();
  }
  DEBUG_PRINTF("angle= %12.8e\n", angle);
  DEBUG_EXPR(axis->display(););
  DEBUG_END("axisAngleFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector axis )\n");
  return angle;
}

double axisAngleFromConfiguration(SP::SiconosVector q, SP::SiconosVector axis)
{
  double angle = ::axisAngleFromQuaternion(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6),axis);
  return angle;
}

void rotationVectorFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector rotationVector)
{
  DEBUG_BEGIN("rotationVectorFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector rotationVector )\n");

  rotationVector->setValue(0, q1);
  rotationVector->setValue(1, q2);
  rotationVector->setValue(2, q3);

  double norm_v = sqrt(q1*q1+q2*q2+q3*q3);
  assert(norm_v <= M_PI);  /* it should be called for a unit quaternion */
  if(norm_v < 1e-12)
  {
    rotationVector->setValue(0, 0.0);
    rotationVector->setValue(1, 0.0);
    rotationVector->setValue(2, 0.0);
  }
  else
  {
    *rotationVector *=  2.0 * asin(norm_v)/norm_v;
  }
  DEBUG_EXPR(rotationVector->display(););
  DEBUG_END("rotationVectorFromQuaternion(double q0, double q1, double q2, double q3, SP::SiconosVector rotationVector )\n");
}

void rotationVectorFromConfiguration(SP::SiconosVector q, SP::SiconosVector rotationVector)
{
  ::rotationVectorFromQuaternion(q->getValue(3),q->getValue(4),q->getValue(5),q->getValue(6), rotationVector);
}

void quaternionFromAxisAngle(SP::SiconosVector axis, double angle, SP::SiconosVector q)
{
  q->setValue(3,cos(angle/2.0));
  q->setValue(4,axis->getValue(0)* sin(angle *0.5));
  q->setValue(5,axis->getValue(1)* sin(angle *0.5));
  q->setValue(6,axis->getValue(2)* sin(angle *0.5));
}

static
double sin_x(double x)
{
  if(std::abs(x) <= 1e-3)
  {
    return 1.0 + x*x / 3.0 + pow(x,4) * 2.0 / 15.0 + pow(x,6) * 17.0 / 315.0 + pow(x,8) * 62.0 / 2835.0;
  }
  else
  {
    return sin(x)/x;
  }
}

void quaternionFromRotationVector(SP::SiconosVector rotationVector, SP::SiconosVector q)
{
  double angle = sqrt(rotationVector->getValue(0)*rotationVector->getValue(0)+
                      rotationVector->getValue(1)*rotationVector->getValue(1)+
                      rotationVector->getValue(2)*rotationVector->getValue(2));

  double f = 0.5 * sin_x(angle *0.5);

  q->setValue(3,cos(angle/2.0));
  q->setValue(4,rotationVector->getValue(0)* f);
  q->setValue(5,rotationVector->getValue(1)* f);
  q->setValue(6,rotationVector->getValue(2)* f);
}

double quaternionNorm(const SiconosVector &q)
{
  double normq = sqrt(q.getValue(3) * q.getValue(3) +
                      q.getValue(4) * q.getValue(4) +
                      q.getValue(5) * q.getValue(5) +
                      q.getValue(6) * q.getValue(6));
  return normq;
}

void normalizeq(SP::SiconosVector q)
{
  double normq = sqrt(q->getValue(3) * q->getValue(3) +
                      q->getValue(4) * q->getValue(4) +
                      q->getValue(5) * q->getValue(5) +
                      q->getValue(6) * q->getValue(6));
  assert(normq > 0);
  normq = 1.0 / normq;
  q->setValue(3, q->getValue(3) * normq);
  q->setValue(4, q->getValue(4) * normq);
  q->setValue(5, q->getValue(5) * normq);
  q->setValue(6, q->getValue(6) * normq);
}

void  normalizeq(SiconosVector &q)
{
  double normq = sqrt(q.getValue(3) * q.getValue(3) +
                      q.getValue(4) * q.getValue(4) +
                      q.getValue(5) * q.getValue(5) +
                      q.getValue(6) * q.getValue(6));
  assert(normq > 0);
  normq = 1.0 / normq;
  q.setValue(3, q.getValue(3) * normq);
  q.setValue(4, q.getValue(4) * normq);
  q.setValue(5, q.getValue(5) * normq);
  q.setValue(6, q.getValue(6) * normq);
}

void quaternionFromTwistVector(SiconosVector& twist, SiconosVector& q)
 {
   assert(twist.size() == 6);
   assert(q.size() == 7);
   double angle = sqrt(twist.getValue(3)*twist.getValue(3)+
                       twist.getValue(4)*twist.getValue(4)+
                       twist.getValue(5)*twist.getValue(5));

   double f = 0.5 * sin_x(angle *0.5);

   q.setValue(3,cos(angle/2.0));
   q.setValue(4,twist.getValue(3)* f);
   q.setValue(5,twist.getValue(4)* f);
   q.setValue(6,twist.getValue(5)* f);

 }
void compositionLawLieGroup(const SiconosVector& a, SiconosVector& b, SiconosVector& ab)
{

  assert(a.size() == 7);
  assert(b.size() == 7);
  assert(ab.size()== 7);

  // For the translational component, the composition law is the addition
  ab.setValue(0,a.getValue(0)+b.getValue(0));
  ab.setValue(1,a.getValue(1)+b.getValue(1));
  ab.setValue(2,a.getValue(2)+b.getValue(2));

  // For the quaternion that encodes rotation, the composition law is the quaternion product.
  ::boost::math::quaternion<double>    quat_a(a.getValue(3), a.getValue(4), a.getValue(5), a.getValue(6));
  ::boost::math::quaternion<double>    quat_b(b.getValue(3), b.getValue(4), b.getValue(5), b.getValue(6));
  ::boost::math::quaternion<double>    quat_ab = quat_a * quat_b;
  ab.setValue(3,quat_ab.R_component_1());
  ab.setValue(4,quat_ab.R_component_2());
  ab.setValue(5,quat_ab.R_component_3());
  ab.setValue(6,quat_ab.R_component_4());
}

void compositionLawLieGroup(const SiconosVector& a, SiconosVector& b)
{

  assert(a.size() == 7);
  assert(b.size() == 7);

  // For the translational component, the composition law is the addition
  b.setValue(0,a.getValue(0)+b.getValue(0));
  b.setValue(1,a.getValue(1)+b.getValue(1));
  b.setValue(2,a.getValue(2)+b.getValue(2));

  // For the quaternion that encodes rotation, the composition law is the quaternion product.
  ::boost::math::quaternion<double>    quat_a(a.getValue(3), a.getValue(4), a.getValue(5), a.getValue(6));
  ::boost::math::quaternion<double>    quat_b(b.getValue(3), b.getValue(4), b.getValue(5), b.getValue(6));
  ::boost::math::quaternion<double>    quat_ab = quat_a * quat_b;
  b.setValue(3,quat_ab.R_component_1());
  b.setValue(4,quat_ab.R_component_2());
  b.setValue(5,quat_ab.R_component_3());
  b.setValue(6,quat_ab.R_component_4());
  //normalizeq(b);
}


#ifndef BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP
#define BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Purpose:  Implementation of the aitoff (Aitoff) and wintri (Winkel Tripel)
// projections.
// Author:   Gerald Evenden
// Copyright (c) 1995, Gerald Evenden

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace aitoff
    {

            struct par_aitoff
            {
                double    cosphi1;
                int        mode;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_aitoff_spheroid : public base_t_fi<base_aitoff_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_aitoff m_proj_parm;

                inline base_aitoff_spheroid(const Parameters& par)
                    : base_t_fi<base_aitoff_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double c, d;

                    if((d = acos(cos(lp_lat) * cos(c = 0.5 * lp_lon)))) {/* basic Aitoff */
                        xy_x = 2. * d * cos(lp_lat) * sin(c) * (xy_y = 1. / sin(d));
                        xy_y *= d * sin(lp_lat);
                    } else
                        xy_x = xy_y = 0.;
                    if (this->m_proj_parm.mode) { /* Winkel Tripel */
                        xy_x = (xy_x + lp_lon * this->m_proj_parm.cosphi1) * 0.5;
                        xy_y = (xy_y + lp_lat) * 0.5;
                    }
                }
                /***********************************************************************************
                *
                * Inverse functions added by Drazen Tutic and Lovro Gradiser based on paper:
                *
                * I.Özbug Biklirici and Cengizhan Ipbüker. A General Algorithm for the Inverse
                * Transformation of Map Projections Using Jacobian Matrices. In Proceedings of the
                * Third International Symposium Mathematical & Computational Applications,
                * pages 175{182, Turkey, September 2002.
                *
                * Expected accuracy is defined by EPSILON = 1e-12. Should be appropriate for
                * most applications of Aitoff and Winkel Tripel projections.
                *
                * Longitudes of 180W and 180E can be mixed in solution obtained.
                *
                * Inverse for Aitoff projection in poles is undefined, longitude value of 0 is assumed.
                *
                * Contact : dtutic@geof.hr
                * Date: 2015-02-16
                *
                ************************************************************************************/

                // INVERSE(s_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                        int iter, MAXITER = 10, round = 0, MAXROUND = 20;
                    double EPSILON = 1e-12, D, C, f1, f2, f1p, f1l, f2p, f2l, dp, dl, sl, sp, cp, cl, x, y;

                    if ((fabs(xy_x) < EPSILON) && (fabs(xy_y) < EPSILON )) { lp_lat = 0.; lp_lon = 0.; return; }

                    /* intial values for Newton-Raphson method */
                    lp_lat = xy_y; lp_lon = xy_x;
                    do {
                        iter = 0;
                        do {
                            sl = sin(lp_lon * 0.5); cl = cos(lp_lon * 0.5);
                            sp = sin(lp_lat); cp = cos(lp_lat);
                            D = cp * cl;
                                   C = 1. - D * D;
                            D = acos(D) / pow(C, 1.5);
                                   f1 = 2. * D * C * cp * sl;
                                   f2 = D * C * sp;
                                   f1p = 2.* (sl * cl * sp * cp / C - D * sp * sl);
                                   f1l = cp * cp * sl * sl / C + D * cp * cl * sp * sp;
                                f2p = sp * sp * cl / C + D * sl * sl * cp;
                                  f2l = 0.5 * (sp * cp * sl / C - D * sp * cp * cp * sl * cl);
                                  if (this->m_proj_parm.mode) { /* Winkel Tripel */
                                f1 = 0.5 * (f1 + lp_lon * this->m_proj_parm.cosphi1);
                                f2 = 0.5 * (f2 + lp_lat);
                                f1p *= 0.5;
                                f1l = 0.5 * (f1l + this->m_proj_parm.cosphi1);
                                f2p = 0.5 * (f2p + 1.);
                                f2l *= 0.5;
                            }
                            f1 -= xy_x; f2 -= xy_y;
                            dl = (f2 * f1p - f1 * f2p) / (dp = f1p * f2l - f2p * f1l);
                            dp = (f1 * f2l - f2 * f1l) / dp;
                            while (dl > geometry::math::pi<double>()) dl -= geometry::math::pi<double>(); /* set to interval [-geometry::math::pi<double>(), geometry::math::pi<double>()]  */
                            while (dl < -geometry::math::pi<double>()) dl += geometry::math::pi<double>(); /* set to interval [-geometry::math::pi<double>(), geometry::math::pi<double>()]  */
                            lp_lat -= dp;    lp_lon -= dl;
                        } while ((fabs(dp) > EPSILON || fabs(dl) > EPSILON) && (iter++ < MAXITER));
                        if (lp_lat > geometry::math::two_pi<double>()) lp_lat -= 2.*(lp_lat-geometry::math::two_pi<double>()); /* correct if symmetrical solution for Aitoff */
                        if (lp_lat < -geometry::math::two_pi<double>()) lp_lat -= 2.*(lp_lat+geometry::math::two_pi<double>()); /* correct if symmetrical solution for Aitoff */
                        if ((fabs(fabs(lp_lat) - geometry::math::two_pi<double>()) < EPSILON) && (!this->m_proj_parm.mode)) lp_lon = 0.; /* if pole in Aitoff, return longitude of 0 */

                        /* calculate x,y coordinates with solution obtained */
                        if((D = acos(cos(lp_lat) * cos(C = 0.5 * lp_lon)))) {/* Aitoff */
                            x = 2. * D * cos(lp_lat) * sin(C) * (y = 1. / sin(D));
                            y *= D * sin(lp_lat);
                        } else
                            x = y = 0.;
                        if (this->m_proj_parm.mode) { /* Winkel Tripel */
                            x = (x + lp_lon * this->m_proj_parm.cosphi1) * 0.5;
                            y = (y + lp_lat) * 0.5;
                        }
                    /* if too far from given values of x,y, repeat with better approximation of phi,lam */
                    } while (((fabs(xy_x-x) > EPSILON) || (fabs(xy_y-y) > EPSILON)) && (round++ < MAXROUND));

                    if (iter == MAXITER && round == MAXROUND) fprintf(stderr, "Warning: Accuracy of 1e-12 not reached. Last increments: dlat=%e and dlon=%e\n", dp, dl);
                }

                static inline std::string get_name()
                {
                    return "aitoff_spheroid";
                }

            };

            template <typename Parameters>
            void setup(Parameters& par, par_aitoff& proj_parm)
            {
                boost::ignore_unused(proj_parm);
                par.es = 0.;
            }

            // Aitoff
            template <typename Parameters>
            void setup_aitoff(Parameters& par, par_aitoff& proj_parm)
            {
                proj_parm.mode = 0;
                setup(par, proj_parm);
            }

            // Winkel Tripel
            template <typename Parameters>
            void setup_wintri(Parameters& par, par_aitoff& proj_parm)
            {
                proj_parm.mode = 1;
                if (pj_param(par.params, "tlat_1").i)
                    {
                    if ((proj_parm.cosphi1 = cos(pj_param(par.params, "rlat_1").f)) == 0.)
                        throw proj_exception(-22);
                    }
                else /* 50d28' or acos(2/pi) */
                    proj_parm.cosphi1 = 0.636619772367581343;
                setup(par, proj_parm);
            }

        }} // namespace detail::aitoff
    #endif // doxygen

    /*!
        \brief Aitoff projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Example
        \image html ex_aitoff.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct aitoff_spheroid : public detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>
    {
        inline aitoff_spheroid(const Parameters& par) : detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::aitoff::setup_aitoff(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Winkel Tripel projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - lat_1: Latitude of first standard parallel (degrees)
        \par Example
        \image html ex_wintri.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct wintri_spheroid : public detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>
    {
        inline wintri_spheroid(const Parameters& par) : detail::aitoff::base_aitoff_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::aitoff::setup_wintri(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class aitoff_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<aitoff_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        class wintri_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<wintri_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void aitoff_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("aitoff", new aitoff_entry<Geographic, Cartesian, Parameters>);
            factory.add_to_factory("wintri", new wintri_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_AITOFF_HPP


//
// Created by Standard on 17/04/2021.
//
#include "MTTensor.h"
#include "gnuplot-iostream.h"
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/program_options.hpp>
#include "Parser.h"
#include <fstream>
#include <strstream>
#include <algorithm>
#include <cmath>
#include <boost/filesystem.hpp>

struct measure{
    double val{nan("")};
    double error{0};
};

constexpr double mu0 = M_PI*4e-7;

measure get_rho(double z_real, double z_imag, double z_var, double freq);
measure get_phi(double z_real, double z_imag, double z_var);

boost::program_options::options_description parse_cmdline(int argc, char *argv[], boost::program_options::variables_map& p_vm);
std::string getFileContents(std::ifstream&);

int main(int argc, char* argv[]){
    try {
        // program configuration
        boost::program_options::variables_map vm;
        auto desc = parse_cmdline(argc, argv, vm);
        if (vm.count("help")) {
            std::cout << desc << std::endl;
            return 0;
        }
        auto fileName = vm["in-path"].as<std::string>();
        auto pathObject = boost::filesystem::path(fileName);
        std::cout << "handling file: " << pathObject.filename().string() << "\n";
        std::ifstream is;
        is.open(fileName);
        if(!is.is_open())throw std::runtime_error("Cannot open file " + fileName+ "\n");
        std::string fileContents = getFileContents(is);
        is.close();
        MTparser::Parser p;
        p.parse(fileContents);
        auto skip_string = p.get_option_list_for(">HEAD")["EMPTY"];
        std::cout << "skip_string: " << skip_string << "\n";
        auto edi_contents = p.get();
        MTparser::Option_list ol;
        MTparser::Data_set ds;
//        for (auto it=edi_contents.begin();it!=edi_contents.end();it++) {
//            std::cout << it->first << ":\n\toption list:\n";
//            ol = p.get_option_list_for(it->first);
//            for (auto oit = ol.begin(); oit != ol.end(); oit++) {
//                std::cout << "\t\t" << oit->first << "==" << oit->second << std::endl;
//            }
//        }

        Gnuplot gp;
        auto freq = MTparser::dataset2double(p.get_data_set_for(">FREQ"), skip_string);
        // OFF-DIAG
        auto zxyr = MTparser::dataset2double(p.get_data_set_for(">ZXYR"), skip_string);
        auto zyxr = MTparser::dataset2double(p.get_data_set_for(">ZYXR"), skip_string);
        auto zxyi = MTparser::dataset2double(p.get_data_set_for(">ZXYI"), skip_string);
        auto zyxi = MTparser::dataset2double(p.get_data_set_for(">ZYXI"), skip_string);
        // MAIN-DIAG
        auto zxxr = MTparser::dataset2double(p.get_data_set_for(">ZXXR"), skip_string);
        auto zyyr = MTparser::dataset2double(p.get_data_set_for(">ZYYR"), skip_string);
        auto zxxi = MTparser::dataset2double(p.get_data_set_for(">ZXXI"), skip_string);
        auto zyyi = MTparser::dataset2double(p.get_data_set_for(">ZYYI"), skip_string);

        auto zxyv = MTparser::dataset2double(p.get_data_set_for(">ZXY.VAR"), skip_string);
        auto zyxv = MTparser::dataset2double(p.get_data_set_for(">ZYX.VAR"), skip_string);
        auto zxxv = MTparser::dataset2double(p.get_data_set_for(">ZXX.VAR"), skip_string);
        auto zyyv = MTparser::dataset2double(p.get_data_set_for(">ZYY.VAR"), skip_string);

        std::vector<double> zxys,zyxs,zxxs,zyys;
        for (auto v : zxyv) zxys.push_back(std::sqrt(v));
        for (auto v : zyxv) zyxs.push_back(std::sqrt(v));
        for (auto v : zxxv) zxxs.push_back(std::sqrt(v));
        for (auto v : zyyv) zyys.push_back(std::sqrt(v));

        auto title = pathObject.filename().string();

        auto pos = title.find('_');
        if(pos!=std::string::npos)title.insert(pos,"\\");
        gp << "set term qt enhanced 'Times-Roman, 9'\n";
        gp << "set title '" + title +"'\n";
        gp << "unset key\n";
        gp << "set multiplot layout 2,2\n";
        gp << "set logscale x\n";
//        gp << "set logscale y\n";
        if (false) {
            gp << "set xlabel 'Frequency (Hz)'\n";
            gp << "set ylabel 'Impedance (Ohm)'\n";

            // XX
            gp << "set title '" + title + "-XX'\n";
            gp << "plot "
               << "'-' with errorbars pt 3 lc rgb '#F0BC42' title 're(z_{xx})', "
               << "'-' with errorbars pt 4 lc rgb '#8E1F2F' title 'im(z_{xx})'"
               << "\n";
            gp.send1d(std::make_tuple(freq, zxxr, zxxs));
            gp.send1d(std::make_tuple(freq, zxxi, zxxs));
            // XY
            gp << "set title '" + title + "-XY'\n";
            gp << "plot "
               << "'-' with errorbars pt 3 lc rgb '#F0BC42' title 're(z_{xy})', "
               << "'-' with errorbars pt 4 lc rgb '#8E1F2F' title 'im(z_{xy})'"
               << "\n";
            gp.send1d(std::make_tuple(freq, zxyr, zxys));
            gp.send1d(std::make_tuple(freq, zxyi, zxys));
            // YX
            gp << "set title '" + title + "-YX'\n";
            gp << "plot "
               << "'-' with errorbars pt 3 lc rgb '#F0BC42' title 're(z_{yx})', "
               << "'-' with errorbars pt 4 lc rgb '#8E1F2F' title 'im(z_{yx})'"
               << "\n";
            gp.send1d(std::make_tuple(freq, zyxr, zyxs));
            gp.send1d(std::make_tuple(freq, zyxi, zyxs));
            // YY
            gp << "set title '" + title + "-YY'\n";
            gp << "plot "
               << "'-' with errorbars pt 3 lc rgb '#F0BC42' title 're(z_{yy})', "
               << "'-' with errorbars pt 4 lc rgb '#8E1F2F' title 'im(z_{yy})'"
               << "\n";
            gp.send1d(std::make_tuple(freq, zyyr, zyys));
            gp.send1d(std::make_tuple(freq, zyyi, zyys));
        } else {
            std::vector<double> rhoxx, srhoxx,phixx, sphixx;
            for(int i=0;i<zxxr.size();++i){
                auto this_rho = get_rho(zxxr[i],zxxi[i],zxxv[i],freq[i]);
                rhoxx.push_back(this_rho.val);
                srhoxx.push_back(this_rho.error);
                auto this_phi = get_phi(zxxr[i],zxxi[i],zxxv[i]);
                phixx.push_back(this_phi.val);
                sphixx.push_back(this_phi.error);
            }

            std::vector<double> rhoxy, srhoxy,phixy, sphixy;
            for(int i=0;i<zxyr.size();++i){
                auto this_rho = get_rho(zxyr[i],zxyi[i],zxyv[i],freq[i]);
                rhoxy.push_back(this_rho.val);
                srhoxy.push_back(this_rho.error);
                auto this_phi = get_phi(zxyr[i],zxyi[i],zxyv[i]);
                phixy.push_back(this_phi.val);
                sphixy.push_back(this_phi.error);
            }

            std::vector<double> rhoyx, srhoyx,phiyx, sphiyx;
            for(int i=0;i<zyxr.size();++i){
                auto this_rho = get_rho(zyxr[i],zyxi[i],zyxv[i],freq[i]);
                rhoyx.push_back(this_rho.val);
                srhoyx.push_back(this_rho.error);
                auto this_phi = get_phi(zyxr[i],zyxi[i],zyxv[i]);
                phiyx.push_back(this_phi.val);
                sphiyx.push_back(this_phi.error);
            }

            std::vector<double> rhoyy, srhoyy,phiyy, sphiyy;
            for(int i=0;i<zyyr.size();++i){
                auto this_rho = get_rho(zyyr[i],zyyi[i],zyyv[i],freq[i]);
                rhoyy.push_back(this_rho.val);
                srhoyy.push_back(this_rho.error);
                auto this_phi = get_phi(zyyr[i],zyyi[i],zyyv[i]);
                phiyy.push_back(this_phi.val);
                sphiyy.push_back(this_phi.error);
            }

            gp << "set xlabel 'Frequency (Hz)'\n";
            gp << "set y2tics -180, 60\n";
            gp << "set y2range [-180:180]\n";
            gp << "set ytics nomirror\n";
            gp << "set logscale y\n";
            gp << "set ylabel '{/Symbol r}_{app} ({/Symbol W}m)'\n";
            gp << "set y2label '{/Symbol F} ({^0})'\n";

            gp << "plot "
               <<"'-' with errorbars pt 4 lc rgb '#8E1F2F' axis x1y1,"
               <<"'-' with errorbars pt 3 lc rgb '#F0BC42' axis x1y2\n";
            gp.send1d(std::make_tuple(freq,rhoxx,srhoxx));
            gp.send1d(std::make_tuple(freq,phixx,sphixx));

            gp << "plot "
               <<"'-' with errorbars pt 4 lc rgb '#8E1F2F' axis x1y1,"
               <<"'-' with errorbars pt 3 lc rgb '#F0BC42' axis x1y2\n";
            gp.send1d(std::make_tuple(freq,rhoxy,srhoxy));
            gp.send1d(std::make_tuple(freq,phixy,sphixy));

            gp << "plot "
               <<"'-' with errorbars pt 4 lc rgb '#8E1F2F' axis x1y1,"
               <<"'-' with errorbars pt 3 lc rgb '#F0BC42' axis x1y2\n";
            gp.send1d(std::make_tuple(freq,rhoyx,srhoyx));
            gp.send1d(std::make_tuple(freq,phiyx,sphiyx));

            gp << "plot "
               <<"'-' with errorbars pt 4 lc rgb '#8E1F2F' axis x1y1,"
               <<"'-' with errorbars pt 3 lc rgb '#F0BC42' axis x1y2\n";
            gp.send1d(std::make_tuple(freq,rhoyy,srhoyy));
            gp.send1d(std::make_tuple(freq,phiyy,sphiyy));
        }

        return 0;
    }
    catch (std::exception &e){
        std::cout << "Error: " << e.what() <<"\n";
        return 1;
    }
    catch(...){
        std::cerr << "Unhandled exception. Quit.\n";
        return 2;
    }
    return 0;
}

boost::program_options::options_description parse_cmdline(int argc, char *argv[], boost::program_options::variables_map& p_vm){
    namespace po = boost::program_options;
    po::options_description generic("Generic options");
    generic.add_options()
            ("help,h", "display this message and exit.")
            ("in-path,i", po::value<std::string>(), "PATH to the input .edi file.")
            ("out-path, o", po::value<std::string>()->default_value("./"), "Where I save plots.");
    po::store(po::parse_command_line(argc, argv, generic), p_vm);
    po::notify(p_vm);
    return generic;
}

std::string getFileContents(std::ifstream& input){
    std::ostrstream sstr;
    sstr << input.rdbuf();
    return sstr.str();
}

measure get_rho(double z_real, double z_imag, double z_var, double freq){
    auto omega = 2*M_PI*freq;
    double rho = (z_real*z_real + z_imag*z_imag)/(mu0*omega);
    double sigma_z = sqrt(z_var);
    double sigma_rho = 2*sigma_z*sqrt(pow(z_real+z_imag,2))/(mu0*omega);
//            double sigma_phi = sqrt(
//            pow((z_imag/(z_real*z_real + z_imag*z_imag))*sigma_z,2) +
//            pow((-z_real/(z_real*z_real + z_imag*z_imag))*sigma_z,2)
//    );
    return {rho,sigma_rho};
}
measure get_phi(double z_real, double z_imag, double z_var){
    double phi = atan2(z_imag,z_real);
    double sigma_z = sqrt(z_var);
    double sigma_phi = sqrt(
            pow((z_imag/(z_real*z_real + z_imag*z_imag))*sigma_z,2) +
            pow((-z_real/(z_real*z_real + z_imag*z_imag))*sigma_z,2)
    );
    return {phi*180/M_PI, sigma_phi*180/M_PI};
}


#ifndef CEGO_H
#define CEGO_H

#include <memory>
#include <random>
#include <algorithm>
#include <numeric>
#include <vector>
#include <cassert>
#include <iostream>
#include <iterator>
#include <functional>
#include <tuple>
#include <map>
#include <string>
#include <sstream>
#include <stdexcept>
#include <cassert>
#include <future>       // std::packaged_task, std::future
#include <thread>       // std::thread, std::this_thread::sleep_for

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

#include "CEGO/evolvers/evolvers.hpp"

#include "CEGO/concurrentqueue.h"
#include "ThreadPool.h"

#include "nlohmann/json.hpp"

namespace CEGO{

    struct Result {
        Eigen::ArrayXd c;
        double ssq;
        Result() {};
        Result(Eigen::ArrayXd &&c, double &&ssq) : c(c), ssq(ssq) {};
    };

    /// Return Mersenne twister generator
    inline std::mt19937 get_gen(){
        std::random_device rd;
        return std::mt19937(rd());
    };

    template<class T>
    inline std::string vec2string(const std::vector<T> &v, const std::string &sep = ", ") {
        using std::to_string;
        std::stringstream ss;
        for (auto &&el : v)
            ss << to_string(el) << sep;
        return ss.str();
    }

    inline std::string vec2string(const std::vector<std::vector<double> > &v, const std::string &sep = ", ") {
        std::stringstream ss;
        for (auto &el : v)
            ss << to_string(el) << sep;
        return ss.str();
    }

    /// Generate a random population of individuals
    template<typename T>
    Population random_population(const std::vector<CEGO::Bound> bounds, std::size_t count, const CostFunction &cost_function)
    {
        auto length_ind = bounds.size();
        auto gen = get_gen();
        Population out; out.reserve(count);
        for (std::size_t i = 0; i < count; ++i) {
            std::vector<T> c; c.reserve(length_ind);
            for (std::size_t j = 0; j < length_ind; ++j) {
                auto &&bound = bounds[j];
                double d = 0; int integer = 0;
                bound.gen_uniform(gen, d, integer);

                switch (bound.m_lower.type) {
                case CEGO::numberish::types::DOUBLE:
                    c.emplace_back(d); break;
                case CEGO::numberish::types::INT:
                    c.emplace_back(integer); break;
                }
            }
            assert(c.size() == length_ind);
            out.emplace_back(pIndividual(new NumericalIndividual<T>(std::move(c), cost_function)));
        }
        return out;
    }

    /// Generate a population of individuals with the use of Latin-Hypercube sampling
    template<typename T>
    Population LHS_population(const std::vector<CEGO::Bound> bounds, std::size_t count, const CostFunction &cost_function)
    {
        // Generate the set of floating parameters in [0,1]
        Eigen::ArrayXXd population = LHS_samples(count, bounds.size());

        auto length_ind = bounds.size();
        auto gen = get_gen();
        Population out; out.reserve(count);
        for (std::size_t i = 0; i < count; ++i) {
            std::vector<T> c; c.reserve(length_ind);
            for (std::size_t j = 0; j < length_ind; ++j) {
                auto &&bound = bounds[j];

                switch (bound.m_lower.type) {
                case CEGO::numberish::types::DOUBLE:
                {
                    double w = population(i,j);
                    c.emplace_back(bound.m_lower.as_double()*w + bound.m_upper.as_double()*(1-w)); break;
                }
                case CEGO::numberish::types::INT:{
                    double w = population(i, j);
                    c.emplace_back(int(round(bound.m_lower.as_int()*w + bound.m_upper.as_int()*(1 - w)))); break;
                }
                }
            }
            assert(c.size() == length_ind);
            out.emplace_back(pIndividual(new NumericalIndividual<T>(std::move(c), cost_function)));
        }
        return out;
    }

    enum class LoggingScheme { none = 0, all, custom };
    enum class FilterOptions { accept, reject };
    enum class GenerationOptions { LHS, random };

    template<typename T>
    class Layers {

    private:
        std::vector<Population> m_layers;
        std::size_t m_generation = 0;
        moodycamel::ConcurrentQueue<Result> result_queue;

        void initialize_layers() {

            // Generate all the individuals serially(!)
            MutantVector mutants;
            for (auto i = 0; i < Nlayers; ++i) {
                auto generator = (m_generation_flag == GenerationOptions::LHS) ? LHS_population<T> : random_population<T>;
                for (auto && ind : generator(m_bounds, Npop_size, m_cost_function)) {
                    mutants.emplace_back(std::make_pair(i, std::move(ind)));
                }
            }

            // Evaluate all of the layers and individuals (in parallel, if desired)
            evaluate_mutants(mutants);

            // Recollect the individuals into layers, again in serial
            m_layers.resize(Nlayers);
            for (auto &&mut : mutants) {
                auto i = std::get<0>(mut);
                m_layers[i].emplace_back(std::move(std::get<1>(mut)));
            }

            // Sort all of the layers
            sort_all_layers();
        };
        std::unique_ptr<ThreadPool> m_pool;
        LoggingScheme m_log_scheme = LoggingScheme::none;
        std::function<FilterOptions(const Result &)> m_filter_function;
        std::unique_ptr<AbstractEvolver<T> > m_evolver; ///< The functor that is to be used to evolve a layer
        std::vector<Bound> m_bounds;
        GenerationOptions m_generation_flag = GenerationOptions::random;
        CostFunction m_cost_function;
        std::size_t m_Nelite = 2;
    public:

        bool parallel = false;
        bool print_chunk_times = false;
        std::size_t parallel_threads = 6;
        std::size_t Nind_size, Npop_size, Nlayers, age_gap;

        Layers(const std::function<double(const std::vector<T>&)> &function, std::size_t Nind_size, std::size_t Npop_size, std::size_t Nlayers, std::size_t age_gap = 5)
            : Nind_size(Nind_size), Npop_size(Npop_size), Nlayers(Nlayers), age_gap(age_gap){

            m_cost_function = [function](const CEGO::AbstractIndividual *pind) {
                const std::vector<T> &c = static_cast<const CEGO::NumericalIndividual<T>*>(pind)->get_coefficients();
                return function(c);
            };
        };

        /// Constructor into which is passed a CostFunction and information about the layers
        Layers(CostFunction &function, std::size_t Nind_size, std::size_t Npop_size, std::size_t Nlayers, std::size_t age_gap = 5)
            : Nind_size(Nind_size), Npop_size(Npop_size), Nlayers(Nlayers), age_gap(age_gap), m_cost_function(function){ };

        /// Specify the logging scheme that is to be employed
        void set_logging_scheme(LoggingScheme scheme){ m_log_scheme = scheme; }

        /// Get the logging scheme in use
        LoggingScheme get_logging_scheme() { return m_log_scheme; }

        /// Set the filtering function that should be used
        void set_filtering_function(const std::function<FilterOptions(const Result &)> &f){ m_filter_function = f; }

        /// Set the bounds on each element in the individual.  If a one-element vector, the same bounds are used for each parameter
        void set_bounds(const std::vector<Bound> &bounds){ m_bounds = bounds; };

        /// Get the bounds applied to each element in the individual
        const std::vector<Bound > & get_bounds (){ return m_bounds; };

        /// Get the flags for the evolver in JSON format
        const nlohmann::json get_evolver_flags() const {
            if (m_evolver == nullptr) {
                throw std::invalid_argument("Evolver has not been selected yet!");
            }
            return m_evolver->get_flags();
        }

        /// Set the flags for the evolver in JSON format
        const void set_evolver_flags(const nlohmann::json &flags) const {
            m_evolver->set_flags(flags);
        }

        /// Pick one of the builtin evolvers
        void set_builtin_evolver(BuiltinEvolvers e) {
            switch (e) {
            case BuiltinEvolvers::differential_evolution:
                m_evolver.reset(new DE1BinEvolver<T>); break;
            default:
                throw std::invalid_argument("Invalid builtin evolver");
            }
        }

        /// Get the cost function that is being used currently
        const CostFunction &get_cost_function() {
            return m_cost_function;
        }

        /// Set the flag to determine whether LHS or random (or other) is to be used to generate the population
        void set_generation_mode(GenerationOptions flag) {
            m_generation_flag = flag;
        }

        /// Get the flag to determine whether LHS or random is to be used to generate the population
        GenerationOptions get_generation_mode() {
            return m_generation_flag;
        }

        /** Iterate over the layers and find individuals that are too old for the given layer
         *
         * First try to see if the elderly individual dominates any individual in a layer with a higher age limit
         * If it does, replace the individual in the higher age limit layer
         */
        void graduate_elderly_individuals() {

            // Iterate backwards through the layers, starting at the N-1 layer, since the
            // last layer has an infinite age limit, and you cannot age out of the
            // highest age limit layer
            for (int ilayer = static_cast<int>(m_layers.size())-2; ilayer >= 0; --ilayer) {

                double age_threshold = static_cast<double>(age_gap*pow(2, ilayer));
                auto &this_layer = m_layers[ilayer];

                // Iterate backwards through the individuals in the layer
                for (int iind = static_cast<int>(this_layer.size())-1; iind >= static_cast<int>(m_Nelite); --iind) {
                    auto &ind = this_layer[iind];
                    if (ind->age() > age_threshold) {
                        // If higher age limit layer has an empty slot, use it directly
                        if (m_layers[ilayer+1].size() < Npop_size) {
                            m_layers[ilayer+1].push_back(std::move(ind));
                        }
                        // Or if it dominates the worst individual in the higher age limit layer, replace that one
                        // and sort the higher age limit layer
                        else if (ind->get_cost() < m_layers[ilayer + 1].back()->get_cost()) {
                            std::swap(m_layers[ilayer +1].back(), ind);
                            sort_layer(m_layers[ilayer + 1]);
                        }
                        // Say goodbye to this individual
                        this_layer.erase(this_layer.begin() + iind);
                    }
                }
            }
        }
        /// Repopulate the layer to replace removed old individuals
        void repopulate_layers() {

            // Generate the new individuals serially
            MutantVector mutants;
            for (auto i = 0; i < m_layers.size(); ++i) {
                auto &layer = m_layers[i];
                if (layer.size() < Npop_size) {
                    // How many individuals are missing?
                    auto missing_individuals_count = Npop_size - layer.size();
                    // Get the generator function
                    auto generator = (m_generation_flag == GenerationOptions::LHS) ? LHS_population<T> : random_population<T>;
                    // Then we pad out the population with new random individuals as needed (they start with an age of zero)
                    for (auto && ind : generator(m_bounds, missing_individuals_count, m_cost_function)) {
                        mutants.emplace_back(std::make_pair(i, std::move(ind)));
                    }
                }
            }

            // Evaluate the individuals we just generated
            evaluate_mutants(mutants);

            // Recollect the individuals into layers, again in serial
            for (auto &&mut : mutants) {
                auto i = std::get<0>(mut);
                m_layers[i].emplace_back(std::move(std::get<1>(mut)));
            }

            // Sort all of the layers
            sort_all_layers();
        }

        /// Evaluate a single individual, and store the values if needed
        void evaluate_ind(pIndividual &ind) {
            if (ind->needs_evaluation()){
                ind->evaluate();
                switch (m_log_scheme) {
                    case LoggingScheme::none:
                        break;
                    case LoggingScheme::custom:{
                        if (!m_filter_function) {
                            throw std::invalid_argument("filtering function has not been provided, logging options are inconsistent!");
                        }
                        Result r(ind->get_coeffs_ArrayXd(), ind->get_cost());
                        // If the filter function returns the flag "accept", then store the result
                        switch (m_filter_function(r)) {
                        case FilterOptions::accept:
                            result_queue.enqueue(std::move(r)); break;
                        case FilterOptions::reject: break;
                        }
                        break;
                    }
                    case LoggingScheme::all:{
                        result_queue.enqueue(std::move(Result(ind->get_coeffs_ArrayXd(), ind->get_cost())));
                        break;
                    }
                    default: {
                        throw std::invalid_argument("logging flag is not set; this is an error");
                    }
                }
            }
        }

        /// Evaluate all of the layers
        void evaluate_layers() {
            for (auto &layer : m_layers) {
                for (auto &ind : layer) {
                    evaluate_ind(ind); // A no-op if the individual does not need to be evaluated
                }
            }
        };

        /// Sort all of the layers
        void sort_all_layers() {
            for (auto &layer : m_layers) {
                sort_layer(layer);
            }
        };

        /// Increase the age of all individuals
        void increase_all_ages() {
            for (auto &layer : m_layers) {
                for (auto &ind : layer) {
                    ind->increase_age();
                }
            }
        }

        /// Calculate statistics of the costs of individuals in each layer
        std::vector<std::map<std::string, double> > cost_stats_each_layer() {
            std::vector<std::map<std::string, double> > out;
            for (auto &layer : m_layers) {
                Eigen::ArrayXd cost_layer(layer.size());
                for (auto i = 0; i < layer.size(); ++i) {
                    cost_layer(i) = layer[i]->get_cost();
                }
                std::map<std::string, double> this_layer_map;
                this_layer_map["max"] = cost_layer.maxCoeff();
                this_layer_map["min"] = cost_layer.minCoeff();
                // See https://en.wikipedia.org/wiki/Standard_deviation#Discrete_random_variable
                this_layer_map["stddev"] = sqrt((cost_layer - cost_layer.mean()).square().sum()/cost_layer.size());
                out.push_back(this_layer_map);
            }
            return out;
        }

        /// Sort a given layer
        void sort_layer(Population &pop) {
            auto sort_fcn = [](pIndividual &i1, pIndividual &i2) {
                return i1->get_cost() < i2->get_cost();
            };
            std::sort(pop.begin(), pop.end(), sort_fcn);
        }

        typedef std::vector<std::tuple<std::size_t, pIndividual> > MutantVector;

        void parallel_evaluator(MutantVector::iterator itstart, MutantVector::iterator itend, double &elap_sec) {

            std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
            start = std::chrono::high_resolution_clock::now();
            for (auto it = itstart; it != itend; ++it) {
                auto &ind = std::get<1>(*it);
                evaluate_ind(ind);
            }
            end = std::chrono::high_resolution_clock::now();
            elap_sec = std::chrono::duration<double>(end - start).count();
            return;
        };

        void init_thread_pool(short Nthreads){
            if (!m_pool || m_pool->GetThreads().size() != Nthreads){
                // Make a thread pool for the workers
                m_pool = std::unique_ptr<ThreadPool>(new ThreadPool(Nthreads));
            }
        }

        void evaluate_mutants(MutantVector &mutants) {
            if (!parallel || parallel_threads == 0) {
                MutantVector::iterator itstart = mutants.begin();
                MutantVector::iterator itend = mutants.end();
                double time = 0;
                parallel_evaluator(itstart, itend, time);
            }
            else {
                // Initialize the thread pool, no-op if already initialized and is the right size
                init_thread_pool(static_cast<short>(parallel_threads));
                std::vector< std::future<void> > futures;
                std::vector<std::thread> threads;
                std::size_t Lchunk = mutants.size() / parallel_threads; // Note this is an integer division, so a floor()!
                std::vector<double> times(parallel_threads);

                std::vector<std::size_t> chunksizes(parallel_threads, Lchunk);
                auto Nmax = mutants.size();
                std::size_t remainder = Nmax-Lchunk*parallel_threads;
                // Increase the first remainder chunk sizes
                for (auto i = 0; i < remainder; ++i){
                    chunksizes[i]++;
                }
                // Double-check we get the right sizes
                assert(std::accumulate(chunksizes.begin(), chunksizes.end(), static_cast<std::size_t>(0)) == mutants.size());
                std::size_t isum = 0;
                for (auto j = 0; j < parallel_threads; ++j)
                {
                    std::size_t cs = chunksizes[j];
                    auto itstart = mutants.begin() + isum;
                    auto itend = itstart + cs;
                    isum += cs;
                    double &time = times[j];

                    std::function<void(void)> f = [this, itstart, itend, &time]() {
                        auto startTime = std::chrono::high_resolution_clock::now();
                        parallel_evaluator(itstart, itend, time);
                        auto endTime = std::chrono::high_resolution_clock::now();
                        time = std::chrono::duration<double>(endTime - startTime).count();
                    };
                    m_pool->AddJob(f);
                }
                // Wait until all the threads finish...
                m_pool->WaitAll();
                // Uncomment these lines to print out the times for each chunk; ideally they are all very close
                // to each other.  Similar times means the work per thread is well divided
                if (print_chunk_times){
                    for (auto j = 0; j < parallel_threads; ++j) {
                        std::cout << j << " " << times[j] << std::endl;
                    }
                }
            }
        }

        void evolve_parallel() {
            MutantVector mutants;
            // In serial, generate the mutants; generation of the mutants
            // is very fast.  Store all mutants in a flat vector with the index of the layer
            for (auto i = 0; i < m_layers.size(); ++i) {
                for (auto &&el : m_evolver->evolve_layer(m_layers, i, get_bounds(), get_cost_function())) {
                    mutants.emplace_back(std::make_tuple(i, std::move(el)));
                }
            }

            // Evaluate all the mutants, either in parallel or serial
            evaluate_mutants(mutants);

            // Recollect the mutants into layers, again in serial
            std::vector<Population> new_layers(m_layers.size());
            for (auto &&mut : mutants) {
                auto i = std::get<0>(mut);
                new_layers[i].emplace_back(std::move(std::get<1>(mut)));
            }
            // Then determine which ones should be kept based on cost, greedily keeping
            // the one with the lower cost
            for (auto i = 0; i < m_layers.size(); ++i) {
                assert(new_layers[i].size() == m_layers[i].size());
                for (auto j = 0; j < m_layers[i].size(); ++j) {
                    if (new_layers[i][j]->get_cost() < m_layers[i][j]->get_cost()) {
                        std::swap(m_layers[i][j], new_layers[i][j]); // the other one will get thrown away
                    }
                    assert(new_layers[i][j]->get_cost() >= m_layers[i][j]->get_cost());
                }
            }
        }

        /// Evolve a given layer serially
        void evolve_layer(std::size_t i) {

            // Evolve the layer (elements are unevaluated)
            auto new_layer = m_evolver->evolve_layer(m_layers, i, get_bounds(), get_cost_function());

            for (auto j = 0; j < new_layer.size(); ++j) {
                evaluate_ind(new_layer[j]);
                double newcost = new_layer[j]->get_cost(), oldcost = m_layers[i][j]->get_cost();
                // Keep the one with the lower cost (new_layer is going to be the one that is
                // going to be used)
                if (new_layer[j]->get_cost() > m_layers[i][j]->get_cost()) {
                    // Swap because the older one is better, we are collecting values in new_layer
                    std::swap(new_layer[j], m_layers[i][j]);
                }
                assert(new_layer[j]->get_cost() <= m_layers[i][j]->get_cost());
            }

            // How many individuals are missing?
            auto missing_individuals_count = Npop_size - new_layer.size();

            // Then we pad out the population with new random individuals as needed (they start with an age of zero)
            if (missing_individuals_count > 0) {
                auto generator = (m_generation_flag == GenerationOptions::LHS) ? LHS_population<T> : random_population<T>;
                Population random_inds = generator(m_bounds, missing_individuals_count, m_cost_function);
                std::move(random_inds.begin(), random_inds.end(), std::back_inserter(new_layer));
            }

            // And move back to the layer
            std::swap(m_layers[i], new_layer);

            assert(m_layers[i].size() == Npop_size);
        }

        /// Carry out the steps for one generation
        void do_generation() {
            if (m_evolver == nullptr) {
                throw std::invalid_argument("Evolver has not been selected");
            }
            if (m_layers.size() == 0) {
                initialize_layers();
            }
            else if (m_generation % age_gap == 0 && m_layers.size() > 1) {

                // ========
                // Layer #1
                // ========

                // push into the next-from-bottom layer any individuals that dominate an individual in the higher layer
                // --
                // Join them all together into one population
                Population pop;
                // Move them into the population
                std::move(m_layers[1].begin(), m_layers[1].end(), std::back_inserter(pop));
                std::move(m_layers[0].begin(), m_layers[0].end(), std::back_inserter(pop));
                // Sort them in terms of increasing cost
                sort_layer(pop);
                // Remove the ones that are no longer needed;
                // Keep indices [0,Npop_size-1] inclusive
                for (auto i = pop.size()-1; i >= Npop_size; --i){
                    pop.erase(pop.begin()+i);
                }
                // Keep the best ones
                m_layers[1] = std::move(pop);

                // ========
                // Layer #0
                // ========

                // Generate new individuals in serial (fast)
                MutantVector mutants;
                auto generator = (m_generation_flag == GenerationOptions::LHS) ? LHS_population<T> : random_population<T>;
                for (auto && ind : generator(m_bounds, Npop_size, m_cost_function)) {
                    mutants.emplace_back(std::make_pair(0,std::move(ind)));
                }
                // Evaluate the individuals in parallel
                evaluate_mutants(mutants);

                // Put them back into the 0-th layer
                m_layers[0].clear();
                for (auto &&mut : mutants) {
                    auto i = std::get<0>(mut);
                    m_layers[i].emplace_back(std::move(std::get<1>(mut)));
                }
            }
            graduate_elderly_individuals();
            repopulate_layers();
            evaluate_layers();
            if (parallel){
                evolve_parallel();
            }
            else{
                for (int i = static_cast<int>(m_layers.size()) - 1; i >= 0; i--) {
                    evolve_layer(i);
                }
            }
            evaluate_layers();
            increase_all_ages();
            sort_all_layers();
            m_generation++;
        }
        // Get the best individuals from each layer, starting with layer 0
        std::vector<std::tuple<double, const std::vector<T> > > get_best_per_layer(){
            std::vector<std::tuple<double, const std::vector<T> > > out;
            for (const auto &layer : m_layers) {
                const auto c = static_cast<const NumericalIndividual<T>*>(layer[0].get())->get_coefficients();
                out.emplace_back(std::make_tuple(layer[0]->get_cost(), c));
            }
            return out;
        };
        // Get the layer with the individual with the lowest (best) cost
        std::tuple<double, const std::vector<T> > get_best() {
            auto B = get_best_per_layer();
            // The best individual might not be in the highest layer, let's find the best one
            return *std::min_element(B.begin(), B.end(), [](decltype(B.back()) &b1, decltype(B.back()) &b2) { return std::get<0>(b1) < std::get<0>(b2); });
        };
        /** Return a string with some diagnostic information for the best individual in the population
         * \sa get_best
         * \sa get_best_per_layer
         */
        std::string print_diagnostics() {
            auto best = get_best();
            double best_cost; std::vector<T> c;
            std::tie(best_cost, c) = best;
            std::stringstream ss;
            ss << "i: " << static_cast<int>(m_generation - 1) << " best: " << best_cost << " c: " << vec2string(c) << " queue: " << result_queue.size_approx();
            return ss.str();
        }

        /// Get the results that have been logged during the course of this optimization
        std::vector<Result> get_results() {
            std::vector<Result> results(result_queue.size_approx());
            auto Nels = result_queue.try_dequeue_bulk(results.begin(), results.size());
            return results;
        }
    };
    struct ALPSInputValues {
        std::vector<Bound> bounds; ///< The vector of bounds on the variables
        double VTR; ///< Value to reach (terminates on reaching this cost value)
        CostFunction f; ///< The cost function to be minimized
        bool parallel = false; ///< If true, evaluate each layer in a separate thread
        std::size_t max_gen = 1000; ///< Maximum number of generations that are allowed
        std::size_t NP = 40; ///< The number of individuals in a population (per layer)
        std::size_t Nlayer = 1; ///< The number of layers
        std::size_t age_gap = 5; ///< The number of generations between restarting the bottom layer
        bool disp = false; ///< If true, display diagnostics as you go to standard out
    };
    struct ALPSReturnValues {
        double fval; ///< The function value at termination
        double elapsed_sec; ///< The number of seconds to conduct the entire optimization
        std::string termination_reason; ///< Why the optimization stopped
    };

    template<typename T>
    ALPSReturnValues ALPS(ALPSInputValues &in) {
        if (in.bounds.empty()){ throw std::invalid_argument("bounds variable must be provided"); }
        //if (in.VTR) { throw std::invalid_argument("VTR variable must be provided"); }
        if (!in.f) { throw std::invalid_argument("Cost function f must be provided"); }
        ALPSReturnValues out;
        auto D = in.bounds.size();
        auto layers = Layers<T>(in.f, D, in.NP, in.Nlayer, in.age_gap);
        layers.set_bounds(in.bounds);
        layers.parallel = in.parallel;
        layers.set_builtin_evolver(BuiltinEvolvers::differential_evolution);
        auto startTime = std::chrono::system_clock::now();
        for (auto i = 0; i < in.max_gen; ++i) {
            layers.do_generation();
            if (in.disp){ auto diag = layers.print_diagnostics(); std::cout << diag << std::endl; }
            auto best_layer = layers.get_best();
            if (std::get<0>(best_layer) < in.VTR) {
                out.termination_reason = "Reached VTR";
                break;
            }
            if (i == in.max_gen-1) { out.termination_reason = "Reached max # of generations"; }
        }

        auto endTime = std::chrono::system_clock::now();
        out.elapsed_sec = std::chrono::duration<double>(endTime - startTime).count();
        out.fval = std::get<0>(layers.get_best());
        return out;
    }

} /* namespace CEGO */
#endif


/**
 * @file QuadraticConstraint.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the MIT License.
 * @date 2021
 */

#include <cmath>
#include <limits>
#include <string>

#include <Eigen/Dense>

#include <ScsEigen/Logger.h>
#include <ScsEigen/Math.h>
#include <ScsEigen/QuadraticConstraint.h>

using namespace ScsEigen;

QuadraticConstraint::QuadraticConstraint()
{
    this->m_lowerBound = ScsEigen::Vector1d::Constant(
        -std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity());
}

QuadraticConstraint::QuadraticConstraint(const Eigen::Ref<const Eigen::MatrixXd>& Q,
                                         const Eigen::Ref<const Eigen::MatrixXd>& b,
                                         double upperBound)
    : Constraint((Q.rows() == Q.cols() && Q.rows() == b.rows()) ? Q.rows() : 0,
                 "Quadratic constraint")
{
    if (Q.rows() != Q.cols() || Q.rows() != b.rows())
    {

        log()->error("[QuadraticConstraint::QuadraticConstraint] Q matrix must be square and the "
                     "size of b "
                     "should be coherent with Q");
        assert(false);
    } else
    {
        m_Q = (Q + Q.transpose()) / 2;
        m_b = b;
    }

    // the only admissible lower bound is
    // -std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity()
    this->m_lowerBound = ScsEigen::Vector1d::Constant(
        -std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity());

    // set the upperbound
    this->setUpperBound(ScsEigen::Vector1d::Constant(upperBound));
}

bool QuadraticConstraint::setQ(const Eigen::Ref<const Eigen::MatrixXd>& Q)
{
    if (m_Q.size() != 0)
    {
        if (Q.size() != m_Q.size())
        {
            log()->error("[QuadraticConstraint::setQ] The size of the matrix 'Q' cannot change.");
            return false;
        }
    } else if (Q.rows() != Q.cols())
    {
        log()->error("[QuadraticConstraint::QuadraticConstraint] Q matrix must be square.");
        return false;
    } else if (!this->setNumberOfVariables(Q.rows()))
    {
        log()->error("[QuadraticConstraint::setQ] Unable to set the number of variables.");
        return false;
    }

    m_Q = (Q + Q.transpose()) / 2;
    return true;
}

bool QuadraticConstraint::setB(const Eigen::Ref<const Eigen::VectorXd>& b)
{
    if (m_b.size() != 0)
    {
        if (b.size() != m_b.size())
        {
            log()->error("[QuadraticConstraint::setB] The size of the vector 'b' cannot change.");
            return false;
        }
    } else if (!this->setNumberOfVariables(b.size()))
    {
        log()->error("[QuadraticConstraint::setB] Unable to set the number of variables.");
        return false;
    }

    m_b = b;
    return true;
}

bool QuadraticConstraint::setLowerBound(const Eigen::Ref<const Eigen::VectorXd>& lowerBound)
{
    if (lowerBound.size() != 1)
    {
        log()->error("[QuadraticConstraint::setLowerBound] The lower bound should be a scalar.");
        return false;
    }

    if (lowerBound[0] > 0 || !std::isinf(lowerBound[0]))
    {
        log()->error("[QuadraticConstraint::setLowerBound] The only admissible lowerBound is "
                     "-std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity()");
        return false;
    }

    // the only admissible lowerBound is
    // -std::numeric_limits<ScsEigen::Vector1d::Scalar>::infinity() and it has been already set in
    // the constructor
    return true;
}

Eigen::Ref<const Eigen::VectorXd> QuadraticConstraint::getB() const
{
    return m_b;
}

Eigen::Ref<const Eigen::MatrixXd> QuadraticConstraint::getQ() const
{
    return m_Q;
}


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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
//==============================================================================

#include <nt2/table.hpp>
#include <nt2/include/functions/any.hpp>
#include <nt2/include/functions/seladd.hpp>
#include <nt2/include/functions/plus.hpp>
#include <nt2/include/functions/minus.hpp>
#include <nt2/include/functions/times.hpp>
#include <nt2/include/functions/is_less.hpp>
#include <nt2/include/functions/linspace.hpp>
#include <nt2/include/functions/expand_to.hpp>
#include <nt2/include/functions/colvect.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/arrayfun.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <vector>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/setup/constant.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

namespace mandelbrot
{

  struct step
  {
    template<class Sig> struct result;
    template<class This, class A0, class A1>
    struct result<This(A0,A1)>
    {
      typedef typename boost::dispatch::meta::
              as_integer<typename boost::dispatch::meta::strip<A0>::type
                        >::type type;
    };

    step(std::size_t const& n) : max_iter_(n) {}

    template<class T>
    typename result<step(T,T)>::type operator()(T const& a, T const& b) const
    {
      typedef typename result<step(T const&, T const&)>::type iter_type;
      typedef typename boost::simd::meta::scalar_of<T>::type s_type;
      iter_type iter = nt2::Zero<iter_type>();
      iter_type const o = nt2::One<iter_type>();
      T x = nt2::Zero<T>();
      T y = nt2::Zero<T>();
      T x2,y2,xy,m2;
      typename boost::simd::meta::as_logical<T>::type mask;
      std::size_t i = 0;
      bool flag;
      do
      {
        x2 = x*x;
        y2 = y*y;
        xy = s_type(2)  *x*y;
        x = x2 - y2 + a;
        y = xy + b;
        m2 = x2 + y2;
        mask= m2<s_type(4);
        iter = nt2::seladd(mask, iter, o);
        flag = nt2::any(mask);
        i++;
      }while(flag && i < 256);

      return iter;
    }

    std::size_t max_iter_;
  };
}

template<typename T> struct mandelbrot_nt2
{
  typedef T value_type;
  template<typename Setup>
  mandelbrot_nt2(Setup const& s)
                    :  h_(boost::fusion::at_c<0>(s))
                    ,  w_(boost::fusion::at_c<1>(s))
                    ,  a0_(boost::fusion::at_c<2>(s))
                    ,  a1_(boost::fusion::at_c<3>(s))
                    ,  b0_(boost::fusion::at_c<4>(s))
                    ,  b1_(boost::fusion::at_c<5>(s))
                    ,  max_iter_(boost::fusion::at_c<6>(s))
                    ,  size_(h_*w_)
                    ,  julia(max_iter_)
  {
    A.resize(nt2::of_size(h_,w_));
    B.resize(nt2::of_size(h_,w_));
    C.resize(nt2::of_size(h_,w_));

    A=nt2::expand_to(nt2::linspace(a0_,a1_,h_),h_,w_);
    B=nt2::expand_to(nt2::colvect(nt2::linspace(b0_,b1_,w_)),h_,w_);
  }

  void operator()()
  {
    C = arrayfun(julia, A, B);
  }

  friend std::ostream& operator<<(std::ostream& os, mandelbrot_nt2<T> const& p)
  {
    return os << "(" << p.h_ << " x " << p.w_ << ")";
  }

  std::size_t size() const { return size_; }

  private:
    std::size_t h_, w_;
    value_type a0_, a1_, b0_, b1_;
    std::size_t max_iter_, size_, step_size_, aligned_sz, it;
    mandelbrot::step julia;
    nt2::table<value_type> A, B;
    nt2::table<int> C;
};

NT2_REGISTER_BENCHMARK_TPL( mandelbrot_nt2, (float) )
{

  std::size_t hmin = args("hmin", 100);
  std::size_t hmax = args("hmax", 1600);
  std::size_t hstep = args("hstep", 2);
  std::size_t wmin = args("wmin", 100);
  std::size_t wmax = args("wmax",1600);
  std::size_t wstep = args("wstep", 2);
  T xmin = args("xmin", -1.5);
  T xmax = args("xmax", 1.5);
  T ymin = args("ymin", -1.5);
  T ymax = args("ymax", 1.5);
  T max_iter = args("max_iter", 256);

  run_during_with< mandelbrot_nt2<float> > ( 1.
                                          , and_( geometric(hmin,hmax,hstep)
                                                , geometric(wmin,wmax,wstep)
                                                , constant(xmin)
                                                , constant(xmax)
                                                , constant(ymin)
                                                , constant(ymax)
                                                , constant(max_iter)
                                                )
                                          , cycles_per_element<stats::median_>()
                                          );
}


//---------------------------------------------------------------------------------------------------------------------
//  Vertical Engineering Solutions
//---------------------------------------------------------------------------------------------------------------------
//
//  Copyright 2020 Vertical Engineering Solutions  - All Rights Reserved
//
//  Unauthorized copying of this file, via any medium is strictly prohibited Proprietary and confidential.
//
//  All information contained herein is, and remains the property of Vertical Engineering Solutions.  The
//  intellectual and technical concepts contained herein are proprietary to Vertical Engineering Solutions
//  and its suppliers and may be covered by UE and Foreign Patents, patents in process, and are protected
//  by trade secret or copyright law. Dissemination of this information or reproduction of this material is
//  strictly forbidden unless prior written permission is obtained from Adobe Systems Incorporated.
//
//---------------------------------------------------------------------------------------------------------------------
//
//  Maintainer: pramon@vengineerings.com
//
//---------------------------------------------------------------------------------------------------------------------

#include <QApplication>
#include <QVector>
#include "qcustomplot.h"

#include <pidpp/PID.h>

#include<iostream>

#include <boost/numeric/odeint.hpp>
using namespace boost::numeric::odeint;

typedef std::vector<double> state_type;
/* The rhs of x' = f(x) */
class FirstOrderSystem {
    public:
        FirstOrderSystem( double _c ) : c_(_c) { }

        void operator() ( const state_type &x , state_type &dxdt, const double _t ) {
            dxdt[0] = -1/c_ * x[0] + x[1];
        }

    private:
        double c_;
};

class SecondOrderSystem {
    public:
        SecondOrderSystem( double _a, double _b ) : a_(_a), b_(_b) { }

        void setU(double _u){
            u_ = _u;
        }

        void operator() ( const state_type &x , state_type &dxdt, const double _t ) {
            dxdt[0] = x[1];
            dxdt[1] = u_ - b_*x[0] -a_* x[1];
        }

    private:
        double a_, b_;
        double u_;
};

int main(int _argc, char** _argv){
    QApplication a(_argc, _argv);

    //-----------------------------------------------------------------------------------------------------------------
    // Simulation
    //-----------------------------------------------------------------------------------------------------------------

    float kp = 4.0;
    float ki = 5.0;
    float kd = 0.0;

    //-----------------------------------------------------------------------------------------------------------------
    // Free run
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateFree = {0, 0};
    std::vector<double> valuesFree;
    std::vector<double> stepFree;
    std::vector<double> timeFree;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateFree , 0.0 , 10.0 , 0.01 );

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                system.setU(1);
                stepFree.push_back(1);
            }else{
                stepFree.push_back(0);
            }

            stepper.do_step( system , stateFree , t , dt );
            timeFree.push_back(t);
            valuesFree.push_back(stateFree[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PID no windup
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateNoWindup = {0, 0};
    std::vector<double> valuesNoWindup;
    std::vector<double> stepNoWindup;
    std::vector<double> timeNoWindup;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateNoWindup , 0.0 , 10.0 , 0.01 );
        pidpp::PID pid(kp, ki, kd,-2,2);
        pid.reference(1);

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                stepNoWindup.push_back(1);
                system.setU(pid.update(stateNoWindup[0], dt));
            }else{
                stepNoWindup.push_back(0);
            }

            stepper.do_step( system , stateNoWindup , t , dt );
            timeNoWindup.push_back(t);
            valuesNoWindup.push_back(stateNoWindup[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PID windup sat
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateWindupSat = {0, 0};
    std::vector<double> valuesWindupSat;
    std::vector<double> stepWindupSat;
    std::vector<double> timeWindupSat;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateWindupSat , 0.0 , 10.0 , 0.01 );
        pidpp::PID pid(kp, ki, kd,-2,2);
        pid.reference(1);
        pid.setAntiWindup(pidpp::PID::AntiWindupMethod::Saturation, {-0.2, 0.2});

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                stepWindupSat.push_back(1);
                system.setU(pid.update(stateWindupSat[0], dt));
            }else{
                stepWindupSat.push_back(0);
            }

            stepper.do_step( system , stateWindupSat , t , dt );
            timeWindupSat.push_back(t);
            valuesWindupSat.push_back(stateWindupSat[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PID windup Back calculation
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateBackCalc = {0, 0};
    std::vector<double> valuesBackCalc;
    std::vector<double> stepBackCalc;
    std::vector<double> timeBackCalc;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateBackCalc , 0.0 , 10.0 , 0.01 );
        pidpp::PID pid(kp, ki, kd,-2,2);
        pid.reference(1);
        pid.setAntiWindup(pidpp::PID::AntiWindupMethod::BackCalculation, {1.2});

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                stepBackCalc.push_back(1);
                system.setU(pid.update(stateBackCalc[0], dt));
            }else{
                stepBackCalc.push_back(0);
            }

            stepper.do_step( system , stateBackCalc , t , dt );
            timeBackCalc.push_back(t);
            valuesBackCalc.push_back(stateBackCalc[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PID windup clamp
    //-----------------------------------------------------------------------------------------------------------------
    std::vector<double> stateClamp = {0, 0};
    std::vector<double> valuesClamp;
    std::vector<double> stepClamp;
    std::vector<double> timeClamp;

    {
        SecondOrderSystem system(1,1);
        runge_kutta4< state_type > stepper;
        system.setU(0);
        integrate_const( stepper ,  system, stateClamp , 0.0 , 10.0 , 0.01 );
        pidpp::PID pid(kp, ki, kd,-2,2);
        pid.reference(1);
        pid.setAntiWindup(pidpp::PID::AntiWindupMethod::Clamping, {1});

        const double dt = 0.01;
        for( double t=0.0 ; t<10.0 ; t+= dt ){
            if(t >1){
                stepClamp.push_back(1);
                system.setU(pid.update(stateClamp[0], dt));
            }else{
                stepClamp.push_back(0);
            }

            stepper.do_step( system , stateClamp , t , dt );
            timeClamp.push_back(t);
            valuesClamp.push_back(stateClamp[0]);
        }
    }

    //-----------------------------------------------------------------------------------------------------------------
    // PLOT
    //-----------------------------------------------------------------------------------------------------------------
    QCustomPlot plot;
    plot.setInteraction(QCP::iSelectPlottables, true);
    plot.legend->setVisible(true);
    plot.legend->setBrush(QBrush(QColor(255,255,255,230)));
    plot.setMinimumWidth(800);
    plot.setMinimumHeight(500);
    // Step
    QPen graphPen;
    graphPen.setColor(QColor(255,0,0));
    graphPen.setWidthF(4);
    plot.addGraph()->setPen(graphPen);
    plot.graph(0)->setData(QVector<double>::fromStdVector(timeFree), QVector<double>::fromStdVector(stepFree));
    plot.graph(0)->setName("Target Step");
    // Open step response
    graphPen.setColor(QColor(0,255,0));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(1)->setData(QVector<double>::fromStdVector(timeFree), QVector<double>::fromStdVector(valuesFree));
    plot.graph(1)->setName("Free form");
    // No wind up PID
    graphPen.setColor(QColor(0,0,255));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(2)->setData(QVector<double>::fromStdVector(timeNoWindup), QVector<double>::fromStdVector(valuesNoWindup));
    plot.graph(2)->setName("PID no antiwindup");

    // wind up sat PID
    graphPen.setColor(QColor(0,255,255));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(3)->setData(QVector<double>::fromStdVector(timeWindupSat), QVector<double>::fromStdVector(valuesWindupSat));
    plot.graph(3)->setName("with saturation");

    // wind up back calculation
    graphPen.setColor(QColor(255,0,255));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(4)->setData(QVector<double>::fromStdVector(timeBackCalc), QVector<double>::fromStdVector(valuesBackCalc));
    plot.graph(4)->setName("Back Calculation");

    // wind up Clamp
    graphPen.setColor(QColor(255,255,0));
    graphPen.setWidthF(2);
    plot.addGraph()->setPen(graphPen);
    plot.graph(5)->setData(QVector<double>::fromStdVector(timeClamp), QVector<double>::fromStdVector(valuesClamp));
    plot.graph(5)->setName("Clamping");

    // give the axes some labels:
    plot.xAxis->setLabel("t (s)");
    plot.yAxis->setLabel("value (u)");
    // set axes ranges, so we see all data:
    plot.xAxis->setRange(0, 10);
    plot.yAxis->setRange(   -0.5, 2);
    plot.replot();
    plot.show();

    return a.exec();

}