#include <matrixlib/MatrixUtils.h>
#include <NTL/vec_GF2.h>

void random(NTL::mat_GF2 &L, int n1, int n2)
{
	L.SetDims(n1,n2);
	int hw;
	for (int i = 0; i < n1; i++)
	{
		do
	    	{
	    		hw = 0;
				for (int j = 0; j < n2; j++)
				{
					L[i][j] = NTL::random_GF2();
					if (L[i][j] == 1)
			            hw += 1;
				}
			} while (hw == 1);
	}
}
int initMatrixFromBit(NTL::mat_GF2 &M, long *data) {
    long i,j,n,m;
	for(i=0, n=M.NumRows(); i<n; i++){
		long t = *data;
		for(j=M.NumCols()-1; j>=0; j--){
			M.put(i,j,t%2);
			t >>= 1;
		}
		data++;
	}

	return 0;
}

int initVecFromBit(NTL::vec_GF2 &M, long data, int len) {
	M.SetLength(len);
    long i,j,n,m;
	for(i=len-1; i>=0; i--){
		M.put(i, data%2);
		data >>=1;
	}
	return 0;
}
int genRandomAffineAugmentedMatrix(NTL::mat_GF2 &X,  NTL::mat_GF2 &Y, int n) {
	NTL::mat_GF2 L;
	random(L, n, n);
	while(determinant(L)==0) {
		random(L, n, n);
	}
	NTL::mat_GF2 LINV = inv(L);
	NTL::vec_GF2 tran;
	// tran.SetLength(m);
	random(tran, n);
	NTL::mat_GF2 AugmentedMatrix;

	X.SetDims(n+1, n+1);
	int i,j;
	for (i=0; i<n; i++) {
		for ( j=0; j<n; j++) {
			X.put(i,j, L.get(i, j));
		}
	}

	for (j=0; j<n; j++) {
		X.put(n, j, 0);
	}
	X.put(n, n, 1);

	for (i=0; i<n; i++) {
		X.put(i, n, tran.get(i));
	}

	NTL::GF2 d;

	inv(d, Y, X);

	return 0;

}

int genRandomAffineMatrix(NTL::mat_GF2 &X,  NTL::mat_GF2 &Y, NTL::vec_GF2 &V, int n) {
	random(X, n, n);
	while(determinant(X)==0) {
		random(X, n, n);
	}
	NTL::GF2 d;
	inv(d, Y, X);
	random(V, n);
	return 0;
}

int genRandomAffineMatrix(NTL::mat_GF2 &x,  NTL::mat_GF2 &inv_x, NTL::vec_GF2 &v, NTL::vec_GF2 &inv_v, int n) {
	random(x, n, n);
	while(determinant(x)==0) {
		random(x, n, n);
	}
	NTL::GF2 d;
	inv(d, inv_x, x);
	random(v, n);
	mul(inv_v, inv_x, v);
	return 0;
}

int genRandomInvMatrix(NTL::mat_GF2 &x,  NTL::mat_GF2 &inv_x, int n) {
	random(x, n, n);
	while(determinant(x)==0) {
		random(x, n, n);
	}
	NTL::GF2 d;
	inv(d, inv_x, x);
	return 0;
}

uint32_t getDigitalFromVec(NTL::vec_GF2 &s) {
	uint32_t d = 0;
	int i;
	int m = s.length();
	for (i=0; i<m; i++) {
		d <<=1 ;
		d += ((s.get(i)==1)?1:0);
	}
	return d;
}

uint32_t get32FromVec(NTL::vec_GF2 &s) {
	uint32_t d = 0;
	int i;
	for (i=0; i<32; i++) {
		d <<=1 ;
		d += ((s.get(i)==1)?1:0);
	}
	return d;
}

uint8_t get8FromVec(const vector_transform_t &s) {
	uint8_t d = 0;
	int i;
	for (i=0; i<8; i++) {
		d <<=1 ;
		d += ((s.get(i)==1)?1:0);
	}
	return d;
}

int getVecFrom32(NTL::vec_GF2 &d,uint32_t s) {
	d.SetLength(32);
	int i;
	for (i=31; i>=0; i--) {
		d.put(i, s%2);
		s>>=1;
	}
	return 0;
}

int getAugmentedVecFrom32(NTL::vec_GF2 &d, uint32_t s) {
	d.SetLength(33);
	int i;
	for (i=31; i>=0; i--) {
		d.put(i, s%2);
		s>>=1;
	}
	d.put(32, 1);
	return 0;
}

int combineDiagMat(NTL::mat_GF2 &d, NTL::mat_GF2 &s1, NTL::mat_GF2 &s2) {
	long n1 = s1.NumCols();
	long n2 = s2.NumCols();
	d.SetDims(n1+n2, n1+n2);
	int i,j;
	for (i=0; i<n1; i++) {
		for (j=0; j<n1; j++) {
			d.put(i, j, s1.get(i,j));
		}
		for (j=0; j<n2; j++) {
			d.put(i, n1+j, 0);
		}
	}
	for (i=0; i<n2; i++) {
		for (j=0; j<n1; j++) {
			d.put(n1+i, j, 0);
		}
		for (j=0; j<n2; j++) {
			d.put(n1+i, n1+j, s2.get(i,j));
		}
	}
	return 0;
}

uint32_t applyAffineToU32(const affine_transform_t &aff, uint32_t data ) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 32);
	// dumpVector(a);
	// dumpMatrix(aff.linearMap);
	b = aff.linearMap * a + aff.vectorTranslation;
	return get32FromVec(b);
}

uint8_t applyAffineToU8(const affine_transform_t &aff, uint8_t data) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 8);
	// dumpVector(a);
	// dumpMatrix(aff.linearMap);
	b = aff.linearMap * a + aff.vectorTranslation;
	return (uint8_t)getDigitalFromVec(b);
}

uint8_t applyMatToU8(const NTL::mat_GF2 &mat, uint8_t data) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 8);
	b = mat*a;
	return (uint8_t)getDigitalFromVec(b);
}

uint32_t applyMatToU32(const matrix_transform_t &mat, uint32_t data) {
	vector_transform_t a,b;
	initVecFromBit(a, data, 32);
	b = mat*a;
	return (uint32_t)getDigitalFromVec(b);
}

uint32_t addVecToU32(NTL::vec_GF2 &vec, uint32_t data) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 32);
	b = vec+a;
	return (uint32_t)getDigitalFromVec(b);
}

uint8_t addVecToU8(NTL::vec_GF2 &vec, uint8_t data) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 8);
	b = vec+a;
	return (uint8_t)getDigitalFromVec(b);
}

int genIndMatrix(NTL::mat_GF2 &mat, int size) {
	mat.SetDims(size, size);
	int i,j;
	for (i=0; i<size; i++) {
		for (j=0; j<size; j++) {
			mat.put(i, j, i==j);
		}
	}
	return 0;
}

int genZeroVec(NTL::vec_GF2 &vec, int size) {
	vec.SetLength(size);
	int i;
	for (i=0; i<size; i++) {
		vec.put(i, 0);
	}
	return 0;
}


#include "simulator.h"
#include <Eigen/Eigenvalues>
#include <future>
#include <iomanip>
#include <iostream>
#include <limits>
#include <set>

using namespace Simulator;

std::string Simulator::AlgorithmToStr(const Algorithms algorithm)
{
	switch (algorithm) {
	case Algorithms::Metropolis:
		return { "Metropolis method" };
	case Algorithms::Glauber:
		return { "Glauber dynamics" };
	case Algorithms::SCA:
		return { "Stochastic cellular automata" };
	case Algorithms::fcSCA:
		return { "Flip-Constrained Stochastic Cellular Automata" };
	case Algorithms::MA:
		return { "Momentum annealing" };
	case Algorithms::MMA:
		return { "Modified momentum annealing" };
	case Algorithms::HillClimbing:
		return { "Hill climbing" };
	default:
		return { "Warning: Unknown type." };
	}
}

// quadraticのキーのペア (i, j) は順番が i < j となっていなければならない。
IsingModel::IsingModel(const LinearBiases linear, const QuadraticBiases quadratic)
	: rand(std::make_unique<Rand>())
	, temperature(0.e0)
	, pinningParameter(0.e0)
	, flipTrialRate(0.e0)
	, algorithm(Algorithms::Metropolis)
{
	// spinsの添字と頂点の名前との対応表を作成。
	std::set<Node> nodes;
	for (auto iter = linear.begin(); iter != linear.end(); iter++)
		nodes.insert(iter->first);
	for (auto iter = quadratic.begin(); iter != quadratic.end(); iter++) {
		nodes.insert(iter->first.first);
		nodes.insert(iter->first.second);
	}
	std::size_t index = 0;
	for (const auto& key : nodes)
		nodeIndices[key] = index++;

	// Hamiltonianの定数と変数を初期化。
	auto maxNodes = nodeIndices.size();
	spins.setConstant(maxNodes, Spin::Up);
	previousSpins = spins;
	externalMagneticField.resize(maxNodes);
	for (const auto& node : nodeIndices) {
		auto iter = linear.find(node.first);
		if (iter != linear.end())
			externalMagneticField(node.second) = iter->second;
		else
			externalMagneticField(node.second) = 0.e0;
	}
	couplingCoefficients = Eigen::MatrixXd::Zero(maxNodes, maxNodes);
	for (const auto& row : nodeIndices) {
		for (const auto& column : nodeIndices) {
			if (row.first > column.first)
				continue;
			auto iter = quadratic.find(std::make_pair(row.first, column.first));
			if (iter != quadratic.end())
				couplingCoefficients(row.second, column.second) = couplingCoefficients(column.second, row.second) = iter->second;
			else
				couplingCoefficients(row.second, column.second) = couplingCoefficients(column.second, row.second) = 0.e0;
		}
	}
}

// 行列 (-J_{x, y})_{x, y} の最大固有値を計算する。
double IsingModel::CalcLargestEigenvalue() const
{
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(-couplingCoefficients);
	return solver.eigenvalues().reverse()(0);
}

double IsingModel::GetEnergy() const
{
	// Remove double-counting duplicates by multiplying the sum by 1/2.
	return -spins.cast<double>().transpose() * (0.5e0 * couplingCoefficients * spins.cast<double>() + externalMagneticField);
}

double IsingModel::GetEnergyOnBipartiteGraph() const
{
	return -0.5e0 * spins.cast<double>().transpose() * couplingCoefficients * spins.cast<double>()
		- 0.5e0 * externalMagneticField.dot(spins.cast<double>() + previousSpins.cast<double>())
		+ 0.5e0 * pinningParameter * (spins.size() - spins.cast<double>().dot(previousSpins.cast<double>()));
}

void IsingModel::GiveSpins(const ConfigurationsType configurationType)
{
	switch (configurationType) {
	case ConfigurationsType::AllDown:
		spins.fill(Spin::Down);
		break;
	case ConfigurationsType::AllUp:
		spins.fill(Spin::Up);
		break;
	case ConfigurationsType::Uniform:
		for (auto i = 0; i < spins.size(); i++)
			spins(i) = rand->Bernoulli(0.5e0) ? Spin::Down : Spin::Up;
		break;
	default:
		break;
	}
	previousSpins = spins;
}

void IsingModel::Update()
{
	auto metropolisMethod = [this]() {
		unsigned int updatedNodeIndex = (*rand)(spins.size());
		double energyDifference = 2.e0 * static_cast<int>(spins(updatedNodeIndex)) * calcLocalMagneticField(updatedNodeIndex);
		if (energyDifference < 0.e0)
			spins(updatedNodeIndex) = flip(spins(updatedNodeIndex));
		else if (rand->Bernoulli(std::exp(-energyDifference / temperature)))
			spins(updatedNodeIndex) = flip(spins(updatedNodeIndex));
	};

	auto glauberDynamics = [this]() {
		unsigned int updatedNodeIndex = (*rand)(spins.size());
		if (rand->Bernoulli(1.e0 / (1.e0 + std::exp(-2.e0 * calcLocalMagneticField(updatedNodeIndex) / temperature))))
			spins(updatedNodeIndex) = Spin::Up;
		else
			spins(updatedNodeIndex) = Spin::Down;
	};

	auto stochasticCellularAutomata = [this]() {
		previousSpins = spins;
		spins = (
			calcLocalMagneticField(spins) + pinningParameter * spins.cast<double>()
			- temperature * Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double { return rand->Logistic(); })
		).array().sign().cast<Spin>();  // 実質起こらないが、符号関数に渡しているため、スピンが0になる場合がある。
	};

	auto flipConstrainedStochasticCellularAutomata = [this]() {
		previousSpins = spins;
		//auto bernoulli =  Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> bool { return rand->Bernoulli(flipTrialRate) ; });  // = true w.p. flipTrialRate and = false w.p. 1 - flipTrialRate.
		spins = (
			calcLocalMagneticField(spins) + pinningParameter * spins.cast<double>()
			- temperature * Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double { return rand->Logistic(); })
			+ Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double {
				return rand->Bernoulli(flipTrialRate) ? 0.e0 : std::numeric_limits<double>::infinity();
			}).cwiseProduct(spins.cast<double>())
			//+ bernoulli.unaryExpr([](bool b) -> double { return b ? 0.e0 : std::numeric_limits<double>::infinity(); }).cwiseProduct(spins.cast<double>())
		).array().sign().cast<Spin>();  // 実質起こらないが、符号関数に渡しているため、スピンが0になる場合がある。
	};

	// 温度を下げなければ ``annealing'' ではないが、論文では区別していないので、ここでもこの名称を用いる。
	auto momentumAnnealing = [this]() {
		Configuration temp = (
			calcLocalMagneticField(spins) + pinningParameter * spins.cast<double>()
			- temperature * Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double { return rand->Exponential(); }).cwiseProduct(previousSpins.cast<double>())
		).array().sign().cast<Spin>();  // 実質起こらないが、符号関数に渡しているため、スピンが0になる場合がある。
		previousSpins = spins;
		spins = temp;
	};

	auto modifiedMomentumAnnealing = [this]() {
		previousSpins = spins;
		spins = (
			calcLocalMagneticField(spins) + pinningParameter * spins.cast<double>()
			- temperature * Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double { return rand->Exponential(); }).cwiseProduct(spins.cast<double>())
		).array().sign().cast<Spin>();  // 実質起こらないが、符号関数に渡しているため、スピンが0になる場合がある。
	};

	auto hillClimbing = [this]() {
		Configuration currentConfiguration = spins;
		while (true) {
			//double nextEval = std::numeric_limits<double>::infinity();
			double energyDifference = 0.e0;
			Configuration nextConfiguration = currentConfiguration;
			for (auto i = 0; i < spins.size(); i++) {
				double beforeEnergy = -1.e0 * static_cast<int>(currentConfiguration(i)) * calcLocalMagneticField(i);
				double afterEnergy = -1.e0 * static_cast<int>(flip(currentConfiguration(i))) * calcLocalMagneticField(i);
				if (energyDifference > afterEnergy - beforeEnergy) {
					energyDifference = afterEnergy - beforeEnergy;
					nextConfiguration(i) = flip(nextConfiguration(i));
				}
			}
			if (energyDifference >= 0.e0)
				break;
			currentConfiguration = nextConfiguration;
		}
		spins = currentConfiguration;
	};

	switch (algorithm) {
	case Algorithms::Metropolis:
		metropolisMethod();
		break;
	case Algorithms::Glauber:
		glauberDynamics();
		break;
	case Algorithms::SCA:
		stochasticCellularAutomata();
		break;
	case Algorithms::fcSCA:
		flipConstrainedStochasticCellularAutomata();
		break;
	case Algorithms::MA:
		momentumAnnealing();
		break;
	case Algorithms::MMA:
		modifiedMomentumAnnealing();
		break;
	case Algorithms::HillClimbing:
		hillClimbing();
		break;
	default:
		break;
	}
}

void IsingModel::Write() const
{
	std::cout << "Current spin configuration:" << std::endl;
	for (auto i = 0; i < spins.size(); i++)
		std::cout << std::setw(2) << static_cast<int>(spins(i));
	std::cout << "External magnetic field:" << std::endl;
	std::cout << externalMagneticField.transpose() << std::endl;
	std::cout << "Coupling coefficinets:" << std::endl;
	std::cout << couplingCoefficients << std::endl;
	std::cout << "Algorithm: " << AlgorithmToStr(algorithm) << std::endl;
	std::cout << "Temperature: " << temperature << std::endl;
	std::cout << "Pinning parameter: " << pinningParameter << std::endl;
	std::cout << "Flip trial rate: " << flipTrialRate << std::endl;
}


/* User code: This file will not be overwritten by TASTE. */

#include "printer.h"
#include <iostream>
#include <fstream>
#include "base_support/Base-samples-RigidBodyStateConvert.hpp"
#include "base_support/OpaqueConversion.hpp"
#include <Eigen/Core>
#include <vector>

void printer_startup()
{
    /* Write your initialization code here,
       but do not make any call to a required interface. */
}

// EIGEN HELPER METHODS
void to4d(Eigen::Matrix3d r, Eigen::Vector3d t, Eigen::Matrix4d & h){

  h<< r(0,0), r(0,1), r(0,2), t[0],
      r(1,0), r(1,1), r(1,2), t[1],
      r(2,0), r(2,1), r(2,2), t[2],
          0,      0,      0,    1;
}

void printer_PI_update_amp(const asn1SccBase_samples_RigidBodyState *IN_pose)
{
  base::Vector3d t;
  base::Quaterniond q;
  Eigen::Matrix3d r;

  static int i = 0;

  // extract orientation / translation
  asn1Scc_Vector3d_fromAsn1(t, IN_pose->position);
  asn1Scc_Quaterniond_fromAsn1(q, IN_pose->orientation);
  // convert quaternion to rotation mat
  r = q.normalized().toRotationMatrix();

  Eigen::Matrix4d p;

  to4d(r,t,p);

  std::ifstream f("marker.csv");
  bool good = f.good();

  std::ofstream myfile;
  if (good) myfile.open ("marker.csv", std::ios::app);
  else myfile.open ("marker.csv", std::ios::out);

  myfile << "\n" << std::floor(((p(0,3)) * 100000) + .5) / 100000
	 << ","  << std::floor(((p(1,3)) * 100000) + .5) / 100000
	 << ","  << std::floor(((p(2,3)) * 100000) + .5) / 100000;

  myfile.close();

  i++;

  std::cout << "got marker position:\n" << p << std::endl;
}

void printer_PI_update_arp(const asn1SccBase_samples_RigidBodyState *IN_pose)
{
  base::Vector3d t;
  base::Quaterniond q;
  Eigen::Matrix3d r;
  // extract orientation / translation
  asn1Scc_Vector3d_fromAsn1(t, IN_pose->position);
  asn1Scc_Quaterniond_fromAsn1(q, IN_pose->orientation);
  // convert quaternion to rotation mat
  r = q.normalized().toRotationMatrix();

  Eigen::Matrix4d p;

  to4d(r,t,p);

  std::ifstream f("marker.csv");
  bool good = f.good();

  std::ofstream myfile;
  if (good) myfile.open ("marker.csv", std::ios::app);
  else myfile.open ("marker.csv", std::ios::out);

  myfile << "\n" << std::floor(((p(0,3)) * 100000) + .5) / 100000
	 << ","  << std::floor(((p(1,3)) * 100000) + .5) / 100000
	 << ","  << std::floor(((p(2,3)) * 100000) + .5) / 100000;

  myfile.close();
  std::cout << "got robot position:\n" << p << std::endl;
}


#include "wx/wxprec.h"
#include "wx/wx.h"
#include "sample.xpm"
#include <Eigen/Dense>

using namespace Eigen;

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxAppHeat
/////////////////////////////////////////////////////////////////////////////////////////////////////

class wxAppHeat : public wxApp
{
public:
  virtual bool OnInit();
};

wxIMPLEMENT_APP(wxAppHeat);

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat
/////////////////////////////////////////////////////////////////////////////////////////////////////

class wxFrameHeat : public wxFrame
{
public:
  wxFrameHeat(const wxString& title);
  void OnQuit(wxCommandEvent& event);
  void OnAbout(wxCommandEvent& event);
  virtual void OnPaint(wxPaintEvent& event);
  wxColour to_color(float N);

  ~wxFrameHeat()
  {
    delete[] T_fvm;
    delete[] T_fdm_1;
    delete[] Tn_fdm_1;
    delete[] T_fdm_2;
    delete[] Tn_fdm_2;
  }

private:
  void init_constants();
  void solve_fvm();
  void solve_fdm_1();
  void solve_fdm_2();
  void draw_cell(wxDC* dc, float N, wxCoord x, wxCoord y, wxCoord width, wxCoord height);

  //temperature at the left hand side of the bar(deg C)
  float T_A;

  //temperature at the left hand side of the bar(deg C)
  float T_B;

  //thermal conductivity k, [W/mK]
  float k;

  //heat source per unit volume (W/m3)
  float S;

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FVM
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //number of cells for FVM
  Index m_nbr_fvm;

  //temperature (solution)
  float* T_fvm;

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FDM
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //number of cells for FVM
  int m_nbr_fdm;

  //temperature (solution)
  float* T_fdm_1;
  float* Tn_fdm_1;

  //temperature (solution)
  float* T_fdm_2;
  float* Tn_fdm_2;

private:
  wxDECLARE_EVENT_TABLE();
};

wxBEGIN_EVENT_TABLE(wxFrameHeat, wxFrame)
EVT_MENU(wxID_EXIT, wxFrameHeat::OnQuit)
EVT_MENU(wxID_ABOUT, wxFrameHeat::OnAbout)
EVT_PAINT(wxFrameHeat::OnPaint)
wxEND_EVENT_TABLE()

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxAppHeat::OnInit()
/////////////////////////////////////////////////////////////////////////////////////////////////////

bool wxAppHeat::OnInit()
{
  if (!wxApp::OnInit())
  {
    return false;
  }
  wxFrameHeat* frame = new wxFrameHeat("Heat");
  frame->Maximize(true);
  frame->Show(true);
  return true;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::wxFrameHeat
/////////////////////////////////////////////////////////////////////////////////////////////////////

wxFrameHeat::wxFrameHeat(const wxString& title)
  : wxFrame(NULL, wxID_ANY, title)
{
  SetIcon(wxICON(sample));
  wxMenu* menu_file = new wxMenu;
  menu_file->Append(wxID_EXIT, "E&xit\tAlt-X", "Quit this program");
  wxMenu* menu_help = new wxMenu;
  menu_help->Append(wxID_ABOUT, "&About\tF1", "Show about dialog");
  wxMenuBar* menu_bar = new wxMenuBar();
  menu_bar->Append(menu_file, "&File");
  menu_bar->Append(menu_help, "&Help");
  SetMenuBar(menu_bar);
  CreateStatusBar(2);
  SetStatusText("Ready");
  init_constants();
  solve_fvm();
  solve_fdm_1();
  solve_fdm_2();
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::OnQuit
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::OnQuit(wxCommandEvent& WXUNUSED(event))
{
  Close(true);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::OnAbout
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::OnAbout(wxCommandEvent& WXUNUSED(event))
{
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::to_color
/////////////////////////////////////////////////////////////////////////////////////////////////////

wxColour wxFrameHeat::to_color(float N)
{
  wxColour C;
  if (N < 130) C = wxColour(187, 206, 255);
  else if (N < 160) C = wxColour(132, 166, 250);
  else if (N < 190) C = wxColour(66, 108, 218);
  else if (N < 200) C = wxColour(50, 72, 141);
  else C = wxColour(34, 47, 87);
  return C;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::draw_cell
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::draw_cell(wxDC* dc, float N, wxCoord x, wxCoord y, wxCoord width, wxCoord height)
{
  dc->SetBrush(wxBrush(wxColour(to_color(N))));
  dc->DrawRectangle(x, y - height / 2, width, height);
  wxString str = wxString::Format(wxT("%.1f"), N);
  dc->DrawText(str, x + width / 2, y);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::OnDraw
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::OnPaint(wxPaintEvent&)
{
  wxPaintDC dc(this);
  wxFont font(wxFontInfo(8).FaceName("Courier"));
  dc.SetFont(font);
  wxCoord width = 100;
  wxCoord height = 50;
  wxCoord x = 0, y = height / 2;

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FVM
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //boundary
  draw_cell(&dc, T_A, x, y, width, height);
  x += width;
  //interior
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    draw_cell(&dc, T_fvm[idx], x, y, width, height);
    x += width;
  }
  //boundary
  draw_cell(&dc, T_B, x, y, width, height);

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FDM_1
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  x = 0; y += 100;
  for (int idx = 0; idx < m_nbr_fdm; idx++)
  {
    draw_cell(&dc, T_fdm_1[idx], x, y, width, height);
    x += width;
  }

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FDM_2
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  x = 0; y += 100;
  for (int idx = 0; idx < m_nbr_fdm; idx++)
  {
    draw_cell(&dc, T_fdm_2[idx], x, y, width, height);
    x += width;
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::init_constants
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::init_constants()
{
  //temperature at the left hand side of the bar(deg C)
  T_A = 100;

  //temperature at the left hand side of the bar(deg C)
  T_B = 200;

  //thermal conductivity k, [W/mK]
  k = 100;

  //heat source per unit volume (W/m3)
  S = 1000.0f;;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::solve_fvm
//Finite Volume Method
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::solve_fvm()
{
  //number of cells
  m_nbr_fvm = 5;

  ////////////////////////////////////////////////////////////////////////////////////////////////////
 //allocate solution vectors
 /////////////////////////////////////////////////////////////////////////////////////////////////////

 //FVM
  T_fvm = new float[m_nbr_fvm];

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //geometry
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //length of the bar(m)
  const float length = 5;

  //coordinates of the cell faces
  float* x_faces = new float[m_nbr_fvm + 1];
  x_faces[0] = 0;
  for (int idx = 1; idx < m_nbr_fvm + 1; idx++)
  {
    x_faces[idx] = x_faces[idx - 1] + length / m_nbr_fvm;
    wxLogDebug("x_faces [%d]=%f", idx, x_faces[idx]);
  }

  //coordinates of the cell centroids
  float* x_center = new float[m_nbr_fvm];
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    x_center[idx] = 0.5f * (x_faces[idx + 1] + x_faces[idx]);
    wxLogDebug("x_center [%d]=%f", idx, x_center[idx]);
  }

  //length of each cell
  float* cell_length = new float[m_nbr_fvm];
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    cell_length[idx] = x_faces[idx + 1] - x_faces[idx];
    wxLogDebug("cell_length [%d]=%f", idx, cell_length[idx]);
  }

  //distance between cell centroids
  float* dist_centroids = new float[m_nbr_fvm + 1];
  for (int idx = 0; idx < m_nbr_fvm - 1; idx++)
  {
    dist_centroids[idx] = x_center[idx + 1] - x_center[idx];
    wxLogDebug("dist_centroids [%d]=%f", idx, dist_centroids[idx]);
  }

  //for the boundary cell on the left, the distance is double the distance
  //from the cell centroid to the boundary face
  float dist_left = 2 * (x_center[0] - x_faces[0]);

  //for the boundary cell on the right, the distance is double the distance from
  //the cell centroid to the boundary cell face
  float dist_right = 2 * (x_faces[m_nbr_fvm] - x_center[m_nbr_fvm - 1]);

  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    dist_centroids[idx + 1] = dist_centroids[idx];
  }
  dist_centroids[0] = dist_left;
  dist_centroids[m_nbr_fvm] = dist_right;
  for (int idx = 0; idx < m_nbr_fvm + 1; idx++)
  {
    wxLogDebug("dist_centroids [%d]=%f", idx, dist_centroids[idx]);
  }

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //constants
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //cross-sectional area A, [m2]
  const float Area = 0.1f;

  //cell volume
  float* cell_volume = new float[m_nbr_fvm];
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    cell_volume[idx] = cell_length[idx] * Area;
    wxLogDebug("cell_volume [%d]=%f", idx, cell_volume[idx]);
  }

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //create the matrices
  //zero initially
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  MatrixXf A = MatrixXf::Zero(m_nbr_fvm, m_nbr_fvm);
  VectorXf B = VectorXf::Zero(m_nbr_fvm);

  for (Index n = 0; n < m_nbr_fvm; n++)
  {
    /////////////////////////////////////////////////////////////////////////////////////////////////////
    //left boundary cell
    /////////////////////////////////////////////////////////////////////////////////////////////////////

    if (n == 0)
    {
      //left coefficient aP, T at n
      float aP = k * Area / dist_centroids[n] + k * Area / dist_centroids[n] * 2.0;
      wxLogDebug("left A[%lld,%lld]=%f", n, n, aP);
      A(n, n) = aP;

      //right coefficient aR, T at n+1
      float aR = k * Area / dist_centroids[n];
      wxLogDebug("left A[%lld,%lld]=%f", n, n + 1, -1.0 * aR);
      A(n, n + 1) = -1.0 * aR;

      //source
      float source = S * cell_volume[n];
      //additional boundary source, temperature A
      source += T_A * Area * k / dist_centroids[n] * 2.0;
      wxLogDebug("B[%lld]=%f", n, source);
      B(n) = source;
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////
    //right boundary cell
    /////////////////////////////////////////////////////////////////////////////////////////////////////

    else if (n == m_nbr_fvm - 1)
    {
      //left coefficient aL, T at n-1
      float aL = k * Area / dist_centroids[n];;
      wxLogDebug("right A[%lld,%lld]=%f", n, n - 1, -1.0 * aL);
      A(n, n - 1) = -1.0 * aL;

      //right coefficient aP, T at n
      float aP = k * Area / dist_centroids[n] + k * Area / dist_centroids[n] * 2.0;
      wxLogDebug("right A[%lld,%lld]=%f", n, n, aP);
      A(n, n) = aP;

      //source
      float source = S * cell_volume[n];
      //additional boundary source, temperature B
      source += T_B * Area * k / dist_centroids[n] * 2.0;
      wxLogDebug("B[%lld]=%f", n, source);
      B(n) = source;
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////
    //interior cells
    /////////////////////////////////////////////////////////////////////////////////////////////////////

    else
    {
      //left coefficient aL, T at n-1
      float aL = k * Area / dist_centroids[n];
      wxLogDebug("A[%lld,%lld]=%f", n, n - 1, -1.0 * aL);
      A(n, n - 1) = -1.0 * aL;

      //right coefficient aR, T at n+1
      float aR = k * Area / dist_centroids[n];
      wxLogDebug("A[%lld,%lld]=%f", n, n + 1, -1.0 * aR);
      A(n, n + 1) = -1.0 * aR;

      //middle coefficient aP, T at n
      float aP = aR + aL;
      wxLogDebug("A[%lld,%lld]=%f", n, n, aP);
      A(n, n) = aP;

      //source
      float source = S * cell_volume[n];
      wxLogDebug("B[%lld]=%f", n, source);
      B(n) = source;
    }
  }

  //solve
  VectorXf T_vec = A.colPivHouseholderQr().solve(B);
  float* t = T_vec.data();
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    T_fvm[idx] = t[idx];
    wxLogDebug("%.1f", t[idx]);
  }

  delete[] x_faces;
  delete[] x_center;
  delete[] cell_length;
  delete[] dist_centroids;
  delete[] cell_volume;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::solve_fdm_1
//Finite Differences Method
//diffusion only
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::solve_fdm_1()
{
  //number of cells
  m_nbr_fdm = 7;

  ////////////////////////////////////////////////////////////////////////////////////////////////////
 //allocate solution vectors
 /////////////////////////////////////////////////////////////////////////////////////////////////////

  //FDM
  T_fdm_1 = new float[m_nbr_fdm];
  Tn_fdm_1 = new float[m_nbr_fdm];

  //time step
  float dt = 0.005f;
  float dx = 1.0f;

  //initial and boundary values
  for (int i = 0; i < m_nbr_fdm; i++)
  {
    T_fdm_1[i] = 0;
  }
  T_fdm_1[0] = T_A;
  T_fdm_1[m_nbr_fdm - 1] = T_B;

  for (int i = 0; i < m_nbr_fdm; i++)
  {
    wxLogDebug("T[%d]=%.1f", i, T_fdm_1[i]);
  }

  //F must be <= 0.5
  float F = k * dt / (dx * dx);
  wxLogDebug("F=%.4f", F);
  for (int n = 0; n < 100; n++)
  {
    for (int i = 1; i < m_nbr_fdm - 1; i++)
    {
      Tn_fdm_1[i] = T_fdm_1[i] + F * (T_fdm_1[i - 1] - 2 * T_fdm_1[i] + T_fdm_1[i + 1]) + dt * S;
    }
    for (int i = 1; i < m_nbr_fdm - 1; i++)
    {
      T_fdm_1[i] = Tn_fdm_1[i];
    }
    for (int i = 0; i < m_nbr_fdm; i++)
    {
      wxLogDebug("T[%d]=%.1f", i, T_fdm_1[i]);
    }
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::solve_fdm_2
//Finite Differences Method
//advection only
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::solve_fdm_2()
{
  //number of cells
  m_nbr_fdm = 7;

  ////////////////////////////////////////////////////////////////////////////////////////////////////
 //allocate solution vectors
 /////////////////////////////////////////////////////////////////////////////////////////////////////

  //FDM
  T_fdm_2 = new float[m_nbr_fdm];
  Tn_fdm_2 = new float[m_nbr_fdm];

  //time step
  float dt = 0.5f;
  float dx = 1.0f;

  //initial and boundary values
  for (int i = 0; i < m_nbr_fdm; i++)
  {
    T_fdm_2[i] = 0;
  }
  T_fdm_2[0] = T_A;
  T_fdm_2[m_nbr_fdm - 1] = T_B;

  for (int i = 0; i < m_nbr_fdm; i++)
  {
    wxLogDebug("T[%d]=%.1f", i, T_fdm_2[i]);
  }

  int iter = 500;
  float time = dt * (float)iter;
  float U = 1.0f; //velocity
  float C = U * dt / dx; //courant number <= 1
  wxLogDebug("time=%.1f F=%.4f", time, C);
  for (int n = 0; n < iter; n++)
  {
    for (int i = 1; i < m_nbr_fdm - 1; i++)
    {
      Tn_fdm_2[i] = T_fdm_2[i] - C * (T_fdm_2[i] - T_fdm_2[i - 1]);
    }
    for (int i = 1; i < m_nbr_fdm - 1; i++)
    {
      T_fdm_2[i] = Tn_fdm_2[i];
    }
    for (int i = 0; i < m_nbr_fdm; i++)
    {
      wxLogDebug("T[%d]=%.1f", i, T_fdm_2[i]);
    }
  }
}


#pragma once

#include <Eigen/Dense>

namespace hacky_toolkit {

//! \brief A Plane (or hyperplane) is a susbspace of which its dimension is one
//! less than of its ambient space. The template argument of the dimension of
//! the plane equals that of its ambient space. I.e., a Plane<float, 2> is a
//! line in a 2d space. A plane is represented by a vector of Dim + 1 parameters
//! that can be used to obtain the general form of the plane equation:
//! a.dot((x, 1)) + d = 0
template <typename Scalar_, int Dim_>
class Plane {
  static_assert(Dim_ > 0, "DYNAMIC_DIMENSION_NOT_SUPPORTED");

 public:
  inline Plane() = default;

  template <typename Derived>
  inline Plane(Eigen::MatrixBase<Derived> const& p) : a_(p) {}

  template <typename Derived>
  inline Plane(Eigen::MatrixBase<Derived> const& normal, Scalar_ d) {
    a_.template head<Dim_>() = normal;
    a_(Dim_) = d;
  }

  //! \brief Creates a normalized plane using two points. The spatial dimension
  //! of the Plane must equal 2 when using this constructor. Input points can be
  //! of dimension 2 or 3.
  template <typename Derived0, typename Derived1>
  inline Plane(
      Eigen::MatrixBase<Derived0> const& x,
      Eigen::MatrixBase<Derived1> const& y) {
    static_assert(Dim_ == 2);
    static_assert(
        Derived0::SizeAtCompileTime == Dim_ ||
        Derived0::SizeAtCompileTime == Dim_ + 1);
    static_assert(Derived0::SizeAtCompileTime == Derived1::SizeAtCompileTime);
    if constexpr (Derived0::SizeAtCompileTime == Dim_) {
      a_ = x.homogeneous().cross(y.homogeneous());
    } else {
      a_ = x.cross(y);
    }
    Normalize();
  }

  //! \brief Creates a normalized plane using three points. The spatial
  //! dimension of the Plane must equal 3 when using this constructor. Input
  //! points can be of dimension 3 or 4.
  template <typename Derived0, typename Derived1, typename Derived2>
  inline Plane(
      Eigen::MatrixBase<Derived0> const& x,
      Eigen::MatrixBase<Derived1> const& y,
      Eigen::MatrixBase<Derived2> const& z) {
    static_assert(Dim_ == 3);
    static_assert(
        Derived0::SizeAtCompileTime == Dim_ ||
        Derived0::SizeAtCompileTime == Dim_ + 1);
    static_assert(
        Derived0::SizeAtCompileTime == Derived1::SizeAtCompileTime &&
        Derived1::SizeAtCompileTime == Derived2::SizeAtCompileTime);
    if constexpr (Derived0::SizeAtCompileTime == Dim_) {
      a_.template head<3>() = (y - x).cross(z - x);
      a_(Dim_) = -x.dot(a_.template head<3>());
    } else {
      auto xn = x.hnormalized().eval();
      a_.template head<3>() =
          (y.hnormalized() - xn).cross(z.hnormalized() - xn);
      a_(Dim_) = -xn.dot(a_.template head<3>());
    }
    Normalize();
  }

  template <typename Derived>
  inline Scalar_ SignedDistance(Eigen::MatrixBase<Derived> const& x) const {
    static_assert(
        Derived::SizeAtCompileTime == Dim_ ||
        Derived::SizeAtCompileTime == Dim_ + 1);
    if constexpr (Derived::SizeAtCompileTime == Dim_) {
      return a_.dot(x.homogeneous());
    } else {
      return a_.dot(x);
    }
  }

  template <typename Derived>
  inline Scalar_ Distance(Eigen::MatrixBase<Derived> const& x) const {
    return std::abs(SignedDistance(x));
  }

  template <typename Derived>
  inline bool Positive(Eigen::MatrixBase<Derived> const& x) const {
    return SignedDistance(x) > Scalar_(0.0);
  }

  template <typename Derived>
  inline bool Negative(Eigen::MatrixBase<Derived> const& x) const {
    return SignedDistance(x) < Scalar_(0.0);
  }

  //! \brief Tests is a point is considered to lie on the plane.
  //! \returns true if \p x is on the plane.
  template <typename Derived>
  inline bool Test(
      Eigen::MatrixBase<Derived> const& x,
      Scalar_ threshold = Eigen::NumTraits<Scalar_>::dummy_precision()) const {
    return Distance(x) < threshold;
  }

  //! \brief Normalizes the plane such that the length of the normal (the first
  //! Dim parameters of a()) equals 1.
  inline void Normalize() { a_ /= a_.template head<Dim_>().norm(); }

  //! \brief Point on the plane closest to the origin.
  inline Eigen::Matrix<Scalar_, Dim_, 1> Origin() const {
    return normal() * -d();
  }

  //! \brief Generates a random point on the unit hypersphere of the subspace
  //! defined by this plane at a distance of \p distance from the point returned
  //! by the Origin() method. The plane does not have to be normalized.
  //! \returns A random point on the plane.
  inline Eigen::Matrix<Scalar_, Dim_, 1> RandomPoint(
      Scalar_ distance = Scalar_(1.0)) const {
    // Perturb the normal with some noise.
    // What are the odds that the added noise equals 0?
    Eigen::Matrix<Scalar_, Dim_, 1> v =
        normal() + Eigen::Matrix<Scalar_, Dim_, 1>::Random();
    // Apply Gram-Schmidt process to get an orthogonal vector. Ie, subtract the
    // projection of v on the normal from v.
    return (v - normal() * normal().dot(v) / normal().squaredNorm())
                   .normalized() *
               distance +
           Origin();
  }

  //! \brief The orientation of the plane is defined by its normal. Not
  //! guaranteed to be normalized.
  //! \returns The plane normal.
  inline auto normal() const {
    // Eigen::VectorBlock<VectorType const, Dim_>
    return a_.template head<Dim_>();
  }

  //! \brief The negative of the distance from the origin to the plane in the
  //! direction of the normal. That is, the closest point on the plane to the
  //! origin equals:
  //! \code
  //! plane.normal() * -plane.d()
  //! \endcode
  //! \returns Negative distance from the origin to the plane.
  Scalar_ const& d() const { return a_(Dim_); }

  //! \brief The parameters of the plane. They describe the General form of the
  //! plane equation: a.dot((x, 1)) = 0
  auto const& a() const { return a_; }

  //! \brief Generates a Plane with a random normal at a distance somewhere in
  //! the range of [-max_distance:max_distance].
  static Plane Random(Scalar_ max_distance = Scalar_(1.0)) {
    return {
        Eigen::Matrix<Scalar_, Dim_, 1>::Random().normalized(),
        Eigen::Matrix<Scalar_, 1, 1>::Random()(0) * max_distance};
  }

 private:
  Eigen::Matrix<Scalar_, Dim_ + 1, 1> a_;
};

using Plane2d = Plane<double, 2>;
using Plane3d = Plane<double, 3>;

using Plane2f = Plane<float, 2>;
using Plane3f = Plane<float, 3>;

}  // namespace hacky_toolkit


#pragma once

#include <Eigen/Core>
#include <deal.II/base/index_set.h>
#include <cmath>

#include "base/numbers.hpp"
#include "enum/enum.hpp"
#include "quadrature/qmidpoint.hpp"
#include "quadrature/quad_handler1d.hpp"
#include "quadrature/tensor_product_quadrature.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

namespace boltzmann {
class Momentum
{
 private:
  typedef Eigen::Vector2d value_t;

 public:
  Momentum() {}

  template <typename SPECTRAL_BASIS>
  Momentum(const SPECTRAL_BASIS& spectral_basis)
  {
    init(spectral_basis);
  }

  template <typename SPECTRAL_BASIS>
  void init(const SPECTRAL_BASIS& spectral_basis);

  /**
   * @brief compute momentum,
   *
   * @param dst
   * @param src
   * @param count #physical dofs
   */
  template <typename NUMERIC_T>
  void compute(NUMERIC_T* dstx, NUMERIC_T* dsty, const NUMERIC_T* src, int count) const;

  template <typename VEC_IN, typename VEC_OUT, typename INDEXER>
  void compute(VEC_OUT& dstx,
               VEC_OUT& dsty,
               const VEC_IN& src,
               const dealii::IndexSet& relevant_dofs,
               const INDEXER& indexer) const;

  template <typename NUMERIC_T>
  value_t compute(const NUMERIC_T* src) const;

 private:
  typedef std::pair<unsigned int, value_t> entry_t;

 private:
  unsigned int n_velo_dofs;
  std::vector<entry_t> contributions;

 public:
  const std::vector<entry_t>& entries() const { return contributions; }
};

// ------------------------------------------------------------
template <typename SPECTRAL_BASIS>
void
Momentum::init(const SPECTRAL_BASIS& spectral_basis)
{
  n_velo_dofs = spectral_basis.n_dofs();

  typedef typename std::tuple_element<1, typename SPECTRAL_BASIS::elem_t::container_t>::type
      radial_elem_t;

  typedef typename std::tuple_element<0, typename SPECTRAL_BASIS::elem_t::container_t>::type
      angular_elem_t;

  auto& QR_handler = QuadHandler<MaxwellQuadrature>::GetInstance();
  typedef QuadAdaptor<MaxwellQuadrature> qr_adapt;

  unsigned int nqR = spectral::get_max_k(spectral_basis) + 2;
  Eigen::VectorXd pts_qR(nqR);
  Eigen::VectorXd wts_qR(nqR);

  qr_adapt::apply(pts_qR.data(), wts_qR.data(), QR_handler.get(nqR), 0.5);

  typedef typename SPECTRAL_BASIS::elem_t elem_t;
  for (auto it = spectral_basis.begin(); it != spectral_basis.end(); ++it) {
    typename elem_t::Acc::template get<radial_elem_t> getter;
    typename elem_t::Acc::template get<angular_elem_t> geta;

    const auto& rr = getter(*it);

    if (geta(*it).get_id().l != 1) continue;

    // make sure we are using the right basis
    const double w = rr.w();
    if (abs(0.5 - w) > 1e-10) throw std::runtime_error("Error: wrong weight in Momentum::init");

    value_t sum;
    sum[0] = 0.0;
    sum[1] = 0.0;
    if (geta(*it).get_id().t == TRIG::SIN) {
      // sin
      for (unsigned int q = 0; q < pts_qR.size(); ++q) {
        const double r = pts_qR[q];
        sum[TRIG::SIN] += rr.evaluate(r) * r * wts_qR[q];
      }
    } else if (geta(*it).get_id().t == TRIG::COS) {
      for (unsigned int q = 0; q < pts_qR.size(); ++q) {
        const double r = pts_qR[q];
        sum[TRIG::COS] += rr.evaluate(r) * r * wts_qR[q];
      }
    }

    const unsigned int i = it - spectral_basis.begin();
    contributions.push_back(std::make_pair(i, numbers::PI * sum));
  }
}

// // ------------------------------------------------------------
template <typename NUMERIC_T>
void
Momentum::compute(NUMERIC_T* dstx, NUMERIC_T* dsty, const NUMERIC_T* src, int count) const
{
#pragma omp parallel for
  for (int i = 0; i < count; ++i) {
    const NUMERIC_T* local_src = src + i * n_velo_dofs;
    value_t sum;
    sum[0] = 0;
    sum[1] = 0;
    for (unsigned int j = 0; j < contributions.size(); ++j) {
      sum += contributions[j].second * local_src[contributions[j].first];
    }
    dstx[i] = sum[0];
    dsty[i] = sum[1];
  }
}

// ------------------------------------------------------------
template <typename NUMERIC_T>
Momentum::value_t
Momentum::compute(const NUMERIC_T* src) const
{
  value_t sum;
  sum[0] = 0;
  sum[1] = 0;
  for (unsigned int j = 0; j < contributions.size(); ++j) {
    sum += contributions[j].second * src[contributions[j].first];
  }
  return sum;
}

// // ------------------------------------------------------------
template <typename VEC_IN, typename VEC_OUT, typename INDEXER>
void
Momentum::compute(VEC_OUT& dstx,
                  VEC_OUT& dsty,
                  const VEC_IN& src,
                  const dealii::IndexSet& relevant_dofs,
                  const INDEXER& indexer) const
{
#pragma omp parallel for
  for (unsigned int ix = 0; ix < relevant_dofs.size(); ++ix) {
    if (relevant_dofs.is_element(ix)) {
      value_t sum;
      sum[0] = 0;
      sum[1] = 0;
      for (unsigned int j = 0; j < contributions.size(); ++j) {
        unsigned int jx = contributions[j].first;
        sum += contributions[j].second * src[indexer.to_global(ix, jx)];
      }
      dstx[ix] = sum[0];
      dsty[ix] = sum[1];
    }
  }
}

}  // end namespace boltzmann


#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
#include <boost/concept_check.hpp>

using namespace std;
using namespace g2o;
//Vertex数量n,这n个Vertex存储的数据类型
class powell_vertex: public BaseVertex<4, Eigen::Matrix<double,1,4>> {
	virtual void oplusImpl(const double* v) {
		//base_vertex.h: EstimateType _estimate;
		//cout << "oplus: "  << endl;
		//cout << Eigen::Matrix<double,1,4>(v) << endl;
		_estimate += Eigen::Matrix<double,1,4>(v);
		//cout << "_estimate: " << _estimate << endl;
	}
        virtual void setToOriginImpl() {
		//cout << "initial _estimate" << endl;
		_estimate << 0.5,1.5,2.5,3.5;
	}

	virtual bool read(std::istream& is) {}
        virtual bool write(std::ostream& os) const {}	//const 不能忽略
};
//e(xi)的维度、类型; vertex的类型
class powell_edge: public BaseUnaryEdge<4, Eigen::Matrix<double,1,4>, powell_vertex> {
	virtual void computeError() {
		//vertex* vtx = new vertex;
		const powell_vertex* v = static_cast<const powell_vertex*> (_vertices[0]);
		Eigen::Matrix<double,1,4> est =  v->estimate();
		//cout << "est: " << est(0,0)  << "---" << est(0,1)  <<"---" << est(0,2) <<  "---"<< est(0,3) << endl;
// 		_error(0,0) = (&_measurement)[0] - (est(0,0) + 10*est(0,1));
// 		_error(0,1) = (&_measurement)[1] - sqrt(5)*(est(0,2) - est(0,3));
// 		_error(0,2) = (&_measurement)[2] - (est(0,1) - 2*est(0,2))*(est(0,1) - 2*est(0,2));
// 		_error(0,3) = (&_measurement)[3] - sqrt(10)*(est(0,0) - est(0,2))*(est(0,0) - est(0,2));
		_error(0,0) = _measurement(0,0)- (est(0,0) + 10*est(0,1));
		_error(1,0) = _measurement(0,1) - sqrt(5)*(est(0,2) - est(0,3));
		_error(2,0) = _measurement(0,2) - (est(0,1) - 2*est(0,2))*(est(0,1) - 2*est(0,2));
		_error(3,0) = _measurement(0,3) - sqrt(10)*(est(0,0) - est(0,2))*(est(0,0) - est(0,2));
		cout << "_error: " << _error.transpose() << endl;
	}

	virtual bool read(std::istream& is) {}
        virtual bool write(std::ostream& os) const {}	//const 不能忽略
};

int main(int argc, char **argv) {
	// 构建图优化，先设定g2o
	//注意维度对应--边--顶点---
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<4,4> > Block;  // int _PoseDim, int _LandmarkDim
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
	Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
	// 梯度下降方法，从GN, LM, DogLeg 中选
	cout << "config blocksolver" << endl;
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
	// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
	// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm( solver );   // 设置求解器
	optimizer.setVerbose( true );       // 打开调试输出
	cout << "add Vertex" << endl;
	// 往图中增加顶点
	powell_vertex* v = new powell_vertex;
	Eigen::Matrix<double,1,4> estimate_initial;
	estimate_initial <<0.5,1.5,2.5,3.5;
	v->setEstimate(estimate_initial);
	v->setId(0);
	optimizer.addVertex( v );
	cout << "add Edge" << endl;
	//加边
	powell_edge* eg = new powell_edge;
	//double measurement_initial[4] = {21,-sqrt(5),16,9*sqrt(10)};
	//for (int i=0;i<4;i++) {
		eg->setId(0);
		eg->setVertex( 0, v );                // 设置连接的顶点
		Eigen::Matrix<double,1,4> measurement_initial;
		measurement_initial << 21,-sqrt(5),16,9*sqrt(10);
		eg->setMeasurement(measurement_initial);      // 观测数值
		//这个'4'对应于powell_edge第一个参数
		eg->setInformation(Eigen::Matrix<double,4,4>::Identity()*1/0.25); // 信息矩阵：协方差矩阵之逆
		optimizer.addEdge( eg );
	//}
	// 执行优化
	cout<<"start optimization"<<endl;
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	cout<<"initial optimization"<<endl;
	optimizer.initializeOptimization();
	cout<<"start"<<endl;
	optimizer.optimize(200);
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
	cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

	// 输出优化值
	Eigen::Matrix<double,1,4> abc_estimate = v->estimate();
	cout<<"estimated value: "<<abc_estimate.transpose()<<endl;

    return 0;
}


/**
 * @ file PointEvaluationRhs_main.cc
 * @ brief NPDE homework PointEvaluationRhs code
 * @ author Christian Mitsch, Liaowang Huang (refactoring)
 * @ date 22/03/2019, 06/01/2020 (refactoring)
 * @ copyright Developed at ETH Zurich
 */

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/mesh_hierarchy.h>

#include <Eigen/Core>
#include <iomanip>
#include <memory>
#include <sstream>
#include <utility>
#include <vector>

#include "pointevaluationrhs.h"

int main() {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(1, 1.0);

  // Start of numerical experiment
  std::vector<double> dof_a{};
  std::vector<double> l2_a{};
  std::vector<double> h1_a{};
  Eigen::VectorXd sol_vec;

  // Runs with initial mesh
  lf::assemble::UniformFEDofHandler dofh_initial(
      mesh_p, {{lf::base::RefEl::kPoint(), 1}});
  auto result = PointEvaluationRhs::normsSolutionPointLoadDirichletBVP(
      dofh_initial, Eigen::Vector2d(1.3, 1.7), sol_vec);
  unsigned N_dofs = dofh_initial.NumDofs();
  dof_a.push_back(N_dofs);
  l2_a.push_back(result.first);
  h1_a.push_back(result.second);

  // Necessary for regular refinement
  std::unique_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory2 =
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::refinement::MeshHierarchy my_hierarchy(mesh_p, std::move(mesh_factory2));

  for (int k = 1; k < 7; k++) {
    my_hierarchy.RefineRegular();

    mesh_p = my_hierarchy.getMesh(k);
    lf::assemble::UniformFEDofHandler dofh(mesh_p,
                                           {{lf::base::RefEl::kPoint(), 1}});
    unsigned N_dofs = dofh.NumDofs();
    dof_a.push_back(N_dofs);
    sol_vec.resize(N_dofs);

    result = PointEvaluationRhs::normsSolutionPointLoadDirichletBVP(
        dofh, Eigen::Vector2d(1.3, 1.7), sol_vec);
    l2_a.push_back(result.first);
    h1_a.push_back(result.second);
    // Write vtk file
    std::stringstream filename;
    filename << "rhseval" << k << ".vtk";
    lf::io::VtkWriter vtk_writer(mesh_p, filename.str());
    // need the newest pointer
    auto mds = lf::mesh::utils::make_CodimMeshDataSet<double>(mesh_p, 2);
    for (auto *node : mesh_p->Entities(2)) {
      mds->operator()(*node) = sol_vec(dofh.GlobalDofIndices(*node)[0]);
    }
    vtk_writer.WritePointData("solution_data", *mds);
  }

  // Print to std output
  std::cout << " dof      l2         h1 " << std::endl;
  for (int i = 0; i < dof_a.size(); i++) {
    std::cout << std::setw(5) << dof_a.at(i) << "   " << std::setw(5)
              << l2_a.at(i) << "   " << std::setw(5) << h1_a.at(i) << std::endl;
  }
}


//   Boost pow.hpp header file
//   Computes a power with exponent known at compile-time

//  (C) Copyright Bruno Lalande 2008.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_MATH_POW_HPP
#define BOOST_MATH_POW_HPP

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/policies/policy.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/mpl/greater_equal.hpp>

namespace boost {
namespace math {

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4702) // Unreachable code, only triggered in release mode and /W4
#endif

namespace detail {

template <int N, int M = N%2>
struct positive_power
{
    template <typename T>
    static T result(T base)
    {
        T power = positive_power<N/2>::result(base);
        return power * power;
    }
};

template <int N>
struct positive_power<N, 1>
{
    template <typename T>
    static T result(T base)
    {
        T power = positive_power<N/2>::result(base);
        return base * power * power;
    }
};

template <>
struct positive_power<1, 1>
{
    template <typename T>
    static T result(T base){ return base; }
};

template <int N, bool>
struct power_if_positive
{
    template <typename T, class Policy>
    static T result(T base, const Policy&)
    { return positive_power<N>::result(base); }
};

template <int N>
struct power_if_positive<N, false>
{
    template <typename T, class Policy>
    static T result(T base, const Policy& policy)
    {
        if (base == 0)
        {
            return policies::raise_overflow_error<T>(
                       "boost::math::pow(%1%)",
                       "Attempted to compute a negative power of 0",
                       policy
                   );
        }

        return T(1) / positive_power<-N>::result(base);
    }
};

template <>
struct power_if_positive<0, true>
{
    template <typename T, class Policy>
    static T result(T base, const Policy& policy)
    {
        if (base == 0)
        {
            return policies::raise_indeterminate_result_error<T>(
                       "boost::math::pow(%1%)",
                       "The result of pow<0>(%1%) is undetermined",
                       base,
                       T(1),
                       policy
                   );
        }

        return T(1);
    }
};

template <int N>
struct select_power_if_positive
{
    typedef typename mpl::greater_equal<
                         boost::integral_constant<int, N>,
                         boost::integral_constant<int, 0>
                     >::type is_positive;

    typedef power_if_positive<N, is_positive::value> type;
};

}  // namespace detail

template <int N, typename T, class Policy>
inline typename tools::promote_args<T>::type pow(T base, const Policy& policy)
{
   typedef typename tools::promote_args<T>::type result_type;
   return detail::select_power_if_positive<N>::type::result(static_cast<result_type>(base), policy);
}

template <int N, typename T>
inline typename tools::promote_args<T>::type pow(T base)
{ return pow<N>(base, policies::policy<>()); }

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

}  // namespace math
}  // namespace boost

#endif


#include <boost/functional/hash.hpp>
#include <unordered_map>
#include <memory>
#include "MPIProvider.hpp"
#include "BayesOptVQEBackend.hpp"
#include <boost/math/constants/constants.hpp>

namespace xacc {
namespace vqe {

const VQETaskResult BayesOptVQEBackend::minimize(Eigen::VectorXd parameters) {

	VQETaskResult r;
	const int dim = parameters.size();
	auto pi = boost::math::constants::pi<double>();
	auto computeTask = std::make_shared<ComputeEnergyVQETask>(program);
	bayesopt::Parameters par;
	par = initialize_parameters_to_default();
	par.n_iterations = xacc::optionExists("bo-n-iter") ? std::stoi(xacc::getOption("bo-n-iter")) : par.n_iterations;
	par.noise = xacc::optionExists("bo-noise") ? std::stod(xacc::getOption("bo-noise")) : par.noise;
	par.l_type = xacc::optionExists("bo-learn-type") ? str2learn(xacc::getOption("bo-learn-type").c_str()) : par.l_type;
	par.n_init_samples = xacc::optionExists("bo-n-init-iter") ? std::stoi(xacc::getOption("bo-n-init-iter")) : par.n_init_samples;
	par.verbose_level = xacc::optionExists("bo-verbose-level") ? std::stoi(xacc::getOption("bo-verbose-level")) : par.verbose_level;
	par.epsilon = xacc::optionExists("bo-epsilon") ? std::stod(xacc::getOption("bo-epsilon")) : par.epsilon;
	par.crit_name = xacc::optionExists("bo-crit-name") ? std::string(xacc::getOption("bo-crit-name")) : par.crit_name;
	par.kernel.name = xacc::optionExists("bo-kernel-name") ? std::string(xacc::getOption("bo-kernel-name")) : par.kernel.name;
	par.sc_type = xacc::optionExists("bo-score-type") ? str2score(xacc::getOption("bo-score-type").c_str()) : par.sc_type;
	par.surr_name = xacc::optionExists("bo-surrogate-name") ? std::string(xacc::getOption("bo-surrogate-name")) : par.surr_name;
	par.mean.name = xacc::optionExists("bo-mean-name") ? std::string(xacc::getOption("bo-mean-name")) : par.mean.name;
	//par.kernel.hp_mean = xacc::optionExists("bo-kernel-mean") ? std::stod(xacc::getOption("bo-kernel-mean")) : par.kernel.hp_mean;
	//par.kernel.hp_std = xacc::optionExists("bo-kernel-std") ? std::stod(xacc::getOption("bo-kernel-std")) : par.kernel.hp_std;
	VQEBayesOptFunction f(par, computeTask, dim);

	// Map parameters to boost vector

	boost::numeric::ublas::vector<double> result(dim);
	double* p = parameters.data();
	std::copy(result.begin(), result.end(), p);

	boost::numeric::ublas::vector<double> lowerBound(dim);
	boost::numeric::ublas::vector<double> upperBound(dim);
	for (int i = 0; i < dim; i++) {lowerBound[i] = -1*pi;upperBound[i] = pi;}

	f.setBoundingBox(lowerBound,upperBound);

	f.optimize(result);

	r.energy = f.getValueAtMinimum();
	auto resultAngles = f.getFinalResult();
	const double * data = &(resultAngles.data()[0]);
  r.angles = Eigen::Map<const Eigen::VectorXd>(data, parameters.size());

	return r;

}

}
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/eigen/matrix.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<double> vector;
    typedef ublas::matrix<double, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<double>::reset();
    size_type m=6, n=8;
    matrix A(m, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<m; ++i)
  	A(i, j)=rand_normal<double>::get();
    vector x(m);
    for (size_type i=0; i<m; ++i)
      x(i)=rand_normal<double>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<double>::get();
    double alpha(rand_normal<double>::get());
    matrix A1(alpha*ublas::outer_prod(x, y)+A);
    matrix A2(A);
    blas::gerh(alpha, x, y, A2);
    std::cout << print_mat(A1) << '\n'
  	      << print_mat(A2) << '\n';
  }
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type m=6, n=8;
    matrix A(m, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<m; ++i)
 	A(i, j)=rand_normal<complex>::get();
    vector x(m);
    for (size_type i=0; i<m; ++i)
      x(i)=rand_normal<complex>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<complex>::get();
    complex alpha(rand_normal<complex>::get());
    matrix A1(alpha*ublas::outer_prod(x, ublas::conj(y))+A);
    matrix A2(A);
    blas::gerh(alpha, x, y, A2);
    std::cout << print_mat(A1) << '\n'
	      << print_mat(A2) << '\n';
  }
  return EXIT_SUCCESS;
}


#pragma once

#include <polyvec/core/macros.hpp>
#include <polyvec/core/types.hpp>
#include <polyvec/utils/num.hpp>
#include <polyvec/geometry/path.hpp>
#include <polyvec/utils/matrix.hpp>

#include <Eigen/Geometry>

#define PF_RAD(deg) ((double)deg * M_PI / 180.)
#define PF_DEG(rad) ((double)rad * 180. / M_PI)
#define PF_ISNAN(v) (!((v) == (v)))

NAMESPACE_BEGIN(polyfit)
NAMESPACE_BEGIN(AngleUtils)

/*
	Returns true if both points are in the same quadrant. The test is inclusive
	if any of the two points lie exactly along one of the axis.
*/
PV_INLINE bool lie_in_the_same_quadrant(
	const vec2& p0,
	const vec2& p1
);

PV_INLINE double deviation_from_horizontal(const vec2& d);
PV_INLINE double deviation_from_horizontal(double angle);

PV_INLINE double deviation_from_vertical(const vec2& d);
PV_INLINE double deviation_from_vertical(double angle);

PV_INLINE double spanned_shortest(const vec2 & p0, const vec2 & p1, const vec2 & p2);

PV_INLINE double spanned_shortest_between(const vec2 & d0, const vec2 & d1);

PV_INLINE double spanned_clockwise_between(const vec2& d0, const vec2& d1);

PV_INLINE bool have_opposite_convexity(const vec2 & p0, const vec2 & p1, const vec2 & p2, const vec2 & p3);

PV_INLINE bool have_opposite_convexity_with_tol(const vec2 & p0, const vec2 & p1, const vec2 & p2, const vec2 & p3);

PV_INLINE int count_inflections(const mat2x & points, bool circular = true);

PV_INLINE bool is_flat(const vec2 pp, const vec2 p, const vec2 pn);

PV_INLINE int convexity(const vec2 pp, const vec2 p, const vec2 pn, int orientation);

PV_INLINE int number_of_neighbors_with_different_convexity(const Eigen::Matrix2Xd& polygon, int corner);

// convexity should be writable for 1 + 2 * neighborhood_size elements
// the polygon should contain enough elements before and after the first/last neighboring corners to be able to compute the convexity
PV_INLINE void neighborhood_convexity(const Eigen::Matrix2Xd& P, const int corner, const int neighborhood_size, int* convexity);

/*
	Returns the number of edges in P which overlap 1-pixel axis-aligned steps in B.

	B is assumed to be the raster boundary which P is approximating, though there are
	no requirements on the points except that they should have the same ordering.
*/
PV_INLINE int count_visually_degenerate_edges(
	const mat2x& B,
	const vecXi& P,
	const bool circular = false
);

PV_INLINE int count_visually_inconsistent_edges(const mat2x & P, bool circular = false);

#include "angle.inl"

NAMESPACE_END(AngleUtils)
NAMESPACE_END(polyfit)


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * Redistributions in binary form must reproduce the above copyright        *
 *   notice, this list of conditions and the following disclaimer in the      *
 *   documentation and/or other materials provided with the distribution.     *
 * * Neither the name of ANU nor the names of its contributors may be         *
 *   used to endorse or promote products derived from this software without   *
 *   specific prior written permission.                                       *
 *                                                                            *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
 * SUCH DAMAGE.                                                               *
 ******************************************************************************/

/**
 * \file arun.hpp
 * \brief Arun's method for computing the rotation between two point sets.
 */

#ifndef OPENGV_ARUN_HPP_
#define OPENGV_ARUN_HPP_

#include <stdlib.h>
#include <Eigen/Eigen>
#include <opengv/types.hpp>

/**
 * \brief The namespace of this library.
 */
namespace opengv
{
/**
 * \brief The namespace of the math tools.
 */
namespace math
{

/**
 * \brief Arun's method for computing the rotation between two point sets.
 *        Core function [13].
 *
 * \param[in] Hcross The summation over the exterior products between the
 *            normalized points.
 * \return The rotation matrix that aligns the points.
 */
rotation_t arun( const Eigen::MatrixXd & Hcross );

/**
 * \brief Arun's method for complete point cloud alignment [13]. The method
 *        actually does the same than threept_arun, but has a different
 *        interface.
 *
 * \param[in] p1 The points expressed in the first frame.
 * \param[in] p2 The points expressed in the second frame.
 * \return The Transformation from frame 2 to frame 1 (
 *         \f$ \mathbf{T} = \left(\begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array}\right) \f$,
 *         with \f$ \mathbf{t} \f$ being the position of frame 2 seen from
 *         frame 1, and \f$ \mathbf{R} \f$ being the rotation from
 *         frame 2 to frame 1).
 */
transformation_t arun_complete( const points_t & p1, const points_t & p2 );

}
}

#endif /* OPENGV_ARUN_HPP_ */


/*!
* Copyright 2007  Technical University of Catalonia
*
* 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)
*
*  Authors: Dmitry Bufistov
*           Andrey Parfenov
*/
#include <boost/graph/howard_cycle_ratio.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/graph/random.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

using namespace boost;
typedef adjacency_list<listS, listS, directedS, property<vertex_index_t, int, property<boost::vertex_name_t, std::string> >,
        property<edge_weight_t, double, property<edge_weight2_t, double, property<edge_index_t, int> > > > grap_real_t;

template <typename TGraph>
void gen_rand_graph(TGraph& g, size_t nV, size_t nE)
{
        g.clear();
        boost::mt19937 rng;
        boost::generate_random_graph(g, nV, nE, rng, true, true);
        boost::uniform_real<> ur(-1,10);
        boost::variate_generator<boost::mt19937&, boost::uniform_real<> >       ew1rg(rng, ur);
        randomize_property<edge_weight_t>(g, ew1rg);
        boost::uniform_int<> uint(1,5);
        boost::variate_generator<boost::mt19937&, boost::uniform_int<> >        ew2rg(rng, uint);
        randomize_property<edge_weight2_t>(g, ew2rg);
}

int main(int argc, char* argv[])
{
        const double epsilon = 0.000000001;
        double min_cr, max_cr; ///Minimum and maximum cycle ratio
        typedef std::vector<graph_traits<grap_real_t>::edge_descriptor> ccReal_t;
        ccReal_t cc; ///For storing critical edges

        grap_real_t tgr;
        property_map<grap_real_t, vertex_index_t>::type vim = get(vertex_index, tgr);
        property_map<grap_real_t, edge_weight_t>::type ew1m = get(edge_weight, tgr);
        property_map<grap_real_t, edge_weight2_t>::type ew2m = ew2m;

        gen_rand_graph(tgr, 1000, 300000);
        std::cout << "Vertices number: " << num_vertices(tgr) << '\n';
        std::cout << "Edges number: " << num_edges(tgr) << '\n';
        int i = 0;
        BGL_FORALL_VERTICES(vd, tgr, grap_real_t) put(vertex_index, tgr, vd, i++); ///Initialize vertex index property
        boost::posix_time::ptime        st = boost::posix_time::microsec_clock::local_time();
        max_cr = maximum_cycle_ratio(tgr, get(vertex_index, tgr), get(edge_weight, tgr), get(edge_weight2, tgr));
        std::cout << "Maximum cycle ratio is " << max_cr << '\n';
        std::cout << "Run time of the maximum_cycle_ratio() is " << to_simple_string(boost::posix_time::microsec_clock::local_time() - st) << '\n';

        ///One way to get the "good" value of the plus_infinity parameter
        double pl_infnt = double(*std::max_element(get_property_iter_range(tgr, edge_weight).first, get_property_iter_range(tgr, edge_weight).second)) /
                *std::min_element(get_property_iter_range(tgr, edge_weight2).first, get_property_iter_range(tgr, edge_weight2).second);
        std::cout << "Set infinity for minimum_cycle_ratio() call to " << pl_infnt << '\n';
        i = 0;
        BGL_FORALL_EDGES(ed, tgr, grap_real_t) put(edge_index, tgr, ed, i++); ///Initialize edge index property
        min_cr = minimum_cycle_ratio(tgr, get(vertex_index, tgr), get(edge_weight, tgr), get(edge_weight2, tgr), get(edge_index, tgr), &cc, pl_infnt);
        std::cout << "Minimal cycle ratio is " << min_cr << '\n';
        std::pair<double, double> cr(.0,.0);
        std::cout << "\nCritical cycle is:\n";
        for (ccReal_t::iterator itr = cc.begin(); itr != cc.end(); ++itr)
        {
                cr.first += get(edge_weight, tgr, *itr); cr.second += get(edge_weight2, tgr, *itr);
                std::cout << "(" << get(vertex_index, tgr, source(*itr, tgr)) << "," << get(vertex_index, tgr, target(*itr, tgr)) << ") ";
        }
        std::cout << '\n';
        assert(std::abs(cr.first / cr.second - min_cr) < epsilon);

        return 0;
}


/*!
 * Distance.h
 *
 *  Created on: 2012/12/29
 *  \author Hiroki Sudo
 *
 */

#ifndef DISTANCE_H_
#define DISTANCE_H_

#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/shared_ptr.hpp>
#include "Element.h"

/*!
 *
 */
namespace {
/*!
 * Distance
 */
namespace Distance {

/*!
 * ElementクラスのVectorインスタンスを引数に2つ取り、それぞれの編集距離(Levenstein距離)を計算します。
 * 返値は引数に渡されるVectorインスタンスの要素数で割られるため、0~1の値を取ります。
 * 値が大きければ距離は離れていて、小さければ距離が近いと言う意味になります。計算量はO((n*m)^(1/2))
 */
template<class E>
double levenstein(std::vector<E> ex1, std::vector<E> ex2) {

	boost::numeric::ublas::matrix<int> matrix(0, 0);
	int col_size, row_size;
	int cost_delta, cost1, cost2, cost3, cost;

	if (ex1.size() == 0 && ex2.size() != 0)
		return ex2.size();
	if (ex1.size() != 0 && ex2.size() == 0)
		return ex1.size();
	if (ex1.size() == 0 && ex2.size() == 0)
		return 0;

	row_size = ex1.size() + 1;
	col_size = ex2.size() + 1;

	matrix.resize(row_size, col_size);

	for (int j = 0; j < col_size; j++) {
		matrix(0, j) = j;
	}
	for (int i = 0; i < row_size; i++) {
		matrix(i, 0) = i;
	}

	for (int index_ex1 = 0; index_ex1 < ex1.size(); index_ex1++) {
		for (int index_ex2 = 0; index_ex2 < ex2.size(); index_ex2++) {

			if (ex1[index_ex1].is_sym() && ex2[index_ex2].is_sym()) {
				cost_delta = ex1[index_ex1] == ex2[index_ex2] ? 0 : 1;
			} else if (ex1[index_ex1].is_cat() && ex2[index_ex2].is_cat()) {
				cost_delta = ex1[index_ex1].obj == ex2[index_ex2].obj ? 0 : 1;
			} else {
				cost_delta = 1;
			}

			int x, y;
			y = index_ex1 + 1;
			x = index_ex2 + 1;
			cost1 = matrix(y - 1, x) + 1;
			cost2 = matrix(y, x - 1) + 1;
			cost3 = matrix(y - 1, x - 1) + cost_delta;

			cost = cost1;
			cost = cost2 < cost ? cost2 : cost;
			cost = cost3 < cost ? cost3 : cost;

			matrix(y, x) = cost;
		}
	}

	int len;
	if (row_size > col_size)
		len = row_size;
	else
		len = col_size;

	return (matrix(matrix.size1() - 1, matrix.size2() - 1))
			/ (static_cast<double>(len));
}

/*!
 * Vectorインスタンスの要素数で割らないため，値は0~無限となるLevenstein距離
 */
template<class E>
double levenstein2(std::vector<E> ex1, std::vector<E> ex2) {

	boost::numeric::ublas::matrix<int> matrix(0, 0);
	int col_size, row_size;
	int cost_delta, cost1, cost2, cost3, cost;

	if (ex1.size() == 0 && ex2.size() != 0)
		return ex2.size();
	if (ex1.size() != 0 && ex2.size() == 0)
		return ex1.size();
	if (ex1.size() == 0 && ex2.size() == 0)
		return 0;

	row_size = ex1.size() + 1;
	col_size = ex2.size() + 1;

	matrix.resize(row_size, col_size);

	for (int j = 0; j < col_size; j++) {
		matrix(0, j) = j;
	}
	for (int i = 0; i < row_size; i++) {
		matrix(i, 0) = i;
	}

	for (int index_ex1 = 0; index_ex1 < ex1.size(); index_ex1++) {
		for (int index_ex2 = 0; index_ex2 < ex2.size(); index_ex2++) {

			if (ex1[index_ex1].is_sym() && ex2[index_ex2].is_sym()) {
				cost_delta = ex1[index_ex1] == ex2[index_ex2] ? 0 : 1;
			} else if (ex1[index_ex1].is_cat() && ex2[index_ex2].is_cat()) {
				cost_delta = ex1[index_ex1].obj == ex2[index_ex2].obj ? 0 : 1;
			} else {
				cost_delta = 1;
			}

			int x, y;
			y = index_ex1 + 1;
			x = index_ex2 + 1;
			cost1 = matrix(y - 1, x) + 1;
			cost2 = matrix(y, x - 1) + 1;
			cost3 = matrix(y - 1, x - 1) + cost_delta;

			cost = cost1;
			cost = cost2 < cost ? cost2 : cost;
			cost = cost3 < cost ? cost3 : cost;

			matrix(y, x) = cost;
		}
	}

	int len;
	if (row_size > col_size)
		len = row_size;
	else
		len = col_size;

	return (matrix(matrix.size1() - 1, matrix.size2() - 1));
}

template <class E>
int snake(int k, int y, std::vector<E >& ary1, std::vector<E >& ary2, int m, int n) {
	int x;
	x = y - k;
	while (x < m && y < n && ary1[x] == ary2[y]) {
		x++;
		y++;
	}

	return y;
}
/*!
 * O(np)法の実装
 */
template<class E>
double onp_lv(std::vector<E>& ary1, std::vector<E>& ary2, double limit = -1) {
	std::vector<E> temp;
	std::vector<int> fp;
	int size = 0;
	int delta = 0;
	int offset = 0;
	int n = 0;
	int m = 0;
	int p;
	int k;
	int i;

	if (ary1.size() > ary2.size()) {
		temp = ary1;
		ary1 = ary2;
		ary2 = temp;
	}

	m = ary1.size();
	n = ary2.size();

	offset = m + 1;
	delta = n - m;
	size = m + n + 3;

	if (limit == -1) {
		limit = 2 * m + delta;
	}

	fp = std::vector<int>(size, -1);
	p = -1;
	k = 0;

	while (fp[delta + offset] < n) {
		p++;

		if (2 * p + delta >= (limit) * ary2.size() ) {
			//std::cerr << "Cutted" << std::endl;
			return 1.0;
		}

		k = -p;
		while (k < delta) {
			fp[k + offset] = fp[k - 1 + offset] + 1 > fp[k + 1 + offset] ?
				snake(k, fp[k - 1 + offset] + 1, ary1, ary2, m, n) :
				snake(k, fp[k + 1 + offset], ary1, ary2, m, n);
			k++;
		}

		k = delta + p;
		while (k > delta) {
			fp[k + offset] = fp[k - 1 + offset] + 1 > fp[k + 1 + offset] ?
				snake(k, fp[k - 1 + offset] + 1, ary1, ary2, m, n) :
				snake(k, fp[k + 1 + offset], ary1, ary2, m, n);
			k--;
		}

		fp[delta + offset] = fp[delta - 1 + offset] + 1 > fp[delta + 1 + offset] ?
				snake(delta, fp[delta - 1 + offset] + 1, ary1, ary2, m,	n) :
				snake(delta, fp[delta + 1 + offset], ary1, ary2, m, n);
	}

	return (delta + 2 * p)/(static_cast<double>(ary2.size()));
}

template<class E>
double hamming(std::vector<E> ex1, std::vector<E> ex2){

    if(ex1.size()!=ex2.size()){
        std::cerr <<  "Happened error on hamming distance method." << std::endl;
        std::exit(0);
    }
    typename std::vector<E>::iterator ex1_it;

    ex1_it = ex1.begin();
    int cnt = 0;
    double ham_sum=0;
    for(; ex1_it != ex1.end(); ex1_it++) {

        if((*ex1_it)!=ex2[cnt])
            ham_sum+=1;

        cnt+=1;
    }
    return (ham_sum/(double)cnt);
}

} /*Distance*/
}
#endif /* DISTANCE_H_ */


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "Resize.hpp"

#include "TensorBufferArrayView.hpp"

#include <boost/numeric/conversion/cast.hpp>

#include <cmath>
#include <algorithm>

using namespace armnnUtils;

namespace armnn
{

namespace
{

inline float Lerp(float a, float b, float w)
{
    return w * b + (1.f - w) * a;
}

inline double EuclideanDistance(float Xa, float Ya, const unsigned int Xb, const unsigned int Yb)
{
    return std::sqrt(pow(Xa - boost::numeric_cast<float>(Xb), 2) + pow(Ya - boost::numeric_cast<float>(Yb), 2));
}

}// anonymous namespace

void Resize(Decoder<float>&   in,
            const TensorInfo& inputInfo,
            Encoder<float>&   out,
            const TensorInfo& outputInfo,
            DataLayoutIndexed dataLayout,
            armnn::ResizeMethod resizeMethod,
            bool alignCorners)
{
    // We follow the definition of TensorFlow and AndroidNN: the top-left corner of a texel in the output
    // image is projected into the input image to figure out the interpolants and weights. Note that this
    // will yield different results than if projecting the centre of output texels.

    const unsigned int batchSize = inputInfo.GetShape()[0];
    const unsigned int channelCount = inputInfo.GetShape()[dataLayout.GetChannelsIndex()];

    const unsigned int inputHeight = inputInfo.GetShape()[dataLayout.GetHeightIndex()];
    const unsigned int inputWidth = inputInfo.GetShape()[dataLayout.GetWidthIndex()];
    const unsigned int outputHeight = outputInfo.GetShape()[dataLayout.GetHeightIndex()];
    const unsigned int outputWidth = outputInfo.GetShape()[dataLayout.GetWidthIndex()];

    const unsigned int sizeOffset = resizeMethod == armnn::ResizeMethod::Bilinear && alignCorners ? 1 : 0;

    // How much to scale pixel coordinates in the output image, to get the corresponding pixel coordinates
    // in the input image.
    const float scaleY = boost::numeric_cast<float>(inputHeight - sizeOffset)
                       / boost::numeric_cast<float>(outputHeight - sizeOffset);
    const float scaleX = boost::numeric_cast<float>(inputWidth - sizeOffset)
                       / boost::numeric_cast<float>(outputWidth - sizeOffset);

    TensorShape inputShape =  inputInfo.GetShape();
    TensorShape outputShape =  outputInfo.GetShape();

    for (unsigned int n = 0; n < batchSize; ++n)
    {
        for (unsigned int c = 0; c < channelCount; ++c)
        {
            for (unsigned int y = 0; y < outputHeight; ++y)
            {
                // Corresponding real-valued height coordinate in input image.
                const float iy = boost::numeric_cast<float>(y) * scaleY;

                // Discrete height coordinate of top-left texel (in the 2x2 texel area used for interpolation).
                const float fiy = floorf(iy);
                const unsigned int y0 = boost::numeric_cast<unsigned int>(fiy);

                // Interpolation weight (range [0,1]).
                const float yw = iy - fiy;

                for (unsigned int x = 0; x < outputWidth; ++x)
                {
                    // Real-valued and discrete width coordinates in input image.
                    const float ix = boost::numeric_cast<float>(x) * scaleX;
                    const float fix = floorf(ix);
                    const unsigned int x0 = boost::numeric_cast<unsigned int>(fix);

                    // Interpolation weight (range [0,1]).
                    const float xw = ix - fix;

                    // Discrete width/height coordinates of texels below and to the right of (x0, y0).
                    const unsigned int x1 = std::min(x0 + 1, inputWidth - 1u);
                    const unsigned int y1 = std::min(y0 + 1, inputHeight - 1u);

                    float interpolatedValue;
                    switch (resizeMethod)
                    {
                        case armnn::ResizeMethod::Bilinear:
                        {
                            in[dataLayout.GetIndex(inputShape, n, c, y0, x0)];
                            float input1 = in.Get();
                            in[dataLayout.GetIndex(inputShape, n, c, y0, x1)];
                            float input2 = in.Get();
                            in[dataLayout.GetIndex(inputShape, n, c, y1, x0)];
                            float input3 = in.Get();
                            in[dataLayout.GetIndex(inputShape, n, c, y1, x1)];
                            float input4 = in.Get();

                            const float ly0 = Lerp(input1, input2, xw); // lerp along row y0.
                            const float ly1 = Lerp(input3, input4, xw); // lerp along row y1.
                            interpolatedValue = Lerp(ly0, ly1, yw);
                            break;
                        }
                        case armnn::ResizeMethod::NearestNeighbor:
                        {
                            // calculate euclidean distance to the 4 neighbours
                            auto distance00 = EuclideanDistance(fix, fiy, x0, y0);
                            auto distance01 = EuclideanDistance(fix, fiy, x0, y1);
                            auto distance10 = EuclideanDistance(fix, fiy, x1, y0);
                            auto distance11 = EuclideanDistance(fix, fiy, x1, y1);

                            auto minimum = std::min( { distance00, distance01, distance10, distance11 } );

                            unsigned int xNearest = 0;
                            unsigned int yNearest = 0;

                            if (minimum == distance00)
                            {
                               xNearest = x0;
                               yNearest = y0;
                            }
                            else if (minimum == distance01)
                            {
                                xNearest = x0;
                                yNearest = y1;
                            }
                            else if (minimum == distance10)
                            {
                                xNearest = x1;
                                yNearest = y0;
                            }
                            else if (minimum == distance11)
                            {
                                xNearest = x1;
                                yNearest = y1;
                            }
                            else
                            {
                                throw armnn::InvalidArgumentException("Resize Nearest Neighbor failure");
                            }

                            in[dataLayout.GetIndex(inputShape, n, c, yNearest, xNearest)];
                            interpolatedValue = in.Get();
                            break;
                        }
                        default:
                            throw armnn::InvalidArgumentException("Unknown resize method: " +
                                                                  std::to_string(static_cast<int>(resizeMethod)));
                    }
                    out[dataLayout.GetIndex(outputShape, n, c, y, x)];
                    out.Set(interpolatedValue);
                }
            }
        }
    }
}

} //namespace armnn


#include <iostream>
#include <vector>

#include <Eigen/Dense>
#include <eigen3/Eigen/Core>

using MatrixXd = Eigen::MatrixXd;
using VectorXd = Eigen::VectorXd;
using Vector2d = Eigen::Vector2d;
using Vector4d = Eigen::Vector4d;
using Vector6d = Eigen::Matrix<double, 6, 1>;
using Vector8d = Eigen::Matrix<double, 8, 1>;
using Matrix8d = Eigen::Matrix<double, 8, 8>;
using TimeStamp = double;
using TimeDuration = double;

constexpr double dt = 1.;

struct RocketState {
    // Using a single dt is a better option.
    // But it make index handling a little bit more complex.
    enum StateIndex : int16_t {
        i_position_x = 0,
        i_position_y,
        i_velocity_x,
        i_velocity_y,
        STATE_SIZE
    };

    explicit RocketState(const double px,
        const double py,
        const double vel_x,
        const double vel_y)
    {
        variables << px, py, vel_x, vel_y;
    }

    RocketState() = default;

    static int variable_size()
    {
        return STATE_SIZE;
    }

    static int lcn_state_size()
    {
        return 2;
    }

    static int control_state_size()
    {
        // i_turning_rate,
        // i_acceleration,
        return 2;
    }

    Eigen::Map<Vector2d> lcn_state()
    {
        return Eigen::Map<Vector2d>(variables.data());
    }

    Eigen::Map<Vector2d> cntl_state()
    {
        return Eigen::Map<Vector2d>(variables.data() + 2);
    }

    Vector2d lcn_state() const
    {
        return variables.block<2, 1>(0, 0);
    }

    Vector2d cntl_state() const
    {
        return variables.block<2, 1>(2, 0);
    }

    Eigen::Map<Vector2d> position()
    {
        return Eigen::Map<Vector2d>(variables.data() + i_position_x);
    }

    Eigen::Map<Vector2d> velocity()
    {
        return Eigen::Map<Vector2d>(variables.data() + i_velocity_x);
    }

    Vector2d velocity() const
    {
        return { variables(i_velocity_x), variables(i_velocity_y) };
    }

    Vector4d variables = Vector4d::Zero();
};

struct RocketMotionModel {
    RocketState motion(const RocketState& state_in) const
    {
        auto state = state_in;
        // TODO: non-const getter
        RocketState next_state = state;

        next_state.velocity() = state.velocity();

        next_state.position()[0] = state.position()[0] + sin(state.velocity()[0]);
        next_state.position()[1] = state.position()[1] + state.velocity()[1];

        return next_state;
    }

    MatrixXd jacobian_wrt_state(const RocketState& state_in) const
    {
        using S = RocketState;
        const int residual_size = RocketState::STATE_SIZE;
        MatrixXd jacobi_wrt_s = MatrixXd::Identity(residual_size, residual_size);

        jacobi_wrt_s(S::i_position_x, S::i_velocity_x) = cos(state_in.velocity()[0]);
        jacobi_wrt_s(S::i_position_y, S::i_velocity_y) = 1.;

        return jacobi_wrt_s;
    }
};

#define PRINT_NAME_VAR(var) std::cout << #var << " :" << var << std::endl

using State = Vector2d;
using Control = Vector2d;

struct RocketLandingProblem {
    // Variables
    std::vector<RocketState> states = std::vector<RocketState>(11);

    // support data
    RocketState start_state = RocketState(0.1, 0.1, 0, 0);
    RocketState end_state = RocketState(2, 2, 0, 0);
};

struct QuadraticCost {
    QuadraticCost() = default;

    QuadraticCost(const VectorXd& meanIn, const MatrixXd& weightIn)
        : mean(meanIn)
        , weight(weightIn)
    {
    }

    double eval(const VectorXd& x) const
    {
        return (x - mean).transpose() * weight * (x - mean);
    }

    VectorXd mean;
    MatrixXd weight;
};

// du = K1 * dx + k2
struct FeedbackLaw {
    FeedbackLaw() = default;

    FeedbackLaw(MatrixXd K1, VectorXd k2)
        : K1(K1)
        , k2(k2)
    {
    }

    VectorXd apply(const VectorXd& x) const
    {
        return K1 * x + k2;
    }

    MatrixXd K1;
    VectorXd k2;
};

// The definition of states and controls is differient from previous section.
struct DDPStates {
    State last_state()
    {
        return states.at(num_states - 1);
    }

    int num_states = 0;
    std::vector<Control> controls;
    std::vector<State> states;
    std::vector<FeedbackLaw> feedbacks;

    State init_state;
    State target_state;
};

inline RocketState ddp_vars_to_rocket_state(const State& x, const Control& u)
{
    RocketState rstate;
    rstate.cntl_state() = u;
    rstate.lcn_state() = x;
    return rstate;
}

class DifferentialDynamicProgramming {
public:
    void solve(RocketLandingProblem& problem)
    {
        DDPStates ddp_states = initialize_ddp_state(problem);
        for (int iteration = 0; iteration < 10; ++iteration) {
            forward_pass(ddp_states);
            backward_pass(ddp_states);
            apply_control_law(ddp_states);
        }

        update_output(ddp_states, problem);
    }

private:
    DDPStates initialize_ddp_state(const RocketLandingProblem& problem)
    {
        DDPStates ddp_states;
        ddp_states.num_states = problem.states.size();
        num_states_ = ddp_states.num_states;
        ddp_states.init_state = problem.start_state.lcn_state();
        ddp_states.target_state = problem.end_state.lcn_state();

        PRINT_NAME_VAR(ddp_states.num_states);
        PRINT_NAME_VAR(ddp_states.init_state);
        PRINT_NAME_VAR(ddp_states.target_state);

        ddp_states.states.resize(ddp_states.num_states);
        ddp_states.controls.resize(ddp_states.num_states - 1);
        ddp_states.feedbacks.resize(ddp_states.num_states - 1);

        for (int i = 0; i < ddp_states.num_states; ++i) {
            ddp_states.states.at(i) = problem.states.at(i).lcn_state();
        }

        for (int i = 0; i < ddp_states.num_states - 1; ++i) {
            ddp_states.controls.at(i) = problem.states.at(i).cntl_state();
        }

        return ddp_states;
    }

    void update_output(const DDPStates& ddp_states, RocketLandingProblem& problem)
    {
        for (int i = 0; i < ddp_states.num_states; ++i) {
            problem.states.at(i).lcn_state() = ddp_states.states.at(i);
        }

        for (int i = 0; i < ddp_states.num_states - 1; ++i) {
            problem.states.at(i).cntl_state() = ddp_states.controls.at(i);
        }
    }

    void forward_pass(DDPStates& ddp_states)
    {
        ddp_states.states.resize(ddp_states.num_states);
        ddp_states.controls.resize(ddp_states.num_states - 1);

        ddp_states.states[0] = ddp_states.init_state;

        RocketMotionModel motion_model;
        for (int i = 0; i < ddp_states.num_states - 1; ++i) {
            RocketState current_full_state = ddp_vars_to_rocket_state(
                ddp_states.states.at(i), ddp_states.controls.at(i));

            RocketState next_full_state = motion_model.motion(current_full_state);
            ddp_states.states.at(i + 1) = next_full_state.lcn_state();

            std::cout << "idx forward iter: " << i << std::endl;
            std::cout << "control: " << ddp_states.controls.at(i).transpose() << std::endl;
            std::cout << "state: " << ddp_states.states.at(i).transpose() << std::endl;
            std::cout << "next state: " << ddp_states.states.at(i + 1).transpose() << std::endl;
        }
    }

    void backward_pass(DDPStates& ddp_states)
    {
        QuadraticCost marginal_cost = target_cost(ddp_states.target_state);

        current_best_cost_ = std::min(current_best_cost_, marginal_cost.eval(ddp_states.last_state()));
        std::cout << "target cost :" << marginal_cost.eval(ddp_states.last_state()) << std::endl;

        for (int i = ddp_states.num_states - 2; i >= 0; --i) {
            const State cur_state = ddp_states.states.at(i);
            const Control cur_control = ddp_states.controls.at(i);

            solve_backward_subproblem(cur_state, cur_control, marginal_cost, ddp_states.feedbacks[i]);
        }
    }

    void solve_backward_subproblem(const State& cur_state,
        const Control& cur_control,
        QuadraticCost& marginal_cost,
        FeedbackLaw& feedback_law)
    {
        const int state_size = 2;
        const int control_size = 2;

        MatrixXd lhs;
        VectorXd rhs;
        compute_backward_subproblem_normal_equation(marginal_cost, cur_state, cur_control, lhs, rhs);

        MatrixXd A1 = lhs.block(0, 0, state_size, state_size);
        MatrixXd A2 = lhs.block(0, state_size, state_size, control_size);
        MatrixXd A3 = lhs.block(state_size, 0, control_size, state_size);
        MatrixXd A4 = lhs.block(state_size, state_size, control_size, control_size);

        VectorXd b1 = rhs.block(0, 0, state_size, 1);
        VectorXd b2 = rhs.block(state_size, 0, control_size, 1);

        MatrixXd A4_inv = A4.inverse();

        // eliminate du
        MatrixXd lhs_xi = A1 - A2 * A4_inv * A3;
        VectorXd rhs_xi = b1 - A2 * A4_inv * b2;

        // solve PSD system by Cholesky
        VectorXd xi_star = lhs_xi.llt().solve(rhs_xi);

        marginal_cost = QuadraticCost(cur_state + xi_star, lhs_xi);
        // std::cout << "marginal_cost \n mean:" << marginal_cost.mean.transpose()
        //     << "\n weight:" << marginal_cost.weight << std::endl;
        feedback_law = FeedbackLaw(-A4_inv * A3, A4_inv * b2);
    }

    void compute_backward_subproblem_normal_equation(const QuadraticCost& marginal_cost,
        const State& cur_state,
        const Control& cur_control,
        MatrixXd& lhs,
        VectorXd& rhs)
    {
        const int state_size = 2;
        const int control_size = 2;
        const int total_size = state_size + control_size;

        RocketMotionModel motion_model;
        const RocketState rstate = ddp_vars_to_rocket_state(cur_state, cur_control);

        const MatrixXd jacobi_wrt_augment_state = motion_model.jacobian_wrt_state(rstate);
        MatrixXd jacobian_marginal_cost = jacobi_wrt_augment_state.block(0, 0, state_size, total_size);

        const RocketState predict_state = motion_model.motion(rstate);

        std::cout << "rstate.var:       " << rstate.variables << std::endl;
        std::cout << "predict_state.var:" << predict_state.variables << std::endl;
        std::cout << "jacobian_marginal_cost: " << jacobian_marginal_cost << std::endl;

        const VectorXd residual = predict_state.lcn_state() - marginal_cost.mean;

        lhs = jacobian_marginal_cost.transpose() * marginal_cost.weight * jacobian_marginal_cost;
        rhs = -jacobian_marginal_cost.transpose() * marginal_cost.weight * residual;

        // current cost (just regularizations for this problem)
        const double time_regularization = 1e-3 / num_states_;
        lhs(RocketState::i_velocity_x, RocketState::i_velocity_x) += 2 * time_regularization;
        rhs(RocketState::i_velocity_x) += -2 * time_regularization * cur_control(RocketState::i_velocity_x - 2);

        lhs(RocketState::i_velocity_y, RocketState::i_velocity_y) += 2 * time_regularization;
        rhs(RocketState::i_velocity_y) += -2 * time_regularization * cur_control(RocketState::i_velocity_y - 2);
    }

    QuadraticCost target_cost(const Vector2d& target_state)
    {
        QuadraticCost qcost;
        qcost.mean = target_state;
        MatrixXd jacobi = MatrixXd::Identity(2, 2);

        MatrixXd weight = MatrixXd::Identity(2, 2);
        Vector2d weight_diag;
        //  dt, x, y, vel_x, vel_y, heading
        weight_diag << 1e0, 1e0;
        weight.diagonal() = weight_diag;

        qcost.weight = jacobi.transpose() * weight * jacobi;

        return qcost;
    }

    void apply_control_law(DDPStates& ddp_states)
    {
        std::vector<Control> new_controls = ddp_states.controls;
        std::vector<State> new_states = ddp_states.states;
        RocketMotionModel motion_model;

        double shooting_cost = std::numeric_limits<double>::max();

        PRINT_NAME_VAR(shooting_cost);
        PRINT_NAME_VAR(current_best_cost_);

        // DDP is HARD to converge!
        double step = 0.5;
        // for (; current_best_cost_ < shooting_cost && step > 1e-4; step *= 0.5) {
        for (int i = 0; i < ddp_states.num_states - 1; ++i) {
            const FeedbackLaw& feedback = ddp_states.feedbacks.at(i);
            Vector2d delta_state = new_states.at(i) - ddp_states.states.at(i);
            if (i == 0)
                assert(delta_state.sum() == 0);

            Vector2d delta_control = feedback.apply(delta_state);

            std::cout << "=== idex: " << i << std::endl;
            std::cout << "delta_state: " << delta_state.transpose() << std::endl;
            std::cout << "delta_control: " << delta_control.transpose() << std::endl;

            new_controls.at(i) = ddp_states.controls.at(i) + step * delta_control;

            const RocketState new_rstate = ddp_vars_to_rocket_state(new_states.at(i), new_controls.at(i));
            new_states.at(i + 1) = motion_model.motion(new_rstate).lcn_state();

            std::cout << "new_control: " << new_controls.at(i).transpose() << std::endl;
            std::cout << "cur_state:   " << new_states.at(i).transpose() << std::endl;
            std::cout << "new_state:   " << new_states.at(i + 1).transpose() << std::endl;
        }

        QuadraticCost marginal_cost = target_cost(ddp_states.target_state);
        double cur_cost = marginal_cost.eval(new_states.back());
        shooting_cost = std::min(shooting_cost, cur_cost);
        // }

        current_best_cost_ = std::min(current_best_cost_, shooting_cost);

        std::cout << "ddp backtracking step:" << step << std::endl;
        std::cout << "current_best_cost_:" << current_best_cost_ << std::endl;
        std::cout << "cur_cost:" << cur_cost << std::endl;

        ddp_states.states = new_states;
        ddp_states.controls = new_controls;
    }

    // current minmum target cost. Should use total cost.
    double current_best_cost_ = std::numeric_limits<double>::max();

    double num_states_ = -1;
};

// yimu@yimu-mate:~/Desktop/blog$ /usr/bin/python3 /home/yimu/Desktop/blog/yimu-blog/least_squares/ddp/ddp_optimization.py
// initial_state: [0.1 0.1]
// target_state: [2. 2.]
// num_states: 11
// final_state_end_cost: [[0.22966875]]
// final_state_end_cost: [[0.00102592]]
// final_state_end_cost: [[0.00737957]]
// final_state_end_cost: [[0.00287627]]
// final_state_end_cost: [[0.00056493]]
// final_state_end_cost: [[6.04254466e-05]]
// final_state_end_cost: [[1.60630207e-06]]
// final_state_end_cost: [[8.4323494e-07]]
// final_state_end_cost: [[1.20808104e-06]]
// final_state_end_cost: [[6.61618857e-07]]
// ----------------------------------
// new_controls:
//  [array([0.19102753, 0.18981443]), array([0.19110838, 0.18991752]), array([0.19117968, 0.19001093]), array([0.19123781, 0.19009055]), array([0.19127743, 0.19015008]), array([0.19129027, 0.19017957]), array([0.19126273, 0.19016206]), array([0.19117054, 0.19006576]), array([0.19096421, 0.18982076]), array([0.19051605, 0.18921421])]
// new_states:
//  [array([0.1, 0.1]), array([0.28986784, 0.28981443]), array([0.47981505, 0.47973195]), array([0.66983226, 0.66974288]), array([0.85990655, 0.85983343]), array([1.05001973, 1.04998351]), array([1.24014552, 1.24016308]), array([1.43024427, 1.43032515]), array([1.62025251, 1.6203909 ]), array([1.81005818, 1.81021167]), array([1.9994238 , 1.99942588])]

// ./test_ddp | grep cur_cost
// cur_cost:0.229777
// cur_cost:0.00101972
// cur_cost:0.00736767
// cur_cost:0.00287048
// cur_cost:0.000562644
// cur_cost:5.97038e-05
// cur_cost:1.5031e-06
// cur_cost:9.30061e-07
// cur_cost:1.31691e-06
// cur_cost:7.43505e-07
//
// new_control: 0.190882 0.189625
// cur_state:   0.1 0.1
// --
// new_control: 0.191034 0.189811
// cur_state:   0.289725 0.289625
// --
// new_control: 0.191173 0.189985
// cur_state:   0.479599 0.479436
// --
// new_control: 0.191293 0.190141
// cur_state:    0.66961 0.669421
// --
// new_control: 0.191385 0.190269
// cur_state:   0.859738 0.859562
// --
// new_control: 0.191435 0.190352
// cur_state:   1.04996 1.04983
// --
// new_control: 0.191417 0.190362
// cur_state:   1.24023 1.24018
// --
// new_control:  0.19128 0.190237
// cur_state:   1.43048 1.43055
// --
// new_control:  0.19091 0.189834
// cur_state:   1.62059 1.62078
// --
// new_control: 0.189926 0.188637
// cur_state:   1.81034 1.81062
// --
// new_control: 0.191024 0.189811
// cur_state:   0.1 0.1
// --
// new_control: 0.191105 0.189914
// cur_state:   0.289864 0.289811
// --
// new_control: 0.191176 0.190007
// cur_state:   0.479808 0.479725
// --
// new_control: 0.191234 0.190087
// cur_state:   0.669822 0.669733
// --
// new_control: 0.191274 0.190147
// cur_state:   0.859892  0.85982
// --
// new_control: 0.191287 0.190176
// cur_state:      1.05 1.04997
// --
// new_control: 0.191259 0.190159
// cur_state:   1.24012 1.24014
// --
// new_control: 0.191167 0.190062
// cur_state:   1.43022  1.4303
// --
// new_control: 0.190961 0.189817
// cur_state:   1.62022 1.62036
// --
// new_control: 0.190513 0.189211
// cur_state:   1.81003 1.81018

int main(int argc, char* argv[])
{
    RocketLandingProblem p;

    DifferentialDynamicProgramming solver;
    solver.solve(p);

    return 1;
}