#include <boost/geometry/formulas/mean_radius.hpp>


// Copyright 2020 Erik Kouters (Falcons)
// SPDX-License-Identifier: Apache-2.0
/*
 * cVelocityTransformData.hpp
 *
 *  Created on: Jan 14, 2018
 *  Author: Erik Kouters
 */

#ifndef CVELOCITYTRANSFORMDATA_HPP_
#define CVELOCITYTRANSFORMDATA_HPP_

#include "linepoint2D.hpp"
#include "polygon2D.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "cVelocityTransformTypes.hpp"

class cVelocityTransformData
{
    public:
        cVelocityTransformData();
        ~cVelocityTransformData() { };

        // Getters and setters

        // Robot data
        // Target from VelocityControl
        // Feedback for WorldModel
        void getTargetRobotData(vt_robot_data& targetRobotData);
        void setTargetRobotData(const vt_robot_data& targetRobotData);

        void getFeedbackRobotData(vt_robot_data& robotData);
        void setFeedbackRobotData(const vt_robot_data& robotData);

        // Motors data
        void getTargetMotorsData(vt_motors_data& motorsData);
        void setTargetMotorsData(const vt_motors_data& motorsData);

        void getFeedbackMotorsData(vt_motors_data& motorsData);
        void setFeedbackMotorsData(const vt_motors_data& motorsData);

        // Motor matrix
        void getMatrix(boost::numeric::ublas::matrix<double>& matrix);
        void setMatrix(const boost::numeric::ublas::matrix<double>& matrix);

        void getInvMatrix(boost::numeric::ublas::matrix<double>& invMatrix);
        void setInvMatrix(const boost::numeric::ublas::matrix<double>& invMatrix);

        publishFeedbackFunctionType publishFeedback;

        publishTargetFunctionType publishTarget;

    private:

        // Data types
        // target is what should be published to peripheralsInterface
        // feedback is what is received from peripheralsInterface

        // Robot data (RCS)
        vt_robot_data _targetRobotData;
        vt_robot_data _feedbackRobotData;

        // Motors data
        vt_motors_data _targetMotorsData;
        vt_motors_data _feedbackMotorsData;

        // Motor matrix
        boost::numeric::ublas::matrix<double> _matrix;
        boost::numeric::ublas::matrix<double> _inverseMatrix;

};

#endif /* CVELOCITYTRANSFORMDATA_HPP_ */


/**
 * \ file InPointerFilter.cpp
 */

#include <cmath>

#include <boost/math/constants/constants.hpp>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/scoped_array.hpp>

#define PROCESSSIZE (10)

BOOST_AUTO_TEST_CASE( OutPointerFloat_sin1k_test )
{
  boost::scoped_array<float> data(new float[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<float> outdata(new float[PROCESSSIZE]);

  ATK::OutPointerFilter<float> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);

  output.process(2);
  output.process(PROCESSSIZE - 2);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE( OutPointerDouble_sin1k_test )
{
  boost::scoped_array<double> data(new double[PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }

  ATK::InPointerFilter<double> generator(data.get(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<double> outdata(new double[PROCESSSIZE]);

  ATK::OutPointerFilter<double> output(outdata.get(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);

  output.process(2);
  output.process(PROCESSSIZE - 2);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE( OutPointerFloat_sin1k2k_interleaved_test )
{
  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[2*i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
    data[2*i+1] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 2000);
  }

  ATK::InPointerFilter<float> generator(data.get(), PROCESSSIZE, 2, true);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<double> outdata(new double[2*PROCESSSIZE]);

  ATK::OutPointerFilter<double> output(outdata.get(), PROCESSSIZE, 2, true);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);
  output.set_input_port(1, &generator, 1);

  output.process(2);
  output.process(PROCESSSIZE - 2);

  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
}

BOOST_AUTO_TEST_CASE( OutPointerFloat_sin1k2k_noninterleaved_test )
{
  boost::scoped_array<float> data(new float[2*PROCESSSIZE]);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i+PROCESSSIZE] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 2000);
  }

  ATK::InPointerFilter<float> generator(data.get(), 2, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  boost::scoped_array<double> outdata(new double[2*PROCESSSIZE]);

  ATK::OutPointerFilter<double> output(outdata.get(), 2, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &generator, 0);
  output.set_input_port(1, &generator, 1);

  output.process(2);
  output.process(PROCESSSIZE - 2);

  for(int64_t i = 0; i < 2*PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_EQUAL(data[i], outdata[i]);
  }
}


#include "../agg.hpp"
#include <cmath>

namespace std {
  template<class T> vector<T> sqrt(vector<T> val) {
    vector<T> res(val.size());
    for (size_t i = 0; i < val.size(); ++i) {
       res[i] = sqrt(val[i]);
    }
    return res;
  }
}

#include <boost/accumulators/statistics/skewness.hpp>

ARRAY_AGGREGATE_FNC(askew, tag::skewness);
SQL_AGGREGATE_DBL_ONLY_FNC(v_skew, tag::skewness);


// #pragma GCC optimize ("O0")

#include <boost/log/trivial.hpp>

#include "rtcmEncoder.hpp"
#include "acsConfig.hpp"
#include "constants.hpp"

void calculateSsrComb(
	GTime 						curTime,
	int 						period,
	SSRMeta 					ssrMeta,
	int 						masterIod,
	map<SatSys, SSROut>&		ssrOutMap)
{
	if (ssrOutMap.empty())
	{
		BOOST_LOG_TRIVIAL(warning) << "No suitable Ephemeris data available.";
		return;
	}

	map<E_Sys, std::array<double,2>> commonClockOffsetsMap; // 0: bias; 1: bias rate
	map<E_Sys, std::array<GTime, 2>> commonClockEpochsMap;	// 0: 1st straddle pt epoch; 1: 2nd straddle pt epoch

	for (auto& [Sat, ssrOut] : ssrOutMap)
	{
		if (ssrOut.clkInput.vals[0].iode != ssrOut.ephInput.vals[0].iode)
		{
			tracepdeex(3, std::cout, "IODE mismatch between clock and ephemeris for %s\n", Sat.id().c_str());
			continue;
		}

		auto& ssrEph		= ssrOut.ssrEph;
		auto& ssrClk		= ssrOut.ssrClk;
		auto& ssrEphInput	= ssrOut.ephInput;
		auto& ssrClkInput	= ssrOut.clkInput;

		ssrEph.t0 					= curTime + period / 2;
		ssrClk.t0 					= curTime + period / 2;
		ssrEph.udi					= period;
		ssrClk.udi					= period;
		ssrEph.iod					= masterIod;
		ssrClk.iod					= masterIod;

		ssrEph.ssrMeta				= ssrMeta;
		ssrClk.ssrMeta				= ssrMeta;

		ssrEph.iode		= ssrEphInput.vals[0].iode;

		// ssrEph.iodcrc				= ??

		double		clkCorrections[2];
		Vector3d	posCorrections[2];

		for (int i = 0; i < 2; i++)
		{
			posCorrections[i]	= ssrEphInput.vals[i].brdcPos
								- ssrEphInput.vals[i].precPos;
			clkCorrections[i]	= ssrClkInput.vals[i].brdcClk
								- ssrClkInput.vals[i].precClk;
		}
#if 0
		Vector3d	diffRAC[2];
		double		diffClock[2];

		for (int dt : {0, 1})
		{
			double		ephRatio	= (ssrEph.t0 + dt - ssrEphInput.vals[0].time) / (ssrEphInput.vals[1].time - ssrEphInput.vals[0].time);
			double		clkRatio	= (ssrClk.t0 + dt - ssrClkInput.vals[0].time) / (ssrClkInput.vals[1].time - ssrClkInput.vals[0].time);

			Vector3d	posCorrection	= posCorrections[0] 			+ ephRatio	* (posCorrections[1]			- posCorrections[0]);
			double		clkCorrection	= clkCorrections[0]				+ clkRatio	* (clkCorrections[1]			- clkCorrections[0]);

			Vector3d	satPosition		= ssrEphInput.vals[0].brdcPos	+ ephRatio	* (ssrEphInput.vals[1].brdcPos	- ssrEphInput.vals[0].brdcPos);
			Vector3d	satVelocity		= ssrEphInput.vals[0].brdcVel	+ ephRatio	* (ssrEphInput.vals[1].brdcVel	- ssrEphInput.vals[0].brdcVel);

			Vector3d	diffRac			= ecef2rac(satPosition, satVelocity) * posCorrection;

			diffRAC		[dt]	= diffRac;
			diffClock	[dt]	= -clkCorrection;

			std::cout << std::fixed;
			std::cout << Sat.id() << " " << dt <<" BRatio:       "<< ephRatio << std::endl;
			std::cout << Sat.id() << " " << dt <<" iode:         "<< ssrOut.clkInput.vals[0].iode << std::endl;
			std::cout << Sat.id() << " " << dt <<" brdc1:        "<< ssrEphInput.vals[0].brdcPos.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" brdc2:        "<< ssrEphInput.vals[1].brdcPos.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" prec1:        "<< ssrEphInput.vals[0].precPos.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" prec2:        "<< ssrEphInput.vals[1].precPos.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" diffEcef:     "<< posCorrection.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" diffRac:      "<< diffRac.transpose() << std::endl;
			std::cout << Sat.id() << " " << dt <<" brdc1:        "<< ssrClkInput.vals[0].brdcClk << std::endl;
			std::cout << Sat.id() << " " << dt <<" brdc2:        "<< ssrClkInput.vals[1].brdcClk << std::endl;
			std::cout << Sat.id() << " " << dt <<" prec1:        "<< ssrClkInput.vals[0].precClk << std::endl;
			std::cout << Sat.id() << " " << dt <<" prec2:        "<< ssrClkInput.vals[1].precClk << std::endl;
			std::cout << Sat.id() << " " << dt <<" clkCorrection:"<< clkCorrection << std::endl;
		}

		ssrEph. deph	= diffRAC[0];
		ssrEph.ddeph	= diffRAC[1] - diffRAC[0];

		ssrClk.dclk[0]	= diffClock[0];
		ssrClk.dclk[1]	= diffClock[1] - diffClock[0];
		ssrClk.dclk[2]	= 0;	// set to zero (not used)

// 		std::cout << "deph:         "<< ssrEph. deph.transpose() << std::endl;
// 		std::cout << "ddeph:        "<< ssrEph.ddeph.transpose() << std::endl;
		std::cout << "deph:         "<< ssrEph. deph.transpose() << std::endl;
		std::cout << Sat.id() << "dclk[0]:        "<< ssrClk.dclk[0] << std::endl;
		std::cout << Sat.id() << "dclk[1]:        "<< ssrClk.dclk[1] << std::endl << std::endl;

#else	/* Bypass interpolation */
		ssrEph.deph     = ecef2rac(ssrEphInput.vals[1].brdcPos, ssrEphInput.vals[1].brdcVel) * posCorrections[1];
		ssrClk.dclk[0]	= -clkCorrections[1];
		ssrClk.dclk[1]	= 0;	// set to zero (not used)
		ssrClk.dclk[2]	= 0;	// set to zero (not used)
#endif

		//adjust all clock corrections so that they remain within the bounds of the outputs
		if (commonClockOffsetsMap[Sat.sys][0] == 0)
		{
			commonClockOffsetsMap[Sat.sys][0] = ssrClk.dclk[0];
			commonClockOffsetsMap[Sat.sys][1] = ssrClk.dclk[1];
			commonClockEpochsMap [Sat.sys][0] = ssrClkInput.vals[0].time;
			commonClockEpochsMap [Sat.sys][1] = ssrClkInput.vals[1].time;
		}
		if	( ssrClkInput.vals[0].time != commonClockEpochsMap[Sat.sys][0]
			||ssrClkInput.vals[1].time != commonClockEpochsMap[Sat.sys][1])
		{
			continue; // commonClockOffset is lagging/leading this sat's clock, skip
		}
		ssrClk.dclk[0] -= commonClockOffsetsMap[Sat.sys][0];
		ssrClk.dclk[1] -= commonClockOffsetsMap[Sat.sys][1];
	}
// 	std::cout << "Calculated Combined Messages.\n";
}

int RtcmEncoder::getUdiIndex(int udi)
{
	for (int i = 0; i < 16; i++) // 16 from updateInterval[16] above
	{
		if (updateInterval[i] == udi)
		{
			return i;
		}
	}

	BOOST_LOG_TRIVIAL(error) << "Error: udi is not valid :" << udi << ").";

	return -1;
}

void RtcmEncoder::encodeWriteMessages(
	std::ostream& outputStream)
{
	if (outputStream)
	{
		outputStream.write((const char*) &data[0], data.size());
		data.clear();
	}
}

bool RtcmEncoder::encodeWriteMessageToBuffer(
	vector<uint8_t>& buffer)
{
	int i = 0;
	int messLength = buffer.size();

	if (buffer.empty())
	{
		return false;
	}

// 	unsigned char nbuf[messLength+6];
	vector<uint8_t> newbuffer(messLength+6);
	unsigned char* nbuf	= newbuffer.data();
	unsigned char* buf	= buffer.data();

	i = setbituInc(nbuf,i,8,	RTCM_PREAMBLE);
	i = setbituInc(nbuf,i,6,	0);
	i = setbituInc(nbuf,i,10,	messLength);

	memcpy(nbuf+3,buf,sizeof(uint8_t)*messLength);
	i = i + messLength*8;

	const unsigned char* bCrcBuf = (const unsigned char *)nbuf;
	unsigned int crcCalc = crc24q(bCrcBuf, sizeof(char)*(messLength+3));

	unsigned char* bCrcCalc = (unsigned char*)&crcCalc;
	unsigned int b1 = 0;
	unsigned int b2 = 0;
	unsigned int b3 = 0;
	setbituInc((unsigned char *)&b1,0,8,*(bCrcCalc+0));
	setbituInc((unsigned char *)&b2,0,8,*(bCrcCalc+1));
	setbituInc((unsigned char *)&b3,0,8,*(bCrcCalc+2));
	i = setbituInc(nbuf,i,8,b3);
	i = setbituInc(nbuf,i,8,b2);
	i = setbituInc(nbuf,i,8,b1);

	data.insert(data.end(), &nbuf[0], &nbuf[messLength+6]);

	return true;
}

vector<uint8_t> RtcmEncoder::encodeTimeStampRTCM()
{
	// Custom message code, for crcsi maximum length 4096 bits or 512 bytes.
	unsigned int messCode = +RtcmMessageType::CUSTOM;
	unsigned int messType = +E_RTCMSubmessage::TIMESTAMP;

	boost::posix_time::ptime now = boost::posix_time::microsec_clock::universal_time();

	// Number of seconds since 1/1/1970, long is 64 bits and all may be used.
	long int seconds = (now - boost::posix_time::from_time_t(0)).total_seconds();

	//Number of fractional seconds, The largest this can be is 1000 which is 10 bits unsigned.
	boost::posix_time::ptime now_mod_seconds	= boost::posix_time::from_time_t(seconds);
	auto subseconds	= now - now_mod_seconds;
	int milli_sec = subseconds.total_milliseconds();

	unsigned int* var = (unsigned int*) &seconds;

	int i = 0;
	//int byteLen = ceil((12.0+8.0+64.0+10.0)/8.0);
	int byteLen = 12;
	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();
	i = setbituInc(buf, i, 12,		messCode);
	i = setbituInc(buf, i, 8,		messType);
	i = setbituInc(buf, i, 32,		var[0]);
	i = setbituInc(buf, i, 32,		var[1]);
	i = setbituInc(buf, i, 10, (int)milli_sec);

	return buffer;
}

vector<uint8_t> RtcmEncoder::encodeSsrComb(
	map<SatSys, SSROut>& ssrOutMap)
{
	int numSat = ssrOutMap.size();
	if (numSat == 0)
	{
		return vector<uint8_t>();
	}

	auto& [Sat, ssrOut1]	= *ssrOutMap.begin();
	auto& ssrEph			= ssrOut1.ssrEph;
	auto& ssrMeta			= ssrEph.ssrMeta;

	int i		= 0;
	int bitLen	= 0;
	int ni		= 0;
	unsigned int messCode = 0;
	if		(Sat.sys == +E_Sys::GPS) {	messCode = +RtcmMessageType::GPS_SSR_COMB_CORR;		ni= 8; bitLen = 68+numSat*205;}
	else if (Sat.sys == +E_Sys::GAL) {	messCode = +RtcmMessageType::GAL_SSR_COMB_CORR;		ni=10; bitLen = 68+numSat*207;}

	int byteLen = ceil(bitLen/8.0);
	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();

	// Write the header information.
	i = setbituInc(buf,i,12,	messCode);
	i = setbituInc(buf,i,20,	ssrMeta.epochTime1s);
	i = setbituInc(buf,i,4,		ssrMeta.ssrUpdateIntIndex);
	i = setbituInc(buf,i,1,		ssrMeta.multipleMessage);
	i = setbituInc(buf,i,1,		ssrMeta.referenceDatum);
	i = setbituInc(buf,i,4,		ssrEph.iod);
	i = setbituInc(buf,i,16,	ssrMeta.provider);
	i = setbituInc(buf,i,4,		ssrMeta.solution);
	i = setbituInc(buf,i,6,		numSat);

	for (auto& [Sat, ssrOut] : ssrOutMap)
	{
		auto& ssrEph = ssrOut.ssrEph;
		auto& ssrClk = ssrOut.ssrClk;

		i = setbituInc(buf,i, 6,	Sat.prn);
		i = setbituInc(buf,i, ni,	ssrEph.iode);

		int d;
		d = (int)round(ssrEph.deph[0]		/ 0.1e-3);				i = setbitsInc(buf,i,22,d);
		d = (int)round(ssrEph.deph[1]		/ 0.4e-3);				i = setbitsInc(buf,i,20,d);
		d = (int)round(ssrEph.deph[2]		/ 0.4e-3);				i = setbitsInc(buf,i,20,d);
		d = (int)round(ssrEph.ddeph[0]		/ 0.001e-3);			i = setbitsInc(buf,i,21,d);
		d = (int)round(ssrEph.ddeph[1]		/ 0.004e-3);			i = setbitsInc(buf,i,19,d);
		d = (int)round(ssrEph.ddeph[2]		/ 0.004e-3);			i = setbitsInc(buf,i,19,d);

		d = (int)round(ssrClk.dclk[0]		/ 0.1e-3);				i = setbitsInc(buf,i,22,d);
		d = (int)round(ssrClk.dclk[1]		/ 0.001e-3);			i = setbitsInc(buf,i,21,d);
		d = (int)round(ssrClk.dclk[2]		/ 0.00002e-3);			i = setbitsInc(buf,i,27,d);

		outputSsrEphToJson(ssrEph, Sat);
		outputSsrClkToJson(ssrClk, Sat);

	}

	int bitl = byteLen*8-i;
	if (bitl > 7 )
	{
		BOOST_LOG_TRIVIAL(error) << "Error encoding combined.\n";
		BOOST_LOG_TRIVIAL(error) << "bitl : " << bitl << ", i : " << i << ", byteLen : " << byteLen << std::endl;
	}
	i = setbituInc(buf, i, bitl, 0);

	return buffer;
}

vector<uint8_t> RtcmEncoder::encodeSsrPhase(
	SsrPBMap& ssrPBMap)
{
	int numSat = ssrPBMap.size();

	int i = 0;
	int bitLen = 0;

	int totalNbias = 0;
	for (auto& [Sat,ssrPhasBias] : ssrPBMap)
	{
		totalNbias += ssrPhasBias.obsCodeBiasMap.size();
	}

	if (totalNbias == 0)
		return vector<uint8_t>();

	// Write the header information.
	auto s_it = ssrPBMap.begin();
	auto& [Sat, ssrPhasBias] = *s_it;
	SSRMeta& ssrMeta = ssrPhasBias.ssrMeta;

	int np = 0;
	int ni = 0;
	int nj = 0;
	int offp = 0;
	unsigned int messCode = 0;
	if (Sat.sys == +E_Sys::GPS) {	messCode = +RtcmMessageType::GPS_SSR_PHASE_BIAS;	np=6; ni= 8; nj= 0; offp=0;	bitLen = 69+numSat*28+totalNbias*32;}
	if (Sat.sys == +E_Sys::GAL)	{	messCode = +RtcmMessageType::GAL_SSR_PHASE_BIAS;	np=6; ni=10; nj= 0; offp=0;	bitLen = 69+numSat*28+totalNbias*32;}

	int byteLen = ceil(bitLen/8.0);
	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();

	i = setbituInc(buf,i,12,messCode);
	i = setbituInc(buf,i,20,ssrMeta.epochTime1s);
	i = setbituInc(buf,i,4,	ssrMeta.ssrUpdateIntIndex);
	i = setbituInc(buf,i,1,	ssrMeta.multipleMessage);

	i = setbituInc(buf,i,4,	ssrPhasBias.iod);
	i = setbituInc(buf,i,16,ssrMeta.provider);
	i = setbituInc(buf,i,4,	ssrMeta.solution);

	i = setbituInc(buf,i,1,	ssrPhasBias.ssrPhase.dispBiasConistInd);
	i = setbituInc(buf,i,1,	ssrPhasBias.ssrPhase.MWConistInd);

	i = setbituInc(buf,i,6,numSat);

	for (auto& [sat, ssrPhasBias] : ssrPBMap)
	{
		SSRPhase ssrPhase = ssrPhasBias.ssrPhase;

		int d;
															i = setbituInc(buf,i,np,sat.prn);
		d = ssrPhasBias.obsCodeBiasMap.size();				i = setbituInc(buf,i,5,	d);
		d = (int)round(ssrPhase.yawAngle	*256	/PI);	i = setbituInc(buf,i,9,	d);
		d = (int)round(ssrPhase.yawRate		*8192	/PI);	i = setbitsInc(buf,i,8,	d);

		for (auto& [obsCode, entry] : ssrPhasBias.obsCodeBiasMap)
		{
			SSRPhaseCh ssrPhaseCh = ssrPhasBias.ssrPhaseChs[obsCode];

			//BOOST_LOG_TRIVIAL(debug) << "Phase, obsCode : " << obsCode << std::endl;
			//BOOST_LOG_TRIVIAL(debug) << "E_sys         : " << sys << std::endl;
			//BOOST_LOG_TRIVIAL(debug) << "mCodes_gps.size() : " << mCodes_gps.size() << std::endl;
			//print_map( mCodes_gps.left, " E_ObsCode <--> RTCM ", BOOST_LOG_TRIVIAL(debug) );

			int rtcm_code = 0;
			if		( sat.sys == +E_Sys::GPS )		rtcm_code = mCodes_gps.left.at(obsCode);		//todo aaron, crash heaven, needs else, try
			else if ( sat.sys == +E_Sys::GAL )		rtcm_code = mCodes_gal.left.at(obsCode);

			//BOOST_LOG_TRIVIAL(debug) << "rtcm_code      : " << rtcm_code << std::endl;

													i = setbituInc(buf,i,5,	rtcm_code);
													i = setbituInc(buf,i,1,	ssrPhaseCh.signalIntInd);
													i = setbituInc(buf,i,2,	ssrPhaseCh.signalWidIntInd);
													i = setbituInc(buf,i,4,	ssrPhaseCh.signalDisconCnt);
			d = (int)round(entry.bias / 0.0001);	i = setbitsInc(buf,i,20,d);

			traceSsrPhasB(sat, obsCode, ssrPhasBias);
		}
	}

	int bitl = byteLen*8-i;
	if (bitl > 7 )
	{
		BOOST_LOG_TRIVIAL(error) << "Error encoding SSR Phase.\n";
		BOOST_LOG_TRIVIAL(error) << "bitl : " << bitl << ", i : " << i << ", byteLen : " << byteLen << std::endl;
	}

	i = setbituInc(buf,i,bitl,0);

	return buffer;
}

vector<uint8_t> RtcmEncoder::encodeSsrCode(
	SsrCBMap& ssrCBMap)
{
	int numSat = ssrCBMap.size();

	int i = 0;

	int totalNbias = 0;
	for (auto s_it = ssrCBMap.begin(); s_it != ssrCBMap.end(); s_it++)
	{
		SSRCodeBias ssrCodeBias = s_it->second;
		totalNbias += ssrCodeBias.obsCodeBiasMap.size();
	}

	if (totalNbias == 0)
	{
		return vector<uint8_t>();
	}

	// Write the header information.

	auto s_it = ssrCBMap.begin();
	auto& [Sat, ssrCodeBias] = *s_it;
	SSRMeta& ssrMeta = ssrCodeBias.ssrMeta;

	int bitLen	= 67
				+ 11 * numSat
				+ 19 * totalNbias;
	int byteLen = ceil(bitLen / 8.0);

	vector<uint8_t> buffer(byteLen);
	unsigned char* buf = buffer.data();

	unsigned int messCode = 0;
	if (Sat.sys == +E_Sys::GPS)	{	messCode = +RtcmMessageType::GPS_SSR_CODE_BIAS;}
	if (Sat.sys == +E_Sys::GAL)	{	messCode = +RtcmMessageType::GAL_SSR_CODE_BIAS;}

	i = setbituInc(buf,i,12,messCode);
	i = setbituInc(buf,i,20,ssrMeta.epochTime1s);
	i = setbituInc(buf,i,4,	ssrMeta.ssrUpdateIntIndex);
	i = setbituInc(buf,i,1,	ssrMeta.multipleMessage);

	i = setbituInc(buf,i,4,	ssrCodeBias.iod);
	i = setbituInc(buf,i,16,ssrMeta.provider);
	i = setbituInc(buf,i,4,	ssrMeta.solution);
	i = setbituInc(buf,i,6,	numSat);

	for (auto& [sat, ssrCodeBias] : ssrCBMap)
	{
		i = setbituInc(buf, i, 6, sat.prn);
		unsigned int nbias = ssrCodeBias.obsCodeBiasMap.size();

		i = setbituInc(buf, i, 5, nbias);

		for (auto& [obsCode, entry] : ssrCodeBias.obsCodeBiasMap)
		{
			int rtcm_code = 0;
			if		( sat.sys == +E_Sys::GPS )	{	rtcm_code = mCodes_gps.left.at(obsCode);		}
			else if ( sat.sys == +E_Sys::GAL )	{	rtcm_code = mCodes_gal.left.at(obsCode);		}

													i = setbituInc(buf, i, 5,	rtcm_code);
			int d = (int)round(entry.bias / 0.01);	i = setbitsInc(buf, i, 14,	d);

			traceSsrCodeB(sat, obsCode, ssrCodeBias);
		}
	}

	int bitl = byteLen*8-i;
	if (bitl > 7)
	{
		BOOST_LOG_TRIVIAL(error) << "Error encoding SSR Code.\n";
		BOOST_LOG_TRIVIAL(error) << "bitl : " << bitl << ", i : " << i << ", byteLen : " << byteLen << std::endl;
	}

	i = setbituInc(buf,i,bitl,0);

	return buffer;
}


#include <stan/variational/advi.hpp>
#include <stan/callbacks/stream_logger.hpp>
#include <gtest/gtest.h>
#include <test/unit/util.hpp>
#include <string>
#include <test/test-models/good/model/valid.hpp>
#include <boost/random/additive_combine.hpp>  // L'Ecuyer RNG
#include <stan/mcmc/chains.hpp>
#include <stan/io/stan_csv_reader.hpp>
#include <gtest/gtest.h>
#include <fstream>
#include <sstream>

typedef boost::ecuyer1988 rng_t;

class RVI_SUBMODULE_TEST : public testing::Test {
public:
    void SetUp() {
      blocker1_stream.open("src/test/unit/mcmc/test_csv_files/blocker.1.csv");
      blocker2_stream.open("src/test/unit/mcmc/test_csv_files/blocker.2.csv");
    }

    void TearDown() {
      blocker1_stream.close();
      blocker2_stream.close();
    }
    std::ifstream blocker1_stream, blocker2_stream;
};

TEST_F(RVI_SUBMODULE_TEST, ess_mcse) {
std::stringstream out;
stan::io::stan_csv blocker1
        = stan::io::stan_csv_reader::parse(blocker1_stream, &out);
stan::io::stan_csv blocker2
        = stan::io::stan_csv_reader::parse(blocker2_stream, &out);
EXPECT_EQ("", out.str());

stan::mcmc::chains<> chains(blocker1);
chains.add(blocker2);
Eigen::Matrix<Eigen::VectorXd, Eigen::Dynamic, 1> samples(
        chains.num_chains());
std::vector<const double*> draws(chains.num_chains());
std::vector<size_t> sizes(chains.num_chains());

for (int index = 4; index < chains.num_params(); index++) {
  for (int chain = 0; chain < chains.num_chains(); ++chain) {
    samples(chain) = chains.samples(chain, index);
    draws[chain] = &samples(chain)(0);
    sizes[chain] = samples(chain).size();
  }
}

Eigen::VectorXd x(6);
x << 1, 2, 3, 4, 5, 6;
std::fstream data_stream(std::string("").c_str(), std::fstream::in);
stan::io::dump data_var_context(data_stream);
data_stream.close();

std::stringstream output;
stan_model model(data_var_context, 0, static_cast<std::stringstream*>(0));

Eigen::VectorXd cont_params = Eigen::VectorXd::Zero(2);
cont_params(0) = 0.75;
cont_params(1) = 0.75;
rng_t base_rng;
base_rng.seed(3021828106u);

double ess, mcse;
int window = 5;
stan::variational::advi<stan_model, stan::variational::normal_meanfield, rng_t>
  test_advi(model, cont_params, base_rng, 10, 10, 100, 1);
  int num_chains = draws.size();
  std::vector<size_t> windows(num_chains, window);
  test_advi.ESS_MCSE(&ess, &mcse, draws, windows); // J chains of W parameters stored

std::cout << "ess " << ess << "mcse " << mcse << std::endl;
}

//TEST(RVI_SUBMODULE_TEST, gpdfit){
//    // check values are equal to loo's gpdfit()
//    double advi_k, advi_sigma;
//    Eigen::VectorXd x(6);
//    x << 1, 2, 3, 4, 5, 6;
//    double loo_wip_k = 0.0275548, loo_wip_sigma = 5.616346; // wip = true
//    double loo_nowip_k = -0.7598539, loo_nowip_sigma = 5.616346; // wip = false
//
//    std::fstream data_stream(std::string("").c_str(), std::fstream::in);
//    stan::io::dump data_var_context(data_stream);
//    data_stream.close();
//
//    std::stringstream output;
//    stan_model model(data_var_context, 0, static_cast<std::stringstream*>(0));
//
//    Eigen::VectorXd cont_params = Eigen::VectorXd::Zero(2);
//    cont_params(0) = 0.75;
//    cont_params(1) = 0.75;
//    rng_t base_rng;
//    base_rng.seed(3021828106u);
//
//    stan::variational::advi<stan_model, stan::variational::normal_meanfield, rng_t>
//        test_advi(model, cont_params, base_rng, 10,
//                    10, 100, 1);
//
//    test_advi.gpdfit(x, advi_k, advi_sigma, true);
//    EXPECT_FLOAT_EQ(loo_wip_k, advi_k);
//    EXPECT_FLOAT_EQ(loo_wip_sigma, advi_sigma);
//
//    test_advi.gpdfit(x, advi_k, advi_sigma, false);
//    EXPECT_FLOAT_EQ(loo_nowip_k, advi_k);
//    EXPECT_FLOAT_EQ(loo_nowip_sigma, advi_sigma);
//
//    std::cout << advi_sigma << std::endl;
//}


/*
 * Copyright 2017 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   SurveySystemTest.cpp
 * Author: glm,jordan
 *
 */

#include <Eigen/Dense>
#include <Eigen/src/Core/Matrix.h>
#include "catch.hpp"
#include "../src/SurveySystem.hpp"
#include "../src/utils/Exception.hpp"
#include <cmath>

TEST_CASE("Reading metadata from original MSPAC") {
    SurveySystem params;

    REQUIRE(params.readFile("test/data/metadata/TestMetaData.txt"));
    /*
                MultibeamModel R2Sonic2022
                AntPositionOffsetX 1.31
                AntPositionOffsetY 1.45
                AntPositionOffsetZ 4.32
                MBEOffsetX 0.765
                MBEOffsetY 0.685
                MBEOffsetZ 1.359
                MBEDraft 0.70
                MBEOffset2X 0.364
                MBEOffset2Y 0.687
                MBEOffset2Z 1.093
                PitchRollAccuracy 0.050
                HeadingAccuracy 0.050
                HeaveAccuracy 0.050
                PositionAccuracy 0.010
                PitchAlignment 1.64
                RollAlignment 0.62
                HeadingAlignment 1.88
     */

    //Test draft
    /*
                MBEDraft 0.70
     */
    double draft = 0.7;
    double draftPrecision = 0.000000001;
    REQUIRE(std::abs(params.getDraft() - draft) < draftPrecision);

    //Test model name
    /*
                MultibeamModel R2Sonic2022
     */
    std::string testModelName = "R2Sonic2022";
    REQUIRE(params.getMBES_model().compare(testModelName) == 0);

    //Test antenna position
    /*
                AntPositionOffsetX 1.31
                AntPositionOffsetY 1.45
                AntPositionOffsetZ 4.32
     */
    double antennaPrecision = 0.000000001;
    Eigen::Vector3d testAntennatPosition(1.31, 1.45, -4.32);
    Eigen::Vector3d testAntenna = params.getAntennaPosition().array() - testAntennatPosition.array();
    REQUIRE(testAntenna.cwiseAbs().maxCoeff() < antennaPrecision);

    //Test echosounder transmitter position
    /*
                MBEOffsetX 0.765
                MBEOffsetY 0.685
                MBEOffsetZ 1.359
     */
    double transmitterPrecision = 0.000000001;
    Eigen::Vector3d testTransmitterPosition(0.765, 0.685, -1.359);
    Eigen::Vector3d testTransmitter = params.getEchosounderTransmitterPosition().array() - testTransmitterPosition.array();
    REQUIRE(testTransmitter.cwiseAbs().maxCoeff() < transmitterPrecision);

    //Test echosounder transmitter position
    /*
                MBEOffset2X 0.364
                MBEOffset2Y 0.687
                MBEOffset2Z 1.093
     */
    double receiverPrecision = 0.000000001;
    Eigen::Vector3d testReceiverPosition;
    testReceiverPosition << 0.364, 0.687, -1.093;
    Eigen::Vector3d testReceiver = params.getEchosounderReceivererPosition().array() - testReceiverPosition.array();
    REQUIRE(testReceiver.cwiseAbs().maxCoeff() < receiverPrecision);

    //Test echosounder transmitter position
    /*
                PitchRollAccuracy 0.050
                HeadingAccuracy 0.050
     */
    double attitudeAccuracyPrecision = 0.000000001;
    double testPitchRollAccuracy = 0.050*D2R;
    Attitude* acc = params.getAttitudeAccuracy();
    REQUIRE(abs(acc->getRoll()*D2R-testPitchRollAccuracy) < attitudeAccuracyPrecision);
    REQUIRE(abs(acc->getPitch()*D2R-testPitchRollAccuracy) < attitudeAccuracyPrecision);
}

TEST_CASE("Reading non-existing file") {
    SurveySystem params;

    REQUIRE(!params.readFile("/non/existant/file"));
}

//TODO: test case with a few invalid values


#include <iostream>
#include <stdint.h>
#include <boost/python.hpp>
#include <thread>
#include "PythonCVMatConvert.h"
#include "PythonUtils.h"
#include "utils_RNG.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <math.h>
#include <omp.h> // OpenMP

namespace bp = boost::python;
using std::cout; using std::endl;

template <class T>
inline std::string to_istring(const T& t)
{
	std::stringstream ss;
	ss << static_cast<int>(t);
	return ss.str();
}
template <class T>
inline std::string to_sstring(const T& t) {
	std::stringstream ss;
	ss << (t);
	return ss.str();
}
#define CVMAP_MAKETYPE(thecvtype) cvdepthsmap[thecvtype] = #thecvtype
static std::map<int,std::string> cvdepthsmap;
void init_cvdepthsmap() {
	CVMAP_MAKETYPE(CV_8U);
	CVMAP_MAKETYPE(CV_16U);
	CVMAP_MAKETYPE(CV_8S);
	CVMAP_MAKETYPE(CV_16S);
	CVMAP_MAKETYPE(CV_32F);
	CVMAP_MAKETYPE(CV_64F);
}
std::string describemat(const cv::Mat & inmat) {
	std::string retstr = std::string("(rows,cols,channels,depth) == (")
					+to_istring(inmat.rows)+std::string(", ")
					+to_istring(inmat.cols)+std::string(", ")
					+to_istring(inmat.channels())+std::string(", ");
	if(cvdepthsmap.find((int)inmat.depth()) != cvdepthsmap.end()) {
		retstr += cvdepthsmap[inmat.depth()]+std::string(")");
	} else {
		retstr += to_istring(inmat.depth())+std::string(")");
	}
	double minval,maxval;
	cv::minMaxIdx(inmat, &minval, &maxval);
	retstr += std::string(", (min,max) = (")+to_sstring(minval)+std::string(", ")+to_sstring(maxval)+std::string(")");
	return retstr;
}
//==============================================================================

#define dSQR(a) ((a)*(a))

void bilateralf(cv::Mat const& input,
								cv::Mat const& featswrt,
						    cv::Mat & out_spatial,
								cv::Mat & out_bilateral,
							  double stdv_space,
							  double stdv_color,
								int grad_chan) {

	out_spatial   = cv::Mat::zeros(input.size(), input.type());
	out_bilateral = cv::Mat::zeros(input.size(), input.type());

	assert(input.rows == featswrt.rows && input.cols == featswrt.cols);
	assert(input.rows == out_spatial.rows && input.cols == out_spatial.cols);
	assert(input.rows == out_bilateral.rows && input.cols == out_bilateral.cols);
	assert(input.channels() == out_spatial.channels() && input.channels() == out_bilateral.channels());
	const int nchans_in  = input.channels();
	const int nchans_wrt = featswrt.channels();
	stdv_space *= stdv_space; // convert to variances
	stdv_color *= stdv_color;
	assert(grad_chan < (2+nchans_wrt));

	double const*const data_input    = (double*)input.data;
	double const*const data_featswrt = (double*)featswrt.data;
	double *const data_out_spatial    = (double*)out_spatial.data;
	double *const data_out_bilateral  = (double*)out_bilateral.data;
	const int step_in  = input.cols*nchans_in;
	const int step_wrt = featswrt.cols*nchans_wrt;

	#pragma omp parallel for
	for(int ii=0; ii<out_spatial.rows; ++ii) {
		// declare these inside the loop, so OpenMP creates one per thread
		double const* elin_oth = nullptr;
		double const* elft_oth = nullptr;
		int cc;
		double dists[2+nchans_wrt];
		double gauss_sp, gauss_bi;
		// each thread processes a row, and loops over all columns
		for(int jj=0; jj<out_spatial.cols; ++jj) {
			double* elou_sp = data_out_spatial   + ii*step_in  + jj*nchans_in;
			double* elou_bi = data_out_bilateral + ii*step_in  + jj*nchans_in;
			double const*const elft_ref = data_featswrt     + ii*step_wrt + jj*nchans_wrt;
			// iterate over all other pixels
			for(int mm=0; mm<input.rows; ++mm) {
				for(int nn=0; nn<input.cols; ++nn) {
					// distances
					dists[0] = (double)(ii-mm);
					dists[1] = (double)(jj-nn);
					elin_oth =    data_input + mm*step_in  + nn*nchans_in;
					elft_oth = data_featswrt + mm*step_wrt + nn*nchans_wrt;
					//vectype_wrt& elin =    input.at<vectype_wrt>(mm,nn);
					//vectype_wrt& elft = featswrt.at<vectype_wrt>(mm,nn);
					for(cc=0; cc<nchans_wrt; ++cc) {
						dists[cc+2] = (elft_ref[cc] - elft_oth[cc]);
					}
					// compute outputs
					gauss_sp = exp(-0.5* (dSQR(dists[0])+dSQR(dists[1]))/stdv_space);
					gauss_bi = exp(-0.5*((dSQR(dists[0])+dSQR(dists[1]))/stdv_color + dSQR(dists[2])+dSQR(dists[3])+dSQR(dists[4]))); //unit stdv for color dist

					if(grad_chan >= 0) {
						if(grad_chan < 2) {
							gauss_sp *= (0.5*dists[grad_chan]/stdv_space);
							gauss_bi *= (0.5*dists[grad_chan]/stdv_color);
						} else {
							gauss_bi *= (0.5*dists[grad_chan]);
						}
					}

					for(cc=0; cc<nchans_in; ++cc) {
						elou_sp[cc] += gauss_sp * elin_oth[cc];
						elou_bi[cc] += gauss_bi * elin_oth[cc];
					}
				}
			}
		}
	}
}

bp::object MyTestGradients(bp::list pyImagesBatch,
														double stdv_space,
														double stdv_color,
														int grad_chan)
{
	init_cvdepthsmap();
	int ii, jj;
	const int numimgs = bp::len(pyImagesBatch);
	if(numimgs <= 0) {
		cout<<"MyTestGradients: error: no images in batch"<<endl<<std::flush;
		return bp::object();
	}
	if(numimgs != 5) {
		cout<<"error: expect these images in batch:"<<endl;
		cout<<"0: input, 1: featswrt, 2: outspatial, 3: outbilat, 4: diffs_outbilat"<<endl<<std::flush;
		return bp::object();
	}

	NDArrayConverter cvt;
	bp::list returnedPyImages; //will be a list of cv2 numpy images
	std::vector<cv::Mat> expectedimages(numimgs);

	for(ii=0; ii < numimgs; ii++) {
		bp::object imgobject = bp::extract<bp::object>(pyImagesBatch[ii]);
		expectedimages[ii] = cvt.toMat(imgobject.ptr());

		cout << "img " << ii << ": " << describemat(expectedimages[ii]) << endl;
	}

	cv::Mat norm_input = cv::Mat::ones(expectedimages[0].size(), expectedimages[0].type());
	const int nchan = expectedimages[0].channels();
	const int npix = expectedimages[0].rows*expectedimages[0].cols;
	for(ii=0; ii<npix; ++ii) {
		for(jj=0; jj<nchan; ++jj) {
			((double*)norm_input.data)[ii*nchan + jj] = 1.0;
		}
	}
	cv::Mat norm_space(expectedimages[0].size(), expectedimages[0].type());
	cv::Mat norm_bilat(expectedimages[0].size(), expectedimages[0].type());

	cout<<"norm_input: "<<describemat(norm_input)<<endl;

	bilateralf(norm_input, expectedimages[1], norm_space, norm_bilat,
						 stdv_space, stdv_color, -1);

	bilateralf(expectedimages[0], expectedimages[1], expectedimages[2], expectedimages[3],
						 stdv_space, stdv_color, grad_chan);

  cout<<endl;
	cout<<"expectedimages[2]: "<<describemat(expectedimages[2])<<endl;
	cout<<"expectedimages[3]: "<<describemat(expectedimages[3])<<endl;
	cout<<"norm_space: "<<describemat(norm_space)<<endl;
	cout<<"norm_bilat: "<<describemat(norm_bilat)<<endl;
	cout<<endl;
	cv::divide(expectedimages[2], norm_space, expectedimages[2]);
	cv::divide(expectedimages[3], norm_bilat, expectedimages[3]);

	for(ii=0; ii < numimgs; ii++) {
		PyObject* thisImgCPP = cvt.toNDArray(expectedimages[ii]);
		//returnedPyImages.append(bp::object(bp::handle<>(bp::borrowed(thisImgCPP)))); // borrowed() increments the reference counter, causing a memory leak when we return to Python
		returnedPyImages.append(bp::object(bp::handle<>(thisImgCPP)));
	}

	return returnedPyImages;
}

static void init() {
	Py_Initialize();
	import_array();
	init_cvdepthsmap();
}
BOOST_PYTHON_MODULE(pymytestgradslib)
{
	init();
	bp::def("MyTestGradients", MyTestGradients);
}


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

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

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

#include <boost/simd/detail/enforce_precision.hpp>
#include <boost/simd/function/simd/if_zero_else.hpp>
#include <boost/simd/function/simd/is_inf.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( two_add_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0,A0 const& a1,A0& a3) const BOOST_NOEXCEPT
    {
      A0 a2;
      boost::simd::two_add(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( two_add_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using result_t = std::pair<A0,A0>                                  ;

    BOOST_FORCEINLINE result_t operator() ( A0 const& a0,A0 const& a1) const BOOST_NOEXCEPT
    {
      A0 first, second;
      boost::simd::two_add( a0, a1, first, second );
      return {first, second};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( two_add_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE void operator() ( A0 const& a,A0 const& b, A0 & r0,A0 & r1) const BOOST_NOEXCEPT
    {
      detail::enforce_precision<A0> enforcer;
      r0   = a  + b;
      A0 z = r0 - a;
#if defined(BOOST_SIMD_NO_INFINITIES)
      r1   = a-(r0-z)+(b-z);
#else
      r1   = if_zero_else(is_inf(r0), (a-(r0-z))+(b-z));
#endif
    }
  };
} } }

#endif


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

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

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

#include <boost/simd/function/definition/sincosd.hpp>
#include <boost/simd/arch/common/generic/function/sincosd.hpp>

#endif


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

  @copyright 2012-2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

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

#include <boost/simd/function/definition/divfix.hpp>
#include <boost/simd/arch/common/scalar/function/divfix.hpp>

#endif


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/import_scitbx_af.h>
#include <scitbx/array_family/versa.h>
#include <scitbx/array_family/accessors/flex_grid.h>
#include <boost/python/def.hpp>

namespace cctbx { namespace maptbx { namespace boost_python {

namespace {

  template <typename ElementType>
  struct set_if_less_than
  {
    static void
    run(
      af::versa<ElementType, af::flex_grid<> >& a,
      ElementType const& threshold_value,
      ElementType const& imposed_value)
    {
      for(std::size_t i=0;i<a.size();i++) {
        if (a[i] < threshold_value) a[i] = imposed_value;
      }
    }
  };

} // namespace <anoymous>

  void wrap_misc()
  {
    using namespace boost::python;
    def("set_if_less_than", set_if_less_than<float>::run);
    def("set_if_less_than", set_if_less_than<double>::run);
  }

}}} // namespace cctbx::maptbx::boost_python


#ifndef STAN_MATH_REV_MAT_FUN_QUAD_FORM_HPP
#define STAN_MATH_REV_MAT_FUN_QUAD_FORM_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/typedefs.hpp>
#include <stan/math/rev/mat/fun/typedefs.hpp>
#include <stan/math/prim/mat/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/quad_form.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>

namespace stan {
  namespace math {

    namespace {
      template <typename TA, int RA, int CA, typename TB, int RB, int CB>
      class quad_form_vari_alloc : public chainable_alloc {
      private:
        inline void compute(const Eigen::Matrix<double, RA, CA>& A,
                            const Eigen::Matrix<double, RB, CB>& B) {
          Eigen::Matrix<double, CB, CB> Cd(B.transpose()*A*B);
          for (int j = 0; j < C_.cols(); j++) {
            for (int i = 0; i < C_.rows(); i++) {
              if (sym_) {
                C_(i, j) = var(new vari(0.5*(Cd(i, j) + Cd(j, i)), false));
              } else {
                C_(i, j) = var(new vari(Cd(i, j), false));
              }
            }
          }
        }

      public:
        quad_form_vari_alloc(const Eigen::Matrix<TA, RA, CA>& A,
                             const Eigen::Matrix<TB, RB, CB>& B,
                             bool symmetric = false)
          : A_(A), B_(B), C_(B_.cols(), B_.cols()), sym_(symmetric) {
          compute(value_of(A), value_of(B));
        }

        Eigen::Matrix<TA, RA, CA>  A_;
        Eigen::Matrix<TB, RB, CB>  B_;
        Eigen::Matrix<var, CB, CB> C_;
        bool sym_;
      };

      template <typename TA, int RA, int CA, typename TB, int RB, int CB>
      class quad_form_vari : public vari {
      protected:
        inline void chainA(Eigen::Matrix<double, RA, CA>& A,
                           const Eigen::Matrix<double, RB, CB>& Bd,
                           const Eigen::Matrix<double, CB, CB>& adjC) {}
        inline void chainB(Eigen::Matrix<double, RB, CB>& B,
                           const Eigen::Matrix<double, RA, CA>& Ad,
                           const Eigen::Matrix<double, RB, CB>& Bd,
                           const Eigen::Matrix<double, CB, CB>& adjC) {}

        inline void chainA(Eigen::Matrix<var, RA, CA>& A,
                           const Eigen::Matrix<double, RB, CB>& Bd,
                           const Eigen::Matrix<double, CB, CB>& adjC) {
          Eigen::Matrix<double, RA, CA>     adjA(Bd*adjC*Bd.transpose());
          for (int j = 0; j < A.cols(); j++) {
            for (int i = 0; i < A.rows(); i++) {
              A(i, j).vi_->adj_ += adjA(i, j);
            }
          }
        }
        inline void chainB(Eigen::Matrix<var, RB, CB>& B,
                           const Eigen::Matrix<double, RA, CA>& Ad,
                           const Eigen::Matrix<double, RB, CB>& Bd,
                           const Eigen::Matrix<double, CB, CB>& adjC) {
          Eigen::Matrix<double, RA, CA> adjB(Ad * Bd * adjC.transpose()
                                             + Ad.transpose()*Bd*adjC);
          for (int j = 0; j < B.cols(); j++)
            for (int i = 0; i < B.rows(); i++)
              B(i, j).vi_->adj_ += adjB(i, j);
        }

        inline void chainAB(Eigen::Matrix<TA, RA, CA>& A,
                            Eigen::Matrix<TB, RB, CB>& B,
                            const Eigen::Matrix<double, RA, CA>& Ad,
                            const Eigen::Matrix<double, RB, CB>& Bd,
                            const Eigen::Matrix<double, CB, CB>& adjC) {
          chainA(A, Bd, adjC);
          chainB(B, Ad, Bd, adjC);
        }

      public:
        quad_form_vari(const Eigen::Matrix<TA, RA, CA>& A,
                       const Eigen::Matrix<TB, RB, CB>& B,
                       bool symmetric = false)
          : vari(0.0) {
          impl_
            = new quad_form_vari_alloc<TA, RA, CA, TB, RB, CB>(A, B, symmetric);
        }

        virtual void chain() {
          Eigen::Matrix<double, CB, CB> adjC(impl_->C_.rows(),
                                             impl_->C_.cols());

          for (int j = 0; j < impl_->C_.cols(); j++)
            for (int i = 0; i < impl_->C_.rows(); i++)
              adjC(i, j) = impl_->C_(i, j).vi_->adj_;

          chainAB(impl_->A_, impl_->B_,
                  value_of(impl_->A_), value_of(impl_->B_),
                  adjC);
        }

        quad_form_vari_alloc<TA, RA, CA, TB, RB, CB> *impl_;
      };
    }

    template <typename TA, int RA, int CA, typename TB, int RB, int CB>
    inline typename
    boost::enable_if_c< boost::is_same<TA, var>::value ||
    boost::is_same<TB, var>::value,
                        Eigen::Matrix<var, CB, CB> >::type
      quad_form(const Eigen::Matrix<TA, RA, CA>& A,
                const Eigen::Matrix<TB, RB, CB>& B) {
      check_square("quad_form", "A", A);
      check_multiplicable("quad_form",
                          "A", A,
                          "B", B);

      quad_form_vari<TA, RA, CA, TB, RB, CB> *baseVari
        = new quad_form_vari<TA, RA, CA, TB, RB, CB>(A, B);

      return baseVari->impl_->C_;
    }
    template <typename TA, int RA, int CA, typename TB, int RB>
    inline typename
    boost::enable_if_c< boost::is_same<TA, var>::value ||
    boost::is_same<TB, var>::value,
                        var >::type
      quad_form(const Eigen::Matrix<TA, RA, CA>& A,
                const Eigen::Matrix<TB, RB, 1>& B) {
      check_square("quad_form", "A", A);
      check_multiplicable("quad_form",
                          "A", A,
                          "B", B);

      quad_form_vari<TA, RA, CA, TB, RB, 1> *baseVari
        = new quad_form_vari<TA, RA, CA, TB, RB, 1>(A, B);

      return baseVari->impl_->C_(0, 0);
    }

  }
}
#endif


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include "boost/program_options.hpp"

#include "Molassembler/Shapes/ContinuousMeasures.h"

#include "Molassembler/Shapes/Data.h"
#include "Molassembler/Shapes/PropertyCaching.h"

#include "boost/math/tools/minima.hpp"
#include "Molassembler/Temple/constexpr/Numeric.h"
#include "Molassembler/Temple/Adaptors/AllPairs.h"
#include "Molassembler/Temple/Adaptors/Iota.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/Stringify.h"
#include "Molassembler/Temple/Random.h"
#include "Molassembler/Temple/constexpr/Jsf.h"
#include "Molassembler/Temple/Permutations.h"
#include "Molassembler/Temple/Loops.h"

#include <Eigen/SparseCore>
#include <Eigen/Eigenvalues>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <memory>
#include <chrono>

/* Summary
 *
 * This file contains a number of methods trying to cheapen finding the correct
 * vertex mapping to evaluate the continuous shape measure.
 *
 * Note that the problem does have some rotational redundancy depending on the
 * number of rotations that the shape has. But exploiting that gets you maybe
 * (N - 1)!. Crasser solutions are necessary.
 */

using namespace Scine;
using namespace Molassembler;
using namespace Shapes;
using namespace Continuous;

constexpr unsigned factorial(unsigned x) {
  if(x <= 1) {
    return 1;
  }

  return x * factorial(x - 1);
}

template<std::size_t ... Inds>
constexpr auto makeFactorials(std::index_sequence<Inds ...> /* inds */) {
  return std::array<unsigned, sizeof...(Inds)> {
    factorial(Inds + 1)...
  };
}

constexpr auto facs = makeFactorials(std::make_index_sequence<14> {});

Continuous::PositionCollection addOrigin(const Continuous::PositionCollection& vs) {
  const unsigned N = vs.cols();
  Continuous::PositionCollection positions(3, N + 1);
  for(unsigned i = 0; i < N; ++i) {
    positions.col(i) = vs.col(i);
  }

  // Add origin point explicitly to consideration
  positions.col(N) = Eigen::Vector3d::Zero(3);
  return positions;
}

void distort(Eigen::Ref<Continuous::PositionCollection> positions, const double distortionNorm = 0.01) {
  const unsigned N = positions.cols();
  for(unsigned i = 0; i < N; ++i) {
    positions.col(i) += distortionNorm * Eigen::Vector3d::Random().normalized();
  }
}

template<typename Derived>
bool centroidIsZero(const Eigen::MatrixBase<Derived>& a) {
  assert(a.rows() == 3);
  return (a.rowwise().sum() / a.cols()).squaredNorm() < 1e-8;
}

template<typename DerivedA, typename DerivedB>
Eigen::Matrix3d fitQuaternion(const Eigen::MatrixBase<DerivedA>& stator, const Eigen::MatrixBase<DerivedB>& rotor) {
  assert(centroidIsZero(stator));
  assert(centroidIsZero(rotor));

  Eigen::Matrix4d b = Eigen::Matrix4d::Zero();
  // generate decomposable matrix per atom and add them
  for (int i = 0; i < rotor.cols(); i++) {
    auto& rotorCol = rotor.col(i);
    auto& statorCol = stator.col(i);

    Eigen::Matrix4d a = Eigen::Matrix4d::Zero();
    a.block<1, 3>(0, 1) = (rotorCol - statorCol).transpose();
    a.block<3, 1>(1, 0) = statorCol - rotorCol;
    a.block<3, 3>(1, 1) = Eigen::Matrix3d::Identity().rowwise().cross(statorCol + rotorCol);
    b += a.transpose() * a;
  }

  // Decompose b
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(b);

  // Do not allow improper rotation
  const Eigen::Vector4d& q = eigensolver.eigenvectors().col(0);
  return Eigen::Quaterniond(q[0], q[1], q[2], q[3]).toRotationMatrix();
}

using Parameters = std::vector<Vertex>;

struct Energy {
  const PositionCollection& referencePositions;
  const PositionCollection& shapePositions;

  double operator() (const Parameters& p) const {
    const unsigned N = shapePositions.cols();

    static PositionCollection permuted(3, N);
    for(unsigned i = 0; i < N; ++i) {
      permuted.col(i) = shapePositions.col(p[i]);
    }

    auto rotationMatrix = fitQuaternion(referencePositions, permuted);
    permuted = rotationMatrix * permuted;

    return (referencePositions - permuted).colwise().squaredNorm().sum();

    // // Minimize over isotropic scaling factor
    // auto scalingMinimizationResult = boost::math::tools::brent_find_minima(
    //   [&](const double scaling) -> double {
    //     return (referencePositions - scaling * permuted).colwise().squaredNorm().sum();
    //   },
    //   0.5,
    //   1.1,
    //   std::numeric_limits<double>::digits
    // );

    // return scalingMinimizationResult.second;
  }
};

struct OldShapeAlgorithm {
  virtual ~OldShapeAlgorithm() = default;

  static PositionCollection shapeCoordinates(const Shape shape) {
    return normalize(addOrigin(coordinates(shape)));
  }

  virtual double shape(const PositionCollection& positions, Shape shape) = 0;
  virtual std::string name() const = 0;
};

/**
 * @brief Abstract base class for algorithms to conform to
 */
struct ShapeAlgorithm {
  virtual ~ShapeAlgorithm() = default;

  using Permutation = std::vector<Vertex>;
  using ResultPair = std::pair<double, Permutation>;

  static Permutation invert(const Permutation& p) {
    const unsigned P = p.size();
    Permutation inverse(P);
    for(unsigned i = 0; i < P; ++i) {
      inverse.at(p.at(i)) = i;
    }
    return inverse;
  }

  /* A forwards permutation maps from positions to shape indices,
   * a backwards permutation maps from shape indices to positions
   *
   * Only backwards permutations have the (added) centroid at the back, and
   * hence their [O, S] = [0, N - 1] = [0, N) ranges can be rotated.
   */
  static std::set<ShapeAlgorithm::Permutation> generateRotations(
    const Permutation& forwardsPermutation,
    const Shape shape
  ) {
    auto backwards = ShapeAlgorithm::invert(forwardsPermutation);
    Vertex centroid = backwards.back();
    backwards.pop_back();
    auto rotations = Properties::generateAllRotations(shape, backwards);

    std::set<ShapeAlgorithm::Permutation> forwardsPermutationRotations;
    for(auto rotation : rotations) {
      rotation.push_back(centroid);
      forwardsPermutationRotations.emplace(
        ShapeAlgorithm::invert(rotation)
      );
    }
    return forwardsPermutationRotations;
  }

  static PositionCollection shapeCoordinates(const Shape shape) {
    return normalize(addOrigin(coordinates(shape)));
  }

  virtual ResultPair shape(
    const PositionCollection& positions,
    Shape shape,
    const Permutation& correctPermutation
  ) = 0;
  virtual std::string name() const = 0;
};

/**
 * @brief Reference continuous shape measure calculation method
 *
 * Complexity: Theta(N!)
 */
struct AllPermutations {
  ShapeAlgorithm::ResultPair shape(const PositionCollection& positions, Shape shape) {
    Energy energy {positions, ShapeAlgorithm::shapeCoordinates(shape)};
    ShapeAlgorithm::Permutation permutation = Temple::iota<Vertex>(Vertex(positions.cols()));
    ShapeAlgorithm::ResultPair minimal {std::numeric_limits<double>::max(), {}};
    do {
      double permutationEnergy = energy(permutation);

      if(permutationEnergy < minimal.first) {
        minimal = {permutationEnergy, permutation};
      }
    } while(Temple::next_permutation(permutation));

    return minimal;
  }

  std::string name() const {
    return "Reference";
  }
};

struct Cooling {
  // This works best for annealing and tunneling
  static double linear(const unsigned step, const unsigned steps) {
    return 1.0 - (static_cast<double>(step) / steps);
  }

  // This doesn't work well at all for annealing or tunneling, but it's here
  static double exponential(const unsigned step, const unsigned steps) {
    const double alpha = std::exp(std::log(1e-2) / steps);
    return std::pow(alpha, step);
  }
};

/**
 * @brief Simulated annealing method
 *
 * With well tuned temperature, this works pretty well, but it's stochastic in
 * nature and its good results are due to tracking the lowest energy state
 * visited instead of using the final annealing state. The energy surface for
 * small distortion norms is super jagged and the "correct" state has no
 * low-energy neighbors, making this type of approach problematic. Behaves
 * better for high distortion norms as there state space can be better sampled.
 *
 * Complexity: Needs at least O((N - 2)!) steps to explore enough state space
 * to encounter the minimum.
 */
struct Anneal final : OldShapeAlgorithm {
  using Permutation = std::vector<Vertex>;

  Temple::JSF64 prng;
  std::ofstream trace {"anneal_trace.csv"};

  Anneal() {
    prng.seed(0);
  }

  static double acceptanceProbability(
    const double energy,
    const double prospectiveEnergy,
    const double temperature
  ) {
    if(prospectiveEnergy < energy) {
      return 1.0;
    }

    return std::exp(-(prospectiveEnergy - energy) / temperature);
  }

  static double temperature(const unsigned step, const unsigned steps) {
    return Cooling::linear(step, steps);
  }

  //! Assumes temperature in 0, 1
  Permutation generateMove(Permutation state, const double temperature) {
    const unsigned N = state.size();

    const unsigned nSwaps = 1 + std::round(temperature * Temple::Random::getSingle<unsigned>(0, N - 1, prng));
    for(unsigned s = 0; s < nSwaps; ++s) {
      const unsigned i = Temple::Random::getSingle<unsigned>(0, N - 2, prng);
      std::swap(state.at(i), state.at(i + 1));
    }

    return state;
  }

  double shape(const PositionCollection& positions, Shape shape) final {
    const unsigned steps = facs.at(positions.cols() - 2);
    Energy energy {positions, shapeCoordinates(shape)};

    auto parameters = Temple::iota<Vertex>(positions.cols());
    decltype(parameters) prospectiveParameters;

    double temperatureMultiplier = Temple::accumulate(
      Temple::Adaptors::range(10),
      0.0,
      [&](const double carry, unsigned /* i */) -> double {
        Temple::Random::shuffle(parameters, prng);
        return carry + energy(parameters);
      }
    ) / 10;

    double currentEnergy = energy(parameters);
    double minimalEnergy = currentEnergy;
    for(unsigned step = 0; step < steps; ++step) {
      const double currentTemperature = temperature(step, steps);
      prospectiveParameters = generateMove(parameters, currentTemperature);
      const double prospectiveEnergy = energy(prospectiveParameters);
      const double p = acceptanceProbability(currentEnergy, prospectiveEnergy, temperatureMultiplier * currentTemperature);

      trace << step << ", " << currentEnergy << ", " << temperatureMultiplier * currentTemperature << ", " << prospectiveEnergy << ", " << p << "\n";

      if(p >= Temple::Random::getSingle(0.0, 1.0, prng)) {
        std::swap(parameters, prospectiveParameters);
        currentEnergy = prospectiveEnergy;
      }

      minimalEnergy = std::min(minimalEnergy, prospectiveEnergy);
    }

    return minimalEnergy;
  }

  std::string name() const final {
    return "Annealing";
  }
};

/**
 * @brief Stochastic tunneling method
 *
 * This works by transforming the energy function to level out minima that have
 * higher function values than the lowest found. This then should help losing
 * time in unhelpful minima.
 *
 * Works pretty well even when the temperature is not well tuned. You have to
 * adjust the gamma parameter for the particular problem you have, and the
 * value we have now works well for CShMs.
 *
 * But now that simulated annealing is well tuned and since there aren't really
 * and multi-state minima to get stuck in in this energy surface for small
 * distortions, this doesn't perform any better.
 *
 * The move generator for this and for simulated annealing generate multi-swap
 * moves for high temperatures and increase locality as the temperature is
 * lowered, generating only single adjacent swap moves.
 *
 * Complexity: Needs at least O((N - 2)!) steps to explore enough state space
 * to encounter the minimum.
 */
struct Tunnel final : OldShapeAlgorithm {
  using Permutation = std::vector<Vertex>;

  Temple::JSF64 prng;
  std::ofstream trace {"tunnel_trace.csv"};
  double gamma = 0.1;

  Tunnel() {
    prng.seed(0);
  }

  static double acceptanceProbability(
    const double energy,
    const double prospectiveEnergy,
    const double temperature
  ) {
    if(prospectiveEnergy < energy) {
      return 1.0;
    }

    return std::exp(-(prospectiveEnergy - energy) / temperature);
  }

  //! Assumes parameters are untransformed!
  double energyTransformation(const double energy, const double lowestEnergy) const {
    return 1.0 - std::exp(-gamma * (energy - lowestEnergy));
  }

  static double temperature(const unsigned step, const unsigned steps) {
    return Cooling::linear(step, steps);
  }

  //! Assumes temperature in 0, 1
  Permutation generateMove(Permutation state, const double temperature) {
    const unsigned N = state.size();

    const unsigned nSwaps = 1 + std::round(temperature * Temple::Random::getSingle<unsigned>(0, N - 1, prng));
    for(unsigned s = 0; s < nSwaps; ++s) {
      const unsigned i = Temple::Random::getSingle<unsigned>(0, N - 2, prng);
      std::swap(state.at(i), state.at(i + 1));
    }

    return state;
  }

  double shape(const PositionCollection& positions, Shape shape) final {
    const unsigned steps = facs.at(positions.cols() - 2);
    Energy energy {positions, shapeCoordinates(shape)};

    auto parameters = Temple::iota<Vertex>(positions.cols());
    decltype(parameters) prospectiveParameters;

    double currentEnergy = energy(parameters);
    double lowestEnergy = currentEnergy;
    double transformedCurrentEnergy = energyTransformation(currentEnergy, lowestEnergy);

    for(unsigned step = 0; step < steps; ++step) {
      const double currentTemperature = temperature(step, steps);
      prospectiveParameters = generateMove(parameters, currentTemperature);
      const double prospectiveEnergy = energy(prospectiveParameters);
      const double transformedProspectiveEnergy = energyTransformation(prospectiveEnergy, lowestEnergy);
      const double p = acceptanceProbability(transformedCurrentEnergy, transformedProspectiveEnergy, 0.7 * currentTemperature);

      trace << step << ", " << currentEnergy << ", " << 0.7 * currentTemperature << ", " << prospectiveEnergy << ", " << p << "\n";

      if(p >= Temple::Random::getSingle(0.0, 1.0, prng)) {
        std::swap(parameters, prospectiveParameters);
        currentEnergy = prospectiveEnergy;
        transformedCurrentEnergy = transformedProspectiveEnergy;
      }

      if(prospectiveEnergy < lowestEnergy) {
        lowestEnergy = prospectiveEnergy;
        transformedCurrentEnergy = energyTransformation(currentEnergy, lowestEnergy);
      }
    }

    return lowestEnergy;
  }

  std::string name() const final {
    return "Tunneling";
  }
};

template<typename T, std::size_t N>
struct CircularBuffer {
  void insert(T value) {
    if(size_ < N) {
      buffer_[size_] = std::move(value);
      ++size_;
    } else {
      buffer_[start_] = std::move(value);
      start_ = (start_ + 1) % N;
    }
  }

  T min() const {
    assert(size_ > 0);
    return *std::min_element(
      std::begin(buffer_),
      std::end(buffer_)
    );
  }

  double average() const {
    return static_cast<double>(
      std::accumulate(
        std::begin(buffer_),
        std::end(buffer_),
        T {0},
        std::plus<>()
      )
    ) / size_;
  }

  double variance(const double average) const {
    return static_cast<double>(
      std::accumulate(
        std::begin(buffer_),
        std::end(buffer_),
        T {0},
        [&](const double carry, const double value) -> double {
          return carry + std::pow(value - average, 2);
        }
      )
    ) / size_;
  }

  void clear() {
    size_ = 0;
    start_ = 0;
  }

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

  std::array<T, N> buffer_;
  std::size_t start_ = 0;
  std::size_t size_ = 0;
};

/**
 * @brief An attempt at thermodynamic simulated annealing
 *
 * In thermodynamic simulated annealing, you try to exploit statistical
 * mechanics / thermodynamics to drive your cooling schedule optimally.
 *
 * However, I couldn't ever get this to work. The temperature adjustments are
 * always too large and no amount of messing about ever got them right.
 *
 * Complexity: ???
 */
struct ThermodynamicAnneal final : OldShapeAlgorithm {
  Temple::JSF64 prng;
  using Permutation = std::vector<Vertex>;

  std::unordered_map<unsigned, unsigned> stateIndexReduction;
  std::vector<double> energies;
  Eigen::SparseMatrix<unsigned> Q;
  CircularBuffer<double, 1000> lastEnergies;
  std::ofstream trace {"thermo_trace.csv"};

  ThermodynamicAnneal() {
    prng.seed(0);
  }

  static double acceptanceProbability(
    const double energy,
    const double prospectiveEnergy,
    const double temperature
  ) {
    if(prospectiveEnergy < energy) {
      return 1.0;
    }

    return std::exp(-(prospectiveEnergy - energy) / temperature);
  }

  //! Assumes temperature in 0, 100
  Permutation generateMove(Permutation state, const double temperature) {
    const unsigned N = state.size();

    const unsigned nSwaps = 1 + std::round((temperature / 10) * Temple::Random::getSingle<unsigned>(0, N - 1, prng));
    for(unsigned s = 0; s < nSwaps; ++s) {
      const unsigned i = Temple::Random::getSingle<unsigned>(0, N - 2, prng);
      std::swap(state.at(i), state.at(i + 1));
    }

    return state;
  }

  double partitionFunction(const double temperature) const {
    return Temple::accumulate(
      energies,
      0.0,
      [&](const double carry, const double energy) -> double {
        return carry + std::exp(-energy / temperature);
      }
    );
  }

  template<typename F>
  static double centralDifference(F&& f, const double x, const double h) {
    return (
      f(x + h) - f(x - h)
    ) / (2 * h);
  }

  double averageEnergy(const double temperature) const {
    // Estimate dln Z/dT by central finite difference
    return temperature * temperature * centralDifference(
      [&](const double x) { return std::log(partitionFunction(x)); },
      temperature,
      1e-4
    );
  }

  double heatCapacity(const double temperature) const {
    return centralDifference(
      [&](const double x) { return averageEnergy(x); },
      temperature,
      1e-4
    );
  }

  double relaxationTime(const double temperature) const {
    // Q is strictly lower triangular (i.e. always access i > j)
    Eigen::SparseMatrix<double> G(Q.rows(), Q.cols());
    for(Eigen::Index k = 0; k < Q.outerSize(); ++k) {
      // Sum up all values in Q's column
      double sum = 0;
      for(Eigen::SparseMatrix<unsigned>::InnerIterator it(Q,k); it; ++it) {
        sum += it.value();
      }

      for(Eigen::SparseMatrix<unsigned>::InnerIterator it(Q,k); it; ++it) {
        if(it.row() != it.col()) {
          const double energyDiff = energies.at(it.row()) - energies.at(it.col());
          if(energyDiff > 0) {
            G.coeffRef(it.row(), it.col()) = it.value() * std::exp(-energyDiff / temperature);
          } else {
            G.coeffRef(it.row(), it.col()) = it.value();
          }
        }
      }

      // Diagonal entries of G are 1 - other column's entries
      sum = 0;
      for(Eigen::SparseMatrix<double>::InnerIterator it(G,k); it; ++it) {
        sum += it.value();
      }
      G.coeffRef(k, k) = 1 - sum;
    }

    Eigen::SelfAdjointEigenSolver<Eigen::SparseMatrix<double>> solver(G);
    return - 1.0 / solver.eigenvalues()(1);
  }

  double updateTemperature(const double temperature, double minimalEnergy) const {
    const double hundredEnergiesAverage = lastEnergies.average();
    const double hundredEnergiesVariance = lastEnergies.variance(hundredEnergiesAverage);
    const double thermodynamicSpeed = (hundredEnergiesAverage - minimalEnergy) / hundredEnergiesVariance;

    const double epsilon = relaxationTime(temperature);
    const double C = heatCapacity(temperature);

    double Theta = 1 + temperature * centralDifference([&](double x) { return heatCapacity(x);}, temperature, 1e-4) / (2 * C);
    auto calculateDelta = [&](const double delta) {
      double correction = std::sqrt(1 + (Theta * epsilon * delta) / temperature);
      return - thermodynamicSpeed * temperature / (
        epsilon * std::sqrt(C) * correction
      );
    };

    // NOTE: this can diverge too :(
    double delta = -1.0;
    for(unsigned i = 0; i < 10; ++i) {
      delta = calculateDelta(delta);
    }

    // NOTE 1e-5 is abitrary, the updates are always too large!
    return std::max(0.0, temperature + 1e-5 * delta);
  }

  double shape(const PositionCollection& positions, Shape shape) final {
    const unsigned steps = 5e5;

    stateIndexReduction.clear();
    energies.clear();
    energies.reserve(100);
    Q.resize(0,0);
    Q.reserve(100);
    lastEnergies.clear();

    const unsigned N = positions.cols();
    Energy energy {positions, shapeCoordinates(shape)};

    auto permutation = Temple::iota<Vertex>(N);
    decltype(permutation) prospectiveMove;
    double currentEnergy = energy(permutation);
    double minimalEnergy = currentEnergy;

    unsigned iop = Temple::permutationIndex(permutation);
    stateIndexReduction.emplace(iop, 0);
    energies.push_back(currentEnergy);
    unsigned currentStateIndex = 0;

    double temperature = 10.0;

    /* First 1000 steps of annealing without changing temperature to collect
     * statistics we can use to update the temperature
     */
    unsigned step = 0;
    for(; step < 1000; ++step) {
      prospectiveMove = generateMove(permutation, temperature);
      const double prospectiveEnergy = energy(prospectiveMove);
      const double p = acceptanceProbability(currentEnergy, prospectiveEnergy, temperature);

      iop = Temple::permutationIndex(prospectiveMove);
      unsigned prospectiveStateIndex;
      auto stateIndexFindIter = stateIndexReduction.find(iop);
      if(stateIndexFindIter == std::end(stateIndexReduction)) {
        prospectiveStateIndex = stateIndexReduction.size();
        stateIndexReduction.emplace(iop, prospectiveStateIndex);
        energies.push_back(prospectiveEnergy);
        Q.conservativeResize(energies.size(), energies.size());
        Q.insert(prospectiveStateIndex, currentStateIndex) = 1;
      } else {
        prospectiveStateIndex = stateIndexFindIter->second;
        unsigned col = prospectiveStateIndex;
        unsigned row = currentStateIndex;
        if(col > row) {
          std::swap(col, row);
        }
        Q.coeffRef(row, col) = Q.coeff(row, col) + 1;
      }

      trace << step << ", " << currentEnergy << ", " << temperature << ", " << prospectiveEnergy << ", " << p << "\n";

      if(p >= Temple::Random::getSingle(0.0, 1.0, prng)) {
        std::swap(permutation, prospectiveMove);
        currentEnergy = prospectiveEnergy;
        currentStateIndex = prospectiveStateIndex;
      }

      minimalEnergy = std::min(minimalEnergy, prospectiveEnergy);
      lastEnergies.insert(currentEnergy);
    }

    /* Now let temperature freely vary according to update formula */
    while(temperature > 0.01 && step < steps) {
      temperature = updateTemperature(temperature, minimalEnergy);
      prospectiveMove = generateMove(permutation, temperature);
      const double prospectiveEnergy = energy(prospectiveMove);
      const double p = acceptanceProbability(currentEnergy, prospectiveEnergy, temperature);

      /* Update tracking state */
      iop = Temple::permutationIndex(prospectiveMove);
      unsigned prospectiveStateIndex;
      auto stateIndexFindIter = stateIndexReduction.find(iop);
      if(stateIndexFindIter == std::end(stateIndexReduction)) {
        prospectiveStateIndex = stateIndexReduction.size();
        stateIndexReduction.emplace(iop, prospectiveStateIndex);
        energies.push_back(prospectiveEnergy);
        Q.conservativeResize(energies.size(), energies.size());
        Q.insert(prospectiveStateIndex, currentStateIndex) = 1;
      } else {
        prospectiveStateIndex = stateIndexFindIter->second;
        unsigned col = prospectiveStateIndex;
        unsigned row = currentStateIndex;
        if(col > row) {
          std::swap(col, row);
        }
        Q.coeffRef(row, col) = Q.coeff(row, col) + 1;
      }

      trace << step << ", " << currentEnergy << ", " << temperature << ", " << prospectiveEnergy << ", " << p << "\n";

      // Conditionally accept the move
      if(p >= Temple::Random::getSingle(0.0, 1.0, prng)) {
        std::swap(permutation, prospectiveMove);
        currentEnergy = prospectiveEnergy;
        currentStateIndex = prospectiveStateIndex;
      }

      minimalEnergy = std::min(minimalEnergy, prospectiveEnergy);
      lastEnergies.insert(currentEnergy);
      ++step;
    }

    return minimalEnergy;
  }

  std::string name() const final {
    return "Thermodyn.";
  }
};

/**
 * @brief Fixed-step number greedy minimization with shuffling
 *
 * This just greedily minimizes the permutation and shuffles when it's at the
 * minimum. Unreliable.
 *
 * Complexity: Needs at least Theta(N - 2)! steps to discover enough state
 * space that it might find the minimum.
 */
struct Greedy final : OldShapeAlgorithm {
  Temple::JSF64 prng;

  double shape(const PositionCollection& positions, Shape shape) final {
    const unsigned N = positions.cols();
    const unsigned steps = 4 * facs.at(N - 2);

    Energy energy {positions, shapeCoordinates(shape)};
    auto permutation = Temple::iota<Vertex>(positions.cols());
    double lowestEnergy = std::numeric_limits<double>::max();

    bool foundBetterPermutation;
    for(unsigned m = 0; m < steps; ++m) {
      Temple::Random::shuffle(permutation, prng);
      double minimizationEnergy = energy(permutation);

      do {
        foundBetterPermutation = false;

        // Single swaps
        for(unsigned i = 0; i < N - 1 && m < steps; ++i) {
          for(unsigned j = i + 1; j < N && m < steps; ++j) {
            std::swap(permutation.at(i), permutation.at(j));

            double value = energy(permutation);
            ++m;
            if(value < minimizationEnergy) {
              minimizationEnergy = value;
              foundBetterPermutation = true;
              break;
            }

            // Swap back
            std::swap(permutation.at(i), permutation.at(j));
          }
        }

        // Propose double-swaps
        for(unsigned i = 0; i < N && !foundBetterPermutation; ++i) {
          for(unsigned j = i + 1; j < N && !foundBetterPermutation; ++j) {
            for(unsigned k = 0; k < N && k != i && !foundBetterPermutation; ++k) {
              for(unsigned l = k + 1; l < N && l != j; ++l) {
                std::swap(permutation.at(i), permutation.at(j));
                std::swap(permutation.at(k), permutation.at(l));

                // Calculate the value
                double value = energy(permutation);
                if(value < minimizationEnergy) {
                  minimizationEnergy = value;
                  foundBetterPermutation = true;
                  break;
                }
                ++m;
                if(m >= steps) {
                  return std::min(lowestEnergy, minimizationEnergy);
                }

                // Swap back
                std::swap(permutation.at(k), permutation.at(l));
                std::swap(permutation.at(i), permutation.at(j));
              }
            }
          }
        }

      } while(foundBetterPermutation);

      lowestEnergy = std::min(lowestEnergy, minimizationEnergy);
    }

    return lowestEnergy;
  }

  std::string name() const final {
    return "Greedy";
  }
};

/**
 * @brief Fixed steps steepest descent minimizer with shuffles
 *
 * Finds best swap to reduce permutation the most at each position and
 * minimizes. Shuffles when it's at a minimum. Unreliable.
 *
 * Complexity: Needs at least Theta(N - 2)! steps to discover enough state
 * space that it might find the minimum.
 */
struct SteepestDescent final : OldShapeAlgorithm {
  Temple::JSF64 prng;

  double shape(const PositionCollection& positions, Shape shape) final {
    const unsigned N = positions.cols();
    const unsigned steps = 4 * facs.at(N - 2);

    Energy energy {positions, shapeCoordinates(shape)};
    auto bestCandidate = Temple::iota<Vertex>(positions.cols());
    auto permutation = bestCandidate;
    double lowestEnergy = std::numeric_limits<double>::max();

    bool foundBetterPermutation;
    for(unsigned m = 0; m < steps; ++m) {
      Temple::Random::shuffle(bestCandidate, prng);
      double minimizationEnergy = energy(bestCandidate);
      lowestEnergy = std::min(lowestEnergy, minimizationEnergy);

      do {
        permutation = bestCandidate;
        foundBetterPermutation = false;

        // Propose double-swaps
        for(unsigned i = 0; i < N && !foundBetterPermutation; ++i) {
          for(unsigned j = i + 1; j < N && !foundBetterPermutation; ++j) {
            for(unsigned k = 0; k < N && !foundBetterPermutation; ++k) {
              for(unsigned l = k + 1; l < N; ++l) {
                std::swap(permutation.at(i), permutation.at(j));
                std::swap(permutation.at(k), permutation.at(l));

                // Calculate the value
                double value = energy(permutation);
                ++m;
                if(value < minimizationEnergy) {
                  minimizationEnergy = value;
                  foundBetterPermutation = true;
                  bestCandidate = permutation;
                }
                if(m >= steps) {
                  return std::min(lowestEnergy, minimizationEnergy);
                }

                // Swap back
                std::swap(permutation.at(k), permutation.at(l));
                std::swap(permutation.at(i), permutation.at(j));
              }
            }
          }
        }
      } while(foundBetterPermutation);

      lowestEnergy = std::min(lowestEnergy, minimizationEnergy);
    }

    return lowestEnergy;
  }

  std::string name() const final {
    return "Steepest";
  }
};

/**
 * @brief Aligns four atoms, then greedily zips the rest
 *
 * For each tuple of four atoms, aligns the positions, then greedily chooses
 * the best next sequence alignment.
 *
 * Really good for small distortions (<= 0.5). Suffers from its greediness
 * afterwards. Some branching might do it good.
 *
 * Complexity: Theta(N! / (N - 5)!) quaternion fits
 *
 * Potentially there is another optimization possible that could reduce the
 * scaling but there is a tradeoff involved that might counteract the formal
 * complexity decrease: Principally, spatial rotations of index mappings from
 * the underlying shape mappings need not be repeated. However, the question is
 * how to store them efficiently.
 */
struct AlignFive final : ShapeAlgorithm {
  using M = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
  using IncrementalPermutation = std::unordered_map<unsigned, unsigned>;

  static Eigen::Matrix3d fitQuaternion(
    const PositionCollection& stator,
    const PositionCollection& rotor,
    const IncrementalPermutation& p
  ) {
    assert(centroidIsZero(stator));
    assert(centroidIsZero(rotor));

    Eigen::Matrix4d b = Eigen::Matrix4d::Zero();
    // generate decomposable matrix per atom and add them
    for(auto& iterPair : p) {
      auto& statorCol = stator.col(iterPair.first);
      auto& rotorCol = rotor.col(iterPair.second);

      Eigen::Matrix4d a = Eigen::Matrix4d::Zero();
      a.block<1, 3>(0, 1) = (rotorCol - statorCol).transpose();
      a.block<3, 1>(1, 0) = statorCol - rotorCol;
      a.block<3, 3>(1, 1) = Eigen::Matrix3d::Identity().rowwise().cross(statorCol + rotorCol);
      b += a.transpose() * a;
    }

    // Decompose b
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(b);

    // Do not allow improper rotation
    const Eigen::Vector4d& q = eigensolver.eigenvectors().col(0);
    return Eigen::Quaterniond(q[0], q[1], q[2], q[3]).toRotationMatrix();
  }

  static Permutation flatten(const IncrementalPermutation& p) {
    const unsigned P = p.size();
    Permutation flat(P);
    for(unsigned i = 0; i < P; ++i) {
      flat.at(i) = p.at(i);
    }
    return flat;
  }

  ResultPair narrow(
    const PositionCollection& stator,
    const PositionCollection& rotor,
    const std::set<Permutation>& correctRotations,
    bool inCorrectBranch,
    std::unordered_map<unsigned, unsigned> permutation,
    std::vector<unsigned> freeLeftVertices,
    std::vector<unsigned> freeRightVertices
  ) {
    const unsigned N = stator.cols();

    const unsigned V = freeLeftVertices.size();
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> costs (V, V);
    for(unsigned i = 0; i < V; ++i) {
      for(unsigned j = 0; j < V; ++j) {
        costs(i, j) = (
          stator.col(freeLeftVertices.at(i))
          - rotor.col(freeRightVertices.at(j))
        ).squaredNorm();
      }
    }

    // Maps freeLeftVertices onto freeRightVertices
    auto subPermutation = Temple::iota<unsigned>(V);

    auto isCorrectBranch = [&](const auto& s) {
      return Temple::any_of(
        correctRotations,
        [&](const auto& rot) -> bool {
          // Rotation has to match the current permutation
          if(!Temple::all_of(
            permutation,
            [&](const unsigned left, const unsigned right) -> bool {
              return rot.at(left) == right;
            }
          )) {
            return false;
          }

          // Rotation has to match the sub-permutation too
          for(unsigned i = 0; i < V; ++i) {
            unsigned left = freeLeftVertices.at(i);
            unsigned right = freeRightVertices.at(s.at(i));
            if(rot.at(left) != right) {
              return false;
            }
          }

          return true;
        }
      );
    };

    decltype(subPermutation) bestPermutation;
    double minimalCost = std::numeric_limits<double>::max();
    do {
      double cost = 0.0;
      for(unsigned i = 0; i < V; ++i) {
        cost += costs(i, subPermutation.at(i));
      }

      if(cost < minimalCost) {
        if(inCorrectBranch && !bestPermutation.empty() && isCorrectBranch(bestPermutation) && !isCorrectBranch(subPermutation)) {
          std::cerr << "Excluding correct branch, decreasing cost from " << minimalCost << " to " << cost << " with non-optimum sub-permutation\n";
        }

        minimalCost = cost;
        bestPermutation = subPermutation;
      } else if(inCorrectBranch && isCorrectBranch(subPermutation)) {
        std::cerr << "Excluding correct branch due to cost " << cost << " > " << minimalCost << "\n";
      }
    } while(Temple::next_permutation(subPermutation));

    // Fuse permutation and subpermutation
    for(unsigned i = 0; i < V; ++i) {
      permutation.emplace(freeLeftVertices.at(i), freeRightVertices.at(bestPermutation.at(i)));
    }

    assert(permutation.size() == N);

    auto R = fitQuaternion(stator, rotor, permutation);
    auto rotated = R * rotor;

    const double energy = Temple::accumulate(
      Temple::Adaptors::range(N),
      0.0,
      [&](const double carry, const unsigned i) -> double {
        return carry + (
          stator.col(i) - rotated.col(permutation.at(i))
        ).squaredNorm();
      }
    );

    return ResultPair {energy, flatten(permutation)};
  }

  ResultPair shape(
    const PositionCollection& positions,
    Shape shape,
    const Permutation& correctPermutation
  ) final {
    const unsigned N = positions.cols();
    if(N <= 4) {
      return AllPermutations {}.shape(positions, shape);
    }

    const auto shapeCoords = shapeCoordinates(shape);

    ResultPair minimal {std::numeric_limits<double>::max(), {}};
    std::unordered_map<unsigned, unsigned> permutation;

    const auto correctRotations = generateRotations(correctPermutation, shape);

    // i != j != k != l != m with {i, j, k, l, m} in [0, N)
    Temple::Loops::different(
      [&](const std::vector<unsigned>& indices) {
        permutation.clear();
        for(unsigned i = 0; i < 5; ++i) {
          permutation.emplace(i, indices[i]);
        }

        const unsigned i = indices[0];
        const unsigned j = indices[1];
        const unsigned k = indices[2];
        const unsigned l = indices[3];
        const unsigned m = indices[4];

        Eigen::Matrix3d R = fitQuaternion(positions, shapeCoords, permutation);
        auto rotatedShape = R * shapeCoords;

        double penalty = (
          (positions.col(0) - rotatedShape.col(i)).squaredNorm()
          + (positions.col(1) - rotatedShape.col(j)).squaredNorm()
          + (positions.col(2) - rotatedShape.col(k)).squaredNorm()
          + (positions.col(3) - rotatedShape.col(l)).squaredNorm()
          + (positions.col(4) - rotatedShape.col(m)).squaredNorm()
        );

        bool inCorrectBranch = Temple::any_of(
          correctRotations,
          [&](const auto& rot) {
            return rot[0] == i && rot[1] == j && rot[2] == k && rot[3] == l && rot[4] == m;
          }
        );
        if(inCorrectBranch) {
          std::cerr << "At correct permutation quintuple!\n";
        }

        if(penalty > minimal.first) {
          return;
        }

        std::vector<unsigned> freeLeftVertices;
        freeLeftVertices.reserve(N - 5);
        for(unsigned a = 5; a < N; ++a) {
          freeLeftVertices.push_back(a);
        }
        std::vector<unsigned> freeRightVertices;
        freeRightVertices.reserve(N - 5);
        for(unsigned a = 0; a < N; ++a) {
          if(a != i && a != j && a != k && a != l && a != m) {
            freeRightVertices.push_back(a);
          }
        }

        auto narrowed = narrow(
          positions,
          rotatedShape,
          correctRotations,
          inCorrectBranch,
          permutation,
          std::move(freeLeftVertices),
          std::move(freeRightVertices)
        );

        if(narrowed.first < minimal.first) {
          minimal = narrowed;
        }
      },
      5,
      N
    );

    return minimal;
  }

  std::string name() const final {
    return "AlignFive";
  }
};

void writeEnergyStatistics() {
  /* Write an R file with all of the energy values for a particular shape
   */
  std::ofstream rFile("energy_statistics.R");
  std::vector<Shape> shapes;
  unsigned shapeNumber = 1;
  for(const Shape shape : allShapes) {
    if(size(shape) <= 4) {
      continue;
    }

    if(size(shape) > 8) {
      break;
    }

    std::cout << "Number of permutations for " << name(shape) << " shape: " << facs.at(size(shape)) << "\n";

    shapes.push_back(shape);

    auto shapeCoordinates = normalize(addOrigin(coordinates(shape)));
    const unsigned N = shapeCoordinates.cols();

    auto distorted = shapeCoordinates;
    const double distortionNorm = 0.2;
    distort(distorted, distortionNorm);

    Energy energy {shapeCoordinates, distorted};

    rFile << "shape" << shapeNumber << " <- c(";
    auto permutation = Temple::iota<Vertex>(N);
    rFile << energy(permutation);
    while(Temple::next_permutation(permutation)) {
      rFile << ", " << energy(permutation);
    }
    rFile << ")\n";

    ++shapeNumber;
  }

  rFile << "shapeNames <- c(" << Temple::condense(Temple::map(shapes, [](auto s) { return "\"" + name(s) + "\""; })) << ")\n";
}

template<typename PRNG>
PositionCollection shuffle(const PositionCollection& positions, PRNG& prng) {
  const unsigned C = positions.cols();
  auto permutation = Temple::iota<Vertex>(C);
  Temple::Random::shuffle(permutation, prng);
  PositionCollection shuffled(3, C);
  for(unsigned i = 0; i < C; ++i) {
    shuffled.col(permutation.at(i)) = positions.col(i);
  }
  return shuffled;
}

namespace color {

std::ostream& boldMagenta(std::ostream& os) {
  os << "\033[1;35m";
  return os;
}

std::ostream& red(std::ostream& os) {
  os << "\033[31m";
  return os;
}

std::ostream& green(std::ostream& os) {
  os << "\033[32m";
  return os;
}

std::ostream& reset(std::ostream& os) {
  os << "\033[0m";
  return os;
}

} // namespace color

int main(int argc, char* argv[]) {
  boost::program_options::options_description options_description("Recognized options");
  options_description.add_options()
    ("help,h", "Produce help message")
    (
      "prng,p",
      boost::program_options::value<int>(),
      "Seed to initialize PRNG with."
    )
    (
      "shape,s",
      boost::program_options::value<unsigned>(),
      "Shape to run algorithms with"
    )
    (
      "distortion,d",
      boost::program_options::value<double>(),
      "Distortion norm to apply to shapes."
    )
    (
      "repeats,r",
      boost::program_options::value<unsigned>(),
      "Number of repetitions"
    )
  ;

  /* Parse */
  boost::program_options::variables_map options_variables_map;
  boost::program_options::store(
    boost::program_options::command_line_parser(argc, argv).
    options(options_description).
    style(
      boost::program_options::command_line_style::unix_style
      | boost::program_options::command_line_style::allow_long_disguise
    ).run(),
    options_variables_map
  );
  boost::program_options::notify(options_variables_map);

  if(options_variables_map.count("help") > 0) {
    std::cout << options_description << "\n";
    return 0;
  }

  using namespace std::chrono;

  Temple::JSF64 prng;
  if(options_variables_map.count("seed")) {
    const int seed = options_variables_map["seed"].as<int>();
    prng.seed(seed);
    std::cout << "PRNG seeded from parameters: " << seed << ".\n";
  } else {
    std::random_device randomDevice;
    const int seed = std::random_device {}();
    std::cout << "PRNG seeded from random_device: " << seed << ".\n";
    prng.seed(seed);
  }

  std::vector<std::unique_ptr<ShapeAlgorithm>> algorithmPtrs;
  // algorithmPtrs.emplace_back(std::make_unique<Anneal>());
  // algorithmPtrs.emplace_back(std::make_unique<Tunnel>());
  // algorithmPtrs.emplace_back(std::make_unique<ThermodynamicAnneal>());
  // algorithmPtrs.emplace_back(std::make_unique<Greedy>());
  // algorithmPtrs.emplace_back(std::make_unique<SteepestDescent>());
  // algorithmPtrs.emplace_back(std::make_unique<CentroidElimination>());
  algorithmPtrs.emplace_back(std::make_unique<AlignFive>());

  const unsigned nameColWidth = 12;
  const unsigned timeColWidth = 6;
  const unsigned repeats = (options_variables_map.count("repeats") == 1)
    ? options_variables_map["repeats"].as<unsigned>()
    : 10;
  const double distortionNorm = (options_variables_map.count("distortion") == 1)
    ? options_variables_map["distortion"].as<double>()
    : 0.1;
  Shape shape = Shape::CappedSquareAntiprism;
  if(options_variables_map.count("shape") == 1) {
    unsigned shapeIndex = options_variables_map["shape"].as<unsigned>();
    if(shapeIndex < nShapes) {
      shape = static_cast<Shape>(shapeIndex);
    }
  }

  std::cout << "For shape " << name(shape) << " (size " << size(shape) << ") with distortion norm " << distortionNorm << "\n\n";

  AllPermutations referenceAlgorithm;
  std::cout << std::setw(nameColWidth) << referenceAlgorithm.name() << std::setw(timeColWidth) << "msec";

  for(auto& algorithmPtr : algorithmPtrs) {
    std::cout << std::setw(nameColWidth) << algorithmPtr->name() << std::setw(timeColWidth) << "msec" << std::setw(timeColWidth) << "S";
  }
  std::cout << "\n";

  const unsigned A = algorithmPtrs.size();
  std::vector<
    std::vector<double>
  > errors (A);

  std::vector<
    std::vector<double>
  > latencies (A);

  std::vector<
    std::vector<double>
  > speedups (A);

  for(unsigned i = 0; i < repeats; ++i) {
    auto coordinates = ShapeAlgorithm::shapeCoordinates(shape);
    distort(coordinates, distortionNorm);
    coordinates = normalize(shuffle(coordinates, prng));

    time_point<steady_clock> start, end;
    start = steady_clock::now();
    const auto referencePair = referenceAlgorithm.shape(coordinates, shape);
    end = steady_clock::now();
    const unsigned referenceLatency = duration_cast<microseconds>(end - start).count();
    std::cout << std::setw(nameColWidth) << referencePair.first << std::setw(timeColWidth) << (referenceLatency / 1000);
    auto rotations = ShapeAlgorithm::generateRotations(referencePair.second, shape);

    for(unsigned j = 0; j < A; ++j) {
      auto& algorithmPtr = algorithmPtrs.at(j);
      start = steady_clock::now();
      const auto valuePair = algorithmPtr->shape(coordinates, shape, referencePair.second);
      end = steady_clock::now();

      if(valuePair.second != referencePair.second) {
        std::cerr << "\n" << std::setw(25) << "Reference permutation" << " " << Temple::condense(referencePair.second) << "\n"
          << std::setw(25) << algorithmPtr->name() << " " << Temple::condense(valuePair.second) << "\n";
        if(rotations.count(valuePair.second) == 1) {
          std::cerr << "Algorithm result is a shape rotation of the reference permutation\n";
        } else {
          std::cerr << "Algorithm result is not a shape rotation of the reference permutation:\n";
          for(const auto& rotation : rotations) {
            std::cerr << "Rotation " << Temple::condense(rotation) << "\n";
          }
        }
      }

      if(valuePair.first < referencePair.first - 1e-5) {
        std::cout << color::boldMagenta << std::setw(nameColWidth) << valuePair.first << color::reset;
      } else if(std::fabs(valuePair.first - referencePair.first) < 1e-5) {
        std::cout << color::green << std::setw(nameColWidth) << valuePair.first << color::reset;
      } else {
        std::cout << color::red << std::setw(nameColWidth) << valuePair.first << color::reset;
      }
      errors.at(j).push_back(std::fabs(referencePair.first - valuePair.first));

      unsigned latency = duration_cast<microseconds>(end - start).count();
      std::cout << std::setw(timeColWidth) << (latency / 1000);
      latencies.at(j).push_back(latency);

      if(latency == 0) {
        std::cout << std::setw(timeColWidth) << "inf";
      } else {
        std::cout << std::setw(timeColWidth) << std::round(static_cast<double>(referenceLatency) / latency);
      }
      speedups.at(j).push_back(static_cast<double>(referenceLatency) / latency);
    }
    std::cout << "\n";
  }

  std::cout << "\n";
  std::cout << std::setw(nameColWidth + timeColWidth) << "averages";
  for(unsigned j = 0; j < A; ++j) {
    const double errorAverage = Temple::average(errors.at(j));
    if(errorAverage < 1e-10) {
      std::cout << color::green << std::setw(nameColWidth) << 0 << color::reset;
    } else {
      std::cout << color::red << std::setw(nameColWidth) << errorAverage << color::reset;
    }

    std::cout << std::setw(timeColWidth) << static_cast<unsigned>(Temple::average(latencies.at(j)) / 1000);
    std::cout << std::setw(timeColWidth) << static_cast<unsigned>(Temple::average(speedups.at(j)));
  }
  std::cout << "\n";
}


//
// Created by Filippo Vicentini on 26/12/17.
//
#ifdef GPU_SUPPORT
#ifndef SIMULATOR_TWMCLIEBGPUSOLVER_HPP
#define SIMULATOR_TWMCLIEBGPUSOLVER_HPP

#include "Base/SolverGPU.hpp"

#include "TWMC/TWMCTypes.h"
#include <vexcl/vexcl.hpp>

#include <stdio.h>

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdocumentation"
#include <Eigen/Sparse>
typedef Eigen::SparseMatrix<complex_p, Eigen::RowMajor> SparseCXd;
#pragma clang pop

class TWMCSystemData;
class TWMCTaskData;
class Settings;
class TWLiebSimData;
class TWMCLiebResults;

class TWMCLiebGPUSolver : public SolverGPU
{

public:
    TWMCLiebGPUSolver();
    ~TWMCLiebGPUSolver();

    virtual void SetupGPU(size_t gpuId);
    virtual void SuggestBatchSize();

    virtual void Setup() final;
	virtual std::vector<std::unique_ptr<TaskResults>> Compute(const std::vector<std::unique_ptr<TaskData>>& tasks) final;

    virtual float ApproximateComputationProgress() const final;

protected:

private:
    const TWMCSystemData* lastSharedSystemData;

    // VexCL Types
    vex::Context ctx;

    MatrixCXd beta_t;
    vex::vector<complex_p> beta_t_GPU;
    MatrixCXd real_step_linear;
    vex::vector<complex_p> real_step_linear_GPU;
    MatrixCXd complexU;
    vex::vector<complex_p> complexU_GPU;
    MatrixCXd iF;
    vex::vector<complex_p> iF_GPU;
    MatrixCXd complexId;
    vex::vector<complex_p> complexId_GPU;
    SparseCXd iJ;
    vex::sparse::matrix<complex_p> iJ_GPU;
    //______END_VEXCL

    size_t nx;
    size_t ny;
    size_t cellSz = 3;

    // Temp sim variables
    MatrixCXd U;
    MatrixCXd F;
    MatrixCXd E;
    MatrixCXd delta;

    MatrixCXd tmpRand;
    MatrixCXd kai_t;
    MatrixCXd a_t;
    float_p fft_norm_factor;
    complex_p temp_gamma;

    SparseCXd coupling_mat;
    SparseCXd coupling_mat_total;

    MatrixCXd beta_t_init;
    float_p t;
    float_p t_start;
    float_p t_end;

    // Progress monitor
    size_t taskBatch = 0;
    size_t completedTasks = 0;
};

static SolverFactory::Registrator<TWMCLiebGPUSolver> TWMCBaseSolver=
        SolverFactory::Registrator<class TWMCLiebGPUSolver::TWMCLiebGPUSolver>("TWMCLieb_GPU");

static SolverGPUFactory::Registrator<TWMCLiebGPUSolver> TWMCBaseSolverGPU=
        SolverGPUFactory::Registrator<class TWMCLiebGPUSolver::TWMCLiebGPUSolver>("TWMCLieb");

#endif //SIMULATOR_TWMCLIEBGPUSOLVER_HPP
#endif //GPU_SUPPORT


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef SFERMIONS_H
#define SFERMIONS_H

#include <Eigen/Core>

namespace flexiblesusy {
namespace sfermions {

enum Sparticles {
   up = 0,
   down = 1,
   neutrino = 2,
   electron = 3,
   NUMBER_OF_MSSM_SPARTICLES
};

extern const double Isospin[NUMBER_OF_MSSM_SPARTICLES];
extern const double Hypercharge_left[NUMBER_OF_MSSM_SPARTICLES];
extern const double Hypercharge_right[NUMBER_OF_MSSM_SPARTICLES];

/**
 * data needed to fill 2 x 2 sfermion mass matrix
 */
struct Mass_data {
   double ml2;    ///< soft mass of left-handed sfermion
   double mr2;    ///< soft mass of right-handed sfermion
   double yf;     ///< Yukawa coupling
   double vd, vu; ///< Higgs VEVs
   double gY, g2; ///< gauge couplings (not GUT normalized)
   double Tyf;    ///< trilinear coupling
   double mu;     ///< Superpotential parameter
   double T3;     ///< weak isospin
   double Yl;     ///< Hypercharge of left-handed sfermion
   double Yr;     ///< Hypercharge of right-handed sfermion
};

double diagonalize_sfermions_2x2(const Mass_data&,
                                 Eigen::Array<double,2,1>&);

} // namespace sfermions
} // namespace flexiblesusy

#endif