/*
 *  model_gmm.cpp
 *
 *  Created by Ania M. Kedzierska on 11/11/11.
 *  Copyright 2011 Politecnic University of Catalonia, Center for Genomic Regulation.  This is program can be redistributed, modified or else as given by the terms of the GNU General Public License.
 *
 */

#include "model_gmm.h"
#include "model_ssm.h"
#include "random.h"
#include "parameters.h"
#include <cstdlib>
#include <iostream>
#include <cmath>
#include <algorithm>

#include <boost/math/special_functions/erf.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "expm.hpp"

#include "miscelania.h"

#include <stdexcept>

//////////////////////////////////////////////////////////////////
// This file implements functions specific to the GMM model
//////////////////////////////////////////////////////////////////

// Fills in a GMM row
void GMM_row(double a, double b, double c, double d, long r, TMatrix &tm) {
  tm[r][0] = a;
  tm[r][1] = b;
  tm[r][2] = c;
  tm[r][3] = d;
}

long GMM_matrix_structure(long i, long j) {
  return 4*i + j;
}

long GMM_root_structure(long i) {
  return i;
}

Permutation GMM_perm(long a, long b, long c, long d) {
  Permutation p;
  p.resize(4);
  p[0] = a; p[1] = b; p[2] = c; p[3] = d;
  return p;
}

void GMM_list_permutations(std::list<Permutation> &L) {
  L.clear();
  L.push_back(GMM_perm(0,1,2,3));
  L.push_back(GMM_perm(0,1,3,2));
  L.push_back(GMM_perm(0,2,1,3));
  L.push_back(GMM_perm(0,2,3,1));
  L.push_back(GMM_perm(0,3,1,2));
  L.push_back(GMM_perm(0,3,2,1));

  L.push_back(GMM_perm(1,0,2,3));
  L.push_back(GMM_perm(1,0,3,2));
  L.push_back(GMM_perm(1,2,0,3));
  L.push_back(GMM_perm(1,2,3,0));
  L.push_back(GMM_perm(1,3,0,2));
  L.push_back(GMM_perm(1,3,2,0));

  L.push_back(GMM_perm(2,0,1,3));
  L.push_back(GMM_perm(2,0,3,1));
  L.push_back(GMM_perm(2,1,0,3));
  L.push_back(GMM_perm(2,1,3,0));
  L.push_back(GMM_perm(2,3,0,1));
  L.push_back(GMM_perm(2,3,1,0));

  L.push_back(GMM_perm(3,0,1,2));
  L.push_back(GMM_perm(3,0,2,1));
  L.push_back(GMM_perm(3,1,0,2));
  L.push_back(GMM_perm(3,1,2,0));
  L.push_back(GMM_perm(3,2,0,1));
  L.push_back(GMM_perm(3,2,1,0));
}

// Computes the mle for the transition matrix of a given edge from the
// the marginalization of the counts on that edge.
// N is a matrix representing the counts on that edge
// tm is the matrix where the mle is stored. tm must be a stochastic matrix
void GMM_mle_edge(TMatrix &N, TMatrix &tm) {
    double s[4];
    long i,j;

    for (i=0; i < 4; i++) {
      s[i] = 0;
      for(j=0; j < 4; j++) {
        s[i] = s[i] + N[i][j];
      }
    }

    tm[0][0] = N[0][0] / s[0];
    tm[0][1] = N[0][1] / s[0];
    tm[0][2] = N[0][2] / s[0];
    tm[0][3] = N[0][3] / s[0];

    tm[1][0] = N[1][0] / s[1];
    tm[1][1] = N[1][1] / s[1];
    tm[1][2] = N[1][2] / s[1];
    tm[1][3] = N[1][3] / s[1];

    tm[2][0] = N[2][0] / s[2];
    tm[2][1] = N[2][1] / s[2];
    tm[2][2] = N[2][2] / s[2];
    tm[2][3] = N[2][3] / s[2];

    tm[3][0] = N[3][0] / s[3];
    tm[3][1] = N[3][1] / s[3];
    tm[3][2] = N[3][2] / s[3];
    tm[3][3] = N[3][3] / s[3];
}

// Computes the mle for the root distribution of a given edge from the
// marginalization of the counts to the root node.
// s is a vector representing the counts on the root
// r is a vector where the root distribution is stored.
void GMM_mle_root(Root &s, Root &r) {
  long i;
  double sum;
  sum = s[0] + s[1] + s[2] + s[3];

  for (i=0; i < 4; i++) {
    r[i] = s[i] / sum;
  }
}

// Uniform random stochastic vector of length 3.
void GMM_random_stochastic_vector3(std::vector<double> &v) {
  v[1] = uniform_real(0,1);
  v[2] = uniform_real(0,1);

  if (v[1] + v[2] > 1) {
    v[1] = 1 - v[1];
    v[2] = 1 - v[2];
  }

  v[0] = 1 - v[1] - v[2];
}

// Uniform random stochastic vector of length 4.
void GMM_random_stochastic_vector4(std::vector<double> &v) {
  double s;

  // A uniform point with x1 + ... + x3 <= 1 is generated
  // by producing a point with x1 + ... + x3 = 1 and scaling
  // by s, s is distributed according to f(x) = 3x^2 (cdf is F(x)=x^3).
  GMM_random_stochastic_vector3(v);

  s = uniform_real(0,1);
  s = s*s*s;

  for (long i=0; i < 3; i++) {
    v[i] = s*v[i];
  }
  v[3] = 1 - s;
}

// Produces a random GMM root distribution.
void GMM_random_root(Root &r) {
  GMM_random_stochastic_vector4(r);
}

// Produces a random GMM transition matrix
void GMM_random_edge(TMatrix &tm) {
  GMM_random_stochastic_vector4(tm[0]);
  GMM_random_stochastic_vector4(tm[1]);
  GMM_random_stochastic_vector4(tm[2]);
  GMM_random_stochastic_vector4(tm[3]);
}

void GMM_random_rate_matrix(double tr, TMatrix &Q) {
  std::vector<double> v;
  v.resize(Q.size());

  // Fill in the diagonal
  GMM_random_stochastic_vector4(v);
  for(long i=0; i < 4; i++) {
    Q[i][i] = tr*v[i];
  }

  // Complete the rows.
  long k;
  for(long i=0; i < 4; i++) {
    GMM_random_stochastic_vector3(v);
    k=0;
    for(long j=0; j < 4; j++) {
      if (j == i) continue;
      Q[i][j] = -Q[i][i] * v[k];
      k++;
    }
  }
}

// translating transition matrix to 'our' format
void GMM_matrix_exponential(TMatrix &Q, TMatrix &A) {
  using namespace boost::numeric;

  ublas::matrix<double> QQ(4,4);
  ublas::matrix<double> AA(4,4);

  for(unsigned long i = 0; i < Q.size(); i++) {
    for(unsigned long j = 0; j < Q.size(); j++) {
      QQ(i,j) = Q[i][j];
    }
  }

  AA = expm_pad(QQ);

  for(unsigned long i = 0; i < Q.size(); i++) {
    for(unsigned long j = 0; j < Q.size(); j++) {
      A[i][j] = AA(i,j);
    }
  }
}

// Random GMM transition matrix of a given branch length
void GMM_random_edge_length(double len, TMatrix &tm) {
  double t;
  TMatrix Q, A, B;
  Q.resize(tm.size());
  A.resize(tm.size());
  B.resize(tm.size());
  for(unsigned long l=0; l < tm.size(); l++) {
    Q[l].resize(tm.size());
    A[l].resize(tm.size());
    B[l].resize(tm.size());
  }

  t = uniform_real(-4*len, 0);
  GMM_random_rate_matrix(t, Q);
  GMM_matrix_exponential(Q, A);

  SSM_random_edge_length(len + t/4, B);

  for(long i = 0; i < 4; i++) {
    for(long j=0; j < 4; j++) {
      tm[i][j] = 0;
      for(long k=0; k < 4; k++) {
        tm[i][j] = tm[i][j] + B[i][k]*A[k][j];
      }
    }
  }
}

// Generates a biologically meaningful GMM matrix (Chang's DLC condition)
void GMM_random_edge_bio_length(double len, TMatrix &tm) {
  TMatrix tmaux;
  long j, k;
  Permutation p;
  p.resize(4);

  tmaux.resize(4);
  for(j=0; j < 4; j++) {
    tmaux[j].resize(4);
  }

  // Loop until there is a permutation that puts it into the DLC form.
  long timeout=0;
  do {
    GMM_random_edge_length(len, tmaux);

    p[0] = max_in_col(tmaux, 0);
    p[1] = max_in_col(tmaux, 1);
    p[2] = max_in_col(tmaux, 2);
    p[3] = max_in_col(tmaux, 3);

    timeout++;
  } while(!(is_permutation(p) && permutation_sign(p) == 1) && (timeout < 1000));

  if (timeout >= 1000) {
    throw std::length_error("ERROR: In sampling for GMM model. Can't generate DLC matrix of length " );
  }

  for(j=0; j < 4; j++) {
    for(k=0; k < 4; k++) {
      tm[j][k] = tmaux[p[j]][k];
    }
  }
}


#include "stdafx.h"

#include "problem.hpp"

#include <fstream>
#include <regex>
#include <vector>
#include <boost/icl/interval_set.hpp>

struct advent_2016_20 : problem
{
	advent_2016_20() noexcept : problem(2016, 20) {
	}

protected:
	using interval_type = boost::icl::closed_interval<size_t>::type;
	std::vector<interval_type> intervals;

	void prepare_input(std::ifstream& fin) override {
		std::regex pattern(R"(([[:digit:]]+)-([[:digit:]]+))");
		for(std::string line; std::getline(fin, line);) {
			std::smatch m;
			std::regex_search(line, m, pattern);
			intervals.push_back({ std::stoull(m[1].str()), std::stoull(m[2].str()) });
		}
	}

	using set_type = boost::icl::interval_set<std::size_t, std::less, interval_type>;
	set_type firewall;

	void precompute() override {
		for(const interval_type& i : intervals) {
			firewall.add(i);
		}
	}

	std::string part_1() override {
		return std::to_string(firewall.begin()->upper() + 1);
	}

	std::string part_2() override {
		std::size_t available = 0x1'0000'0000;
		for(const interval_type& i : firewall) {
			available -= i.upper() - i.lower() + 1;
		}
		return std::to_string(available);
	}
};

REGISTER_SOLVER(2016, 20);


/*
 * XSENSMODEL.hpp
 *
 *  Created on: 04.09.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_MODEL_LAMBDAPATHWITHVELOCITYMODEL_HPP_
#define ESTIMATORS_MODEL_LAMBDAPATHWITHVELOCITYMODEL_HPP_
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "../../Plugins/imu_plugin.hpp"
#include <Eigen_Utils.hpp>
#include "State.hpp"
#include "../StateBoxes/LambdaPathBoxNoAcceleration.hpp"
#include "../StateBoxes/OrientationSpeedBox.hpp"
#include "../StateBoxes/CartesianBox.hpp"
namespace zavi
::estimator::model {
	typedef State<state_boxes::CartesianBox,state_boxes::LambdaPathBoxNoAcceleration, state_boxes::OrientationSpeedBox> LAMBDA_PATH_WITH_VELOCITY_MODEL_TYPE;
	/**
	 * Model for 1D movement with forward velocity prior
	 */
	class LambdaPathWithVelocityModel: public LAMBDA_PATH_WITH_VELOCITY_MODEL_TYPE {
		//static_assert(std::is_base_of<StateEstimator,state_estimator_type>,"NoOrientationModel template fail state_estimator_type is no base class of StateEstimator");

	public:
		static constexpr int orientation_start=11;
		static constexpr int orientation_speed_start=20;
		static constexpr int velocity_start=3;
		static constexpr int lambda_start=9;
		static constexpr int lambda_velocity_start=10;
		static constexpr int acceleration_start=6;
		LambdaPathWithVelocityModel(std::shared_ptr<plugin::SensorPlugin> sensor): LAMBDA_PATH_WITH_VELOCITY_MODEL_TYPE(sensor) {
			plugin::IMU_Plugin * imu=static_cast<plugin::IMU_Plugin *>(sensor.get());
			Eigen::Matrix<double,input_size,input_size> stiffnes;
			stiffnes=stiffnes.Zero();
			stiffnes.block<3,3>(0,0)=imu->getAccelerationCov();
			input_measurement::stiffnes=stiffnes.diagonal().cwiseInverse().cwiseSqrt().asDiagonal();
		}

		virtual ~LambdaPathWithVelocityModel() {

		}
		/**
		 * Function to integrate an input measurement
		 * @param state the current state
		 * @param alignment the calibration matrix from imu to body frame
		 * @param prior an arbitrary prior that can be passed to the function
		 * @return the estimated input at state
		 */
		struct input_measurement: public base_measurement<input_size> {
			static inline Eigen::Matrix<double,input_size,input_size> stiffnes;
			template<typename T>
			inline INPUT_T<T> operator()(const OUTER_T<T> &state,const Eigen::Matrix<T,4,4> & alignment, void * prior) {
				INPUT_T<T> measurement = INPUT_T < T > ::Zero();
				measurement.template block<3,1>(0, 0) =acceleration_measurement(state,alignment, prior);
				return measurement;
			}
		};

		/**
		 * Measurement to set the lambda velocity to the norm of the cartesian velocity
		 * @param state the current state
		 * @param alignment the calibration matrix from imu to body frame
		 * @param prior an arbitrary prior that can be passed to the function
		 * @return The difference between the cartesian velocity norm and the lambda velocity
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 1, 1> dot_lambda_measurement(const OUTER_T<T> & state,const Eigen::Matrix<T,4,4> & alignment,
				void * prior) {
			Eigen::Matrix<T, 1, 1> measurement = Eigen::Matrix<T, 1, 1> ::Zero();
			measurement(0, 0) =getVelocity(state).norm()-getLambdaVelocity(state);
			return measurement;
		}

		/**
		 * Function to integrate an acceleration measurement
		 * @param state the current state
		 * @param alignment the calibration matrix from imu to body frame
		 * @param prior an arbitrary prior that can be passed to the function
		 * @return the estimated acceleration at state
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> acceleration_measurement(const OUTER_T<T> & state,const Eigen::Matrix<T,4,4> & alignment,
				void * prior) {
			Eigen::Matrix<T,3,3> Q_i_b=alignment.template block<3,3>(0,0);
			Eigen::Matrix<T,3,3> S= eigen_util::makeSkewSymmetric<T>(getOrientationChange(state));
			return Q_i_b*getOrientation(state).transpose() * ((state.template block<3,1>(acceleration_start, 0) + zavi::plugin::IMU_Plugin::gravity))+Q_i_b*(S*S )*alignment.template block<3,1>(0,3);

		}
		/**
		 * get the orientation in state
		 * @param state the state
		 * @return the orientation part as 3x3 matrix
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 3> getOrientation(const OUTER_T<T> &state) {
			return Eigen::Map<const Eigen::Matrix<T, 3, 3> >(state.template block<9,1>(orientation_start, 0).data());
		}

		/**
		 * Function to integrate an acceleration measurement
		 * @param state the current state
		 * @param prior unused
		 * @return the estimated orientation change
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> orientation_change_measurement(
				const OUTER_T<T> & state,const Eigen::Matrix<T,4,4> & alignment, void * prior) {
			return alignment.template block<3,3>(0,0)*getOrientationChange(state);
		}

		/**
		 * get the orientation change in state
		 * @param state the state
		 * @return the orientation change part as 3x3 matrix
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> getOrientationChange(const OUTER_T<T> &state) {
			return state.template block<3, 1>(orientation_speed_start, 0);
		}
		/**
		 * Turn rotation matrix into 9x1 block
		 * @param rotation the rotation matrix
		 * @return a 9x1 block containing the rotation matrix
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 9, 1> toStateOrientation(const Eigen::Matrix<T, 3, 3> &rotation) {
			return Eigen::Map<Eigen::Matrix<T, 9, 1> >(rotation.data());
		}

		/**
		 * returns the Velocity
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> getVelocity(const OUTER_T<T> &state) {
			return state.template block<3, 1>(velocity_start, 0);
		}
		/**
		 * returns the Lambda Velocity
		 */
		template<typename T>
		static inline T getLambdaVelocity(const OUTER_T<T> &state) {
			return state(lambda_velocity_start);
		}

	};
}
//zavi::estimator::model

#endif /* ESTIMATORS_MODEL_LAMBDAPATHWITHVELOCITYMODEL_HPP_ */


#include <vector>
#include <list>
#include <map>
#include <set>

#include <queue>
#include <deque>
#include <stack>
#include <bitset>
#include <algorithm>
#include <functional>
#include <numeric>
#include <utility>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <cstdio>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <cstring>
#include <assert.h>
#include <boost/lexical_cast.hpp>

#define INF 1023123123
#define EPS 1e-11
#define LSOne(S) (S & (-S))

#define M_PI           3.14159265358979323846  /* pi */

#define FORN(X,Y) for (int (X) = 0;(X) < (Y);++(X))
#define FORB(X,Y) for (int (X) = (Y);(X) >= 0;--(X))
#define REP(X,Y,Z) for (int (X) = (Y);(X) < (Z);++(X))
#define REPB(X,Y,Z) for (int (X) = (Y);(X) >= (Z);--(X))

#define SZ(Z) ((int)(Z).size())
#define ALL(W) (W).begin(), (W).end()
#define PB push_back

#define MP make_pair
#define A first
#define B second

#define FORIT(X,Y) for(typeof((Y).begin()) X = (Y).begin();X!=(Y).end();X++)

using namespace std;

typedef long long ll;
typedef double db;
typedef vector<int> vi;
typedef pair<int, int> ii;
typedef vector<ii> vii;

void print_case(int i)
{
   cout << "Case #" << i + 1 << ": ";
}

bool myfunction(pair<ll, pair<ll, int>> i, pair<ll, pair<ll, int>> j) { return (i>j); }

int main()
{
   cout.precision(12);
   int ntc;
   cin >> ntc;

   FORN(kk, ntc)
   {
      print_case(kk);
      int n, k;
      cin >> n >> k;
      vector<pair<ll, pair<ll, int>>> hs(n);
      vector<pair<ll, pair<ll, int>>> rs(n);
      vector<pair<ll, ll>> all(n);
      vi nn;
      FORN(i, n)
      {
         hs[i].second.second = i;
         rs[i].second.second = i;
         cin >> rs[i].first;
         cin >> hs[i].first;
         hs[i].second.first = rs[i].first;
         rs[i].second.first = hs[i].first;
         all[i].first = rs[i].first;
         all[i].second = hs[i].first;
         hs[i].first *= rs[i].first;
      }
      sort(hs.begin(), hs.end(), myfunction);
      sort(rs.begin(), rs.end(), myfunction);

      double maxArea = -1;
      FORN(i, n)
      {
         auto top = rs[i];
         int toChoose = k - 1;
         int hIndex = 0;
         double area = M_PI * top.first * top.first + 2 * M_PI * all[top.second.second].second * top.first;
         while (hIndex < n && toChoose > 0)
         {
            if (hs[hIndex].second.second != top.second.second && all[hs[hIndex].second.second].first <= top.first)
            {
               area += 2 * M_PI * hs[hIndex].first;
               toChoose--;
            }
            hIndex++;
         }
         if (toChoose == 0 && area > maxArea)
         {
            maxArea = area;
         }
      }

      cout << maxArea << endl;
   }
}


#include<iostream>
//#define EIGEN_USE_MKL_ALL
#include"basis.hpp"
#include"operators.hpp"
#include"diag.h"
#include"tpoperators.hpp"
#include"files.hpp"
#include"timeev.hpp"
#include"ETH.hpp"
#include<iomanip>
#include <boost/program_options.hpp>

using namespace boost::program_options;
int main(int argc, char *argv[])
{
  using namespace Eigen;
using namespace std;
using namespace Many_Body;
  using Fermi_HubbardBasis= TensorProduct<ElectronBasis, ElectronBasis>;
   using HolsteinBasis= TensorProduct<ElectronBasis, PhononBasis>;
  using Mat= Operators::Mat;
   using boost::program_options::value;
   int N{};
  int L{};
  int i{};
  int j{};
    double T{};
      double tot{};
        double dt{};
  double t0{};
  double omega{};
  double gamma{};
  double cutoff{};
  bool PB{};

  std::string sN{};
  std::string sL{};
  std::string si{};
  std::string sj{};
  std::string star{};
  std::string sT{};
    std::string stot{};
      std::string sdt{};
  std::string st0{};
  std::string somega{};
  std::string sgamma{};
  std::string starget{};
  std::string scutoff{};
  std::string sPB{};
  std::string filename="freefermSPexT";
  try
  {
    boost::program_options::options_description desc{"Options"};
    desc.add_options()
      ("help,h", "Help screen")
      ("L", value(&L)->default_value(4), "L")
      ("i", value(&i)->default_value(0), "i")
      ("j", value(&j)->default_value(1), "j")
      ("T", value(&T)->default_value(0.1), "T")
      ("N,n", value(&N)->default_value(2), "N")
      ("tot", value(&tot)->default_value(1.), "tot")
      ("dt", value(&dt)->default_value(0.1), "dt")
      ("t0", value(&t0)->default_value(1.), "t0")
      ("gam", value(&gamma)->default_value(1.), "gamma")
      ("omg", value(&omega)->default_value(1.), "omega")
      ("pb", value(&PB)->default_value(0), "pb");
  boost::program_options::variables_map vm;
  boost::program_options::store(parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help"))
      {std::cout << desc << '\n'; return 0;}
    else{
      if (vm.count("L"))
      {      std::cout << "L: " << vm["L"].as<int>() << '\n';
      	sL="L"+std::to_string(vm["L"].as<int>());
	filename+=sL;
      }
         if (vm.count("N"))
      {      std::cout << "N: " << vm["N"].as<int>() << '\n';
      	sN="N"+std::to_string(vm["N"].as<int>());
	filename+=sN;
      }
	   if (vm.count("i"))
      {      std::cout << "i: " << i << '\n';
      	si="i"+std::to_string(i);
	filename+=si;
      }
	   if (vm.count("j"))
      {      std::cout << "j: " << j << '\n';
      	sj="j"+std::to_string(j);
	filename+=sj;
      }
      	 if (vm.count("t0"))
      {      std::cout << "t0: " << vm["t0"].as<double>() << '\n';
      	st0="t0"+std::to_string(vm["t0"].as<double>()).substr(0, 3);
      	filename+=st0;
      }

		      		 if (vm.count("tot"))
      {      std::cout << "tot: " << vm["tot"].as<double>() << '\n';
      	stot="tot"+std::to_string(vm["tot"].as<double>()).substr(0, 3);
      		filename+=stot;
      }
				 if (vm.count("dt"))
      {      std::cout << "dt: " << vm["dt"].as<double>() << '\n';
      	sdt="dt"+std::to_string(vm["dt"].as<double>()).substr(0, 4);
      		filename+=sdt;
      }

      		 if (vm.count("T"))
      {      std::cout << "T: " << T << '\n';
      	sT="T"+std::to_string(vm["T"].as<double>()).substr(0, 3);
      	filename+=sT;
      }
		 if (vm.count("pb"))
      {      std::cout << "PB: " << vm["pb"].as<bool>() << '\n';
      	sPB="PB"+std::to_string(vm["pb"].as<bool>());
      		filename+=sPB;
      }
      }
    }
  catch (const boost::program_options::error &ex)
  {
    std::cerr << ex.what() << '\n';
    return 0;
  }
  filename+=".bin";

  // declaring the tensor products with 0 e, 1, e, 2 e
  ElectronBasis e( L);
  //std::cout<< e.dim<< std::endl;
   //std::cout<< e << std::endl;
    auto Ham=Operators::EKinOperator(e, t0, PB);

        Eigen::VectorXd eigenVals(e.dim);

      Eigen::MatrixXd HH=Eigen::MatrixXd(Ham);

  	  Many_Body::diagMat(HH, eigenVals);

   std::cout<< eigenVals(0)<< std::endl;
   //std::cout<< Eigen::MatrixXd(C)<< std::endl;
       Eigen::VectorXd inS=HH.col(0);
       std::cout<<"MIN E0 "<<std::setprecision(15)<< eigenVals(0)<< std::endl;

       for(int b=0; b<L; b++)
	 {
	   auto N_e=Operators::NumberOperatorE( e, 1., PB, b, b+1);
	   //	   std::cout<<N_e << std::endl;

       std::cout<< " Exp "<< b << "  "<<inS.transpose()*N_e*inS<< std::endl;
}
       //       Eigen::VectorXd inS=HH.col(0);

       // inS(0)=1;
  auto N_e=Operators::NumberOperatorE( e, 1., PB);
       std::cout<< " tot Exp "<<inS.transpose()*N_e*inS<< std::endl;
         int n=0;
        Eigen::MatrixXcd evExpbeta=Eigen::MatrixXcd::Zero(eigenVals.rows(), eigenVals.rows());
         double Z{0};
	 //	 std::cout<< " Exp "<< inS.transpose()*N_e*inS<<std::endl;
// 	auto it=TP.basis.begin();
	 std::vector<double > ev;

      std::vector<double> time;
      std::vector<std::complex<double>> cdagc;
      std::vector<std::complex<double>> ccdag;
      auto HT=HH.transpose();

//        while(n*dt<tot)
//        	{

//          Eigen::MatrixXcd evExpm=TimeEv::EigenvalExponent(eigenVals, n*dt);
//             Eigen::MatrixXcd evExpp=TimeEv::EigenvalExponent(eigenVals, -n*dt);

//  	    auto CDAGC=(inS.transpose())*Cdag*HH*evExpm*HT*C*inS;
// 	     auto CCDAG= (inS.transpose())*C*HH*evExpp*HT*Cdag*in

      int ii=int(L/2);
      std::cout<< " i " <<ii<< std::endl;
      double s1{0};
      double s2{0};
      for(int jj=0; jj<L; jj++)
	{
	  auto Cdag=Operators::CdagOperator(e, ii,  PB);
	   auto C=Operators::COperator(e, jj,  PB);
auto CDAGC=(inS.transpose())*Cdag*C*inS;
 	     auto CCDAG= (inS.transpose())*C*Cdag*inS;
	     s1+=CDAGC;
	     s2+=CCDAG;
//     using Many_Body::im;
     std::cout<<jj << "  CDAGC \t"<< CDAGC<< " and CCDAG "<<CCDAG<< "sum "<< CDAGC+CCDAG<<std::endl;
	}
      std::cout<< s1<< " and " << s2<<std::endl;

// /// 		      std::cout<<n*dt << "\t"<<  " and  "<<CCDAG<< "  "<< "  sum  " <<Z<<std::endl;
//        			    	    time.push_back(n*dt);
//      			    	    ccdag.push_back(CCDAG);
//      			    cdagc.push_back(CDAGC)
// 							  ;
//    n++;

//    	 	}
//   // 	std::cout<< ccdag.size() << "  "<< cdagc.size()<< std::endl;
//        bin_write("time"+filename, time);
//        bin_write("CCDAG"+filename, ccdag);
//        bin_write("CDAGC"+filename, cdagc);

       // 			  int pb=PB;
       // 		  	  std::string Hs="H";
       // 			  std::string Ts="T";
       // 			  std::string phds="PHD";

       // 		      bin_write("E"+filename, Evec);
       // 		      bin_write("Nph"+filename, Ovec);
       // 		      bin_write("EK"+filename, Ovec2);
       // 		      bin_write("nX"+filename, Ovec3);
       // 		      bin_write("temp"+filename, Tr);
  return 0;
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * 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.
 ********************************************************************************/

#ifndef RWLIBS_ALGORITHMS_QPCONTROLLER_QPSOLVER_HPP
#define RWLIBS_ALGORITHMS_QPCONTROLLER_QPSOLVER_HPP

/**
 * @file QPSolver.hpp
 */

#include <Eigen/Core>

namespace rwlibs { namespace algorithms { namespace qpcontroller {

    /**
     * @brief Class providing an algorithms for solving the quadratic optimization
     * problem associated with the QPController
     */
    class QPSolver
    {
      public:
        /**
         * @brief Enumeration used to indicate status
         */
        enum Status {
            SUCCESS = 0, /* Solved */
            SUBOPTIMAL, /* Constraint satisfied but the result may be suboptimal. This may occur due
                           to round off errors */
            FAILURE     /* Could not find a solution statisfying all the constraints */
        };

        /**
         * Solves the quadratic problem of minimizing 1/2 x^T.G.x+d^T.x subject
         * to A.x>=b The method used is an iterative method from "Numerical
         * Optimization" by Jorge Nocedal and Stephen J. Wright, Springer 1999.
         * In this implementation we'll require that b<= s.t. x=0 is a feasible
         * initial value
         *
         * \param G [in] The G matrix. It is required that G is n times n and is
         * positive semidefinite
         *
         * \param d [in] Vector of length n
         *
         * \param A [in] Matrix used to represent the linear inequality constraints.
         * The dimensions should be m times n
         *
         * \param b [in] Vector with the lower limit for the constraints. We'll
         * assume that b<=0. The length of b should be m
         *
         * \param xstart [in] Default start configuration of the iterative algorithm
         *
         * \param status [out] Gives the status of the solving
         */
        static Eigen::VectorXd inequalitySolve (const Eigen::MatrixXd& G, const Eigen::VectorXd& d,
                                                Eigen::MatrixXd& A, const Eigen::VectorXd& b,
                                                const Eigen::VectorXd& xstart, Status& status);

        // TODO Investigate the possibility of making a hot-start of the
        // algorithm for better performance

      private:
        static Eigen::VectorXd getInitialConfig (Eigen::MatrixXd& A, const Eigen::VectorXd& b);

        static Eigen::VectorXd safeApprox (Eigen::MatrixXd& A, const Eigen::VectorXd& b);
    };

}}}    // namespace rwlibs::algorithms::qpcontroller

#endif    // end include guard


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>

EVE_TEST_TYPES( "Check return types of cyl_bessel_i1"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::cyl_bessel_i1(T(0)), T);
  TTS_EXPR_IS(eve::cyl_bessel_i1(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of cyl_bessel_i1 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 5.5),
                              eve::test::randoms(5.5, 9.5),
                              eve::test::randoms(9.5, 500.0)
                             )
         )
<typename T>(T const& a0, T const& a1, T const& a2)
{
  using v_t = eve::element_type_t<T>;
  auto eve__cyl_bessel_i1 =  [](auto x) { return eve::cyl_bessel_i1(x); };
#if defined(__cpp_lib_math_special_functions)
  auto std__cyl_bessel_i1 = [](auto x)->v_t { return std::cyl_bessel_i(v_t(1), x); };
#else
  auto std__cyl_bessel_i1 = [](auto x)->v_t { return boost::math::cyl_bessel_i(double(1), double(x)); };
#endif

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__cyl_bessel_i1(eve::inf(eve::as<v_t>())), eve::inf(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_i1(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_i1(eve::inf(eve::as< T>())),  eve::inf(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__cyl_bessel_i1(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }

  TTS_IEEE_EQUAL(eve__cyl_bessel_i1(v_t(-1)), eve::nan(eve::as<v_t>()));
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(v_t(500)), std__cyl_bessel_i1(v_t(500)), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(v_t(10)), std__cyl_bessel_i1(v_t(10))  , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(v_t(5)),  std__cyl_bessel_i1(v_t(5))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(v_t(2)),  std__cyl_bessel_i1(v_t(2))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(v_t(1.5)),std__cyl_bessel_i1(v_t(1.5)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(v_t(0.5)),std__cyl_bessel_i1(v_t(0.5)) , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(v_t(1)),  std__cyl_bessel_i1(v_t(1))   , 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(v_t(0)),  eve::zero(eve::as<v_t>()), 0.0);

  TTS_IEEE_EQUAL(eve__cyl_bessel_i1(T(-1)), eve::nan(eve::as<T>()));
  TTS_ULP_EQUAL(eve__cyl_bessel_i1( T(500)),  T(std__cyl_bessel_i1(v_t(500)) ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1( T(10)) ,  T(std__cyl_bessel_i1( v_t(10)) ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1( T(5))  ,  T(std__cyl_bessel_i1( v_t(5))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1( T(2))  ,  T(std__cyl_bessel_i1( v_t(2))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1( T(1.5)),  T(std__cyl_bessel_i1( v_t(1.5))), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1( T(0.5)),  T(std__cyl_bessel_i1( v_t(0.5))), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1( T(1))  ,  T(std__cyl_bessel_i1( v_t(1))  ), 6.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1( T(0))  , eve::zero(eve::as< T>()), 0.0);

  TTS_ULP_EQUAL(eve__cyl_bessel_i1(a0), map(std__cyl_bessel_i1, a0), 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(a1), map(std__cyl_bessel_i1, a1), 10.0);
  TTS_ULP_EQUAL(eve__cyl_bessel_i1(a2), map(std__cyl_bessel_i1, a2), 10.0);
};

EVE_TEST( "Check behavior of cyl_bessel_i1 on wide with negative non integral order"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 60.0))
        )
  <typename T>(T a0 )
{
  using v_t = eve::element_type_t<T>;
  auto eve__diff_bessel_i1 =  [](auto x) { return eve::diff(eve::cyl_bessel_i1)(x); };
  auto std__diff_bessel_i1 =  [](auto x)->v_t { return boost::math::cyl_bessel_i_prime(double(1), double(x)); };
  TTS_RELATIVE_EQUAL(eve__diff_bessel_i1(a0),   map(std__diff_bessel_i1, a0)   , 1.0e-3);

};