/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program 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.
|
|    This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <iostream>
#include <cstdio>
#include <set>
#include <algorithm>
#include <cmath>

#include <boost/thread.hpp>

#include "saveload_bz2.hpp"
#include "saveload_gz.hpp"
#include "sdr.hpp"

using namespace std;

namespace hashclash {

	std::ostream& operator<<(std::ostream& o, const sdr& n)
	{
		o << "[!";
		bool first = true;
		for (unsigned b = 0; b < 32; ++b)
		{
			if (n[b] != 0)
			{
				if (first)
					first = false;
				else
					o << ",";
				if (n[b] == -1)
					o << "-";
				o << b;
			}
		}
		o << "!]";
		return o;
	}

	std::istream& operator>>(std::istream& i, sdr& n)
	{
		n.clear();
		char c;
		if (!(i >> c)) return i;
		if (c != '[') {
			i.putback(c);
			i.setstate(std::ios::failbit);
			return i;
		}
		if (!(i >> c)) return i;
		if (c != '!') {
			i.putback(c);
			i.setstate(std::ios::failbit);
			return i;
		}
		// check for empty sdr
		if (!(i >> c)) return i;
		if (c == '!') {
			if (!(i >> c)) return i;
			if (c != ']') {
				i.putback(c);
				i.setstate(std::ios::failbit);
			}
			return i;
		}
		i.putback(c);

		char s;
		bool neg = true;
		if (!(i >> s)) return i;
		if (s != '-') {
			neg = false;
			i.putback(s);
		}
		unsigned bit;
		while (i >> bit >> c) {
			if (!(bit < 32 && (c == ',' || c == '!'))) {
				i.putback(c);
				i.setstate(std::ios::failbit);
				return i;
			}
			n.mask |= 1<<bit;
			if (!neg)
				n.sign |= 1<<bit;

			if (c == '!') break;
			neg = true;
			if (!(i >> s)) return i;
			if (s != '-') {
				neg = false;
				i.putback(s);
			}
		}
		if (!(i >> c)) return i;
		if (c != ']') {
			i.putback(c);
			i.setstate(std::ios::failbit);
			return i;
		}
		return i;
	}

	uint32 best_rotated_difference(uint32 diff, int rc)
	{
		if (diff == 0 || (rc&31)==0)
			return diff;

		int rc2 = 32 - rc;
		uint32 bound = 1 << rc2;
		uint32 bound2 = 1 << rc;
		uint32 y = diff >> rc2;
		uint32 x = diff - (y<<rc2);
		uint32 p1 = (bound-x)*y;
		uint32 d1 = ((x<<rc)|y);
		if ((y<<1) > bound2)
			d1 -= bound2;
		else
			p1 = ((bound-x)<<rc) - p1;

		if (x == 0)
			return d1;

		y += 1;
		y &= ~bound2; // erase possible carry at rc-th bit
		uint32 p2 = x*y;
		uint32 d2 = ((x<<rc)|y);
		if ((y<<1) > bound2)
			d2 -= bound2;
		else
			p2 = (x<<rc) - p2;
		if (p1 > p2)
			return d1;
		return d2;
	}

	void rotate_difference(uint32 diff, int rc, std::vector<uint32>& rotateddiff, uint32 minprob)
	{
		rotateddiff.clear();
		if (diff == 0 || (rc&31)==0) {
			rotateddiff.push_back(diff);
			return;
		}
		// now p1,p2,p3,p4 < 1 * 2^32, no overflows

		int rc2 = 32 - rc;
		uint32 bound = 1 << rc2;
		uint32 bound2 = 1 << rc;
		uint32 y = diff >> rc2;
		uint32 x = diff - (y<<rc2);

		uint32 p1 = (bound-x) * y;
		if (p1 >= minprob)
			rotateddiff.push_back( ((x<<rc)|y) - bound2 );
		uint32 p2 = ((bound-x)<<rc) - p1;
		if (p2 >= minprob)
			rotateddiff.push_back( (x<<rc)|y );

		if (x != 0) {
			y += 1;
			y &= ~bound2; // erase possible carry at rc-th bit

			uint32 p3 = x * y;
			if (p3 >= minprob)
				rotateddiff.push_back( ((x<<rc)|y) - bound2 );
			uint32 p4 = (x<<rc) - p1;
			if (p4 >= minprob)
				rotateddiff.push_back( (x<<rc)|y );
		}
	}

	void rotate_difference(uint32 diff, int rc, std::vector<std::pair<uint32,double> >& rotateddiff)
	{
		double pinv = pow(double(2),-32);

		rotateddiff.clear();
		if (diff == 0 || (rc&31)==0) {
			rotateddiff.push_back(std::pair<uint32,double>(diff,1));
			return;
		}

		int rc2 = 32 - rc;
		uint32 bound = 1 << rc2;
		uint32 bound2 = 1 << rc;
		uint32 y = diff >> rc2;
		uint32 x = diff - (y<<rc2);

		uint32 p1 = 0;
		if (y) {
			p1 = (bound-x) * y;
			rotateddiff.push_back( std::pair<uint32,double>
				( ((x<<rc)|y) - bound2, double(p1)*pinv ));
		}

		uint32 p2 = ((bound-x)<<rc) - p1;
		rotateddiff.push_back( std::pair<uint32,double>
				( (x<<rc)|y, double(p2)*pinv ));

		uint32 p3 = 0, p4 = 0;
		if (x != 0) {
			y += 1;
			y &= ~bound2; // erase possible carry at rc-th bit

			if (y) {
				p3 = x * y;
				rotateddiff.push_back( std::pair<uint32,double>
					( ((x<<rc)|y) - bound2, double(p3)*pinv ));
			}

			p4 = (x<<rc) - p3;
			rotateddiff.push_back( std::pair<uint32,double>
					( (x<<rc)|y, double(p4)*pinv ));
		}
	}

	unsigned hw_table[0x800];
	struct sdr_carry {
		std::vector< std::vector<sdr> > positive, negative;
	};
	std::vector<sdr_carry> hashclash_sc(0);
	std::vector< std::vector< std::pair<unsigned,unsigned> > > hashclash_scn(0);
	void hashclash_init_scn();

	unsigned count_sdrs(uint32 n, unsigned maxw)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		uint32 l = n & 0xFFFF;
		uint32 h1 = n - l;
		uint32 h2 = h1 + 0x10000;
		unsigned h1w = hwnaf(h1);
		unsigned h2w = hwnaf(h2);
		unsigned hw = h1w;
		if (h2w<hw) hw = h2w;

		std::vector< std::pair<unsigned,unsigned> >& vl = hashclash_scn[l];
		std::vector< std::pair<unsigned,unsigned> >& vh1 = hashclash_scn[h1>>16];
		std::vector< std::pair<unsigned,unsigned> >& vh2 = hashclash_scn[h2>>16];
		unsigned tot = 0;

		for (unsigned i = 0; i < vl.size() && i+hw <= maxw; ++i)
		{
			if (vl[i].first == 0 && vl[i].second == 0) continue;
			unsigned m = 0;
			for (unsigned j = 0; j < vh1.size() && i+j<=maxw; ++j)
				m += vh1[j].first + vh1[j].second;
			tot += vl[i].first * m;
			m = 0;
			for (unsigned j = 0; j < vh2.size() && i+j<=maxw; ++j)
				m += vh2[j].first + vh2[j].second;
			tot += vl[i].second * m;
		}
		return tot;
	}

	typedef std::vector< std::vector<sdr> > vec_vec_sdr_t;
	void table_sdrs(std::vector<sdr>& result, uint32 n, unsigned maxw)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		result.clear();
		if (maxw < hwnaf(n)) return;
		std::vector< triple<uint32,uint32,uint32> > breakn(8);
		std::vector< triple<vec_vec_sdr_t*, vec_vec_sdr_t*, vec_vec_sdr_t*> > breaknsdr(8);
		uint32 m0 = n & 0x7FF;
		uint32 n0 = n - m0;
		uint32 m1 = n0 & 0x3FFFFF;
		uint32 m2 = n0 - m1;
		breakn[0]=make_triple(m0,m1,m2);
		breakn[4]=make_triple(m0,m1,m2);
		breaknsdr[0]=make_triple(
			&hashclash_sc[m0].positive,
			&hashclash_sc[m1>>11].positive,
			&hashclash_sc[m2>>21].positive);
		breaknsdr[4]=make_triple(
			&hashclash_sc[m0].positive,
			&hashclash_sc[m1>>11].positive,
			&hashclash_sc[m2>>21].negative);

		m2 += 0x400000;
		breakn[2]=make_triple(m0,m1,m2);
		breakn[6]=make_triple(m0,m1,m2);
		breaknsdr[2]=make_triple(
			&hashclash_sc[m0].positive,
			&hashclash_sc[m1>>11].negative,
			&hashclash_sc[m2>>21].positive);
		breaknsdr[6]=make_triple(
			&hashclash_sc[m0].positive,
			&hashclash_sc[m1>>11].negative,
			&hashclash_sc[m2>>21].negative);

		n0 += 0x800;
		m1 = n0 & 0x3FFFFF;
		m2 = n0 - m1;
		breakn[1]=make_triple(m0,m1,m2);
		breakn[5]=make_triple(m0,m1,m2);
		breaknsdr[1]=make_triple(
			&hashclash_sc[m0].negative,
			&hashclash_sc[m1>>11].positive,
			&hashclash_sc[m2>>21].positive);
		breaknsdr[5]=make_triple(
			&hashclash_sc[m0].negative,
			&hashclash_sc[m1>>11].positive,
			&hashclash_sc[m2>>21].negative);

		m2 += 0x400000;
		breakn[3]=make_triple(m0,m1,m2);
		breakn[7]=make_triple(m0,m1,m2);
		breaknsdr[3]=make_triple(
			&hashclash_sc[m0].negative,
			&hashclash_sc[m1>>11].negative,
			&hashclash_sc[m2>>21].positive);
		breaknsdr[7]=make_triple(
			&hashclash_sc[m0].negative,
			&hashclash_sc[m1>>11].negative,
			&hashclash_sc[m2>>21].negative);
		std::vector<sdr>::const_iterator cit, citend;
		for (unsigned l = 0; l < breakn.size(); ++l)
		{
			m0 = breakn[l].first;
			m1 = breakn[l].second;
			m2 = breakn[l].third;
			vec_vec_sdr_t& v0 = *breaknsdr[l].first;
			vec_vec_sdr_t& v1 = *breaknsdr[l].second;
			vec_vec_sdr_t& v2 = *breaknsdr[l].third;
			unsigned w0 = 0; while (w0 < v0.size() && 0 == v0[w0].size()) ++w0;
			unsigned w1 = 0; while (w1 < v1.size() && 0 == v1[w1].size()) ++w1;
			unsigned w2 = 0; while (w2 < v2.size() && 0 == v2[w2].size()) ++w2;
			unsigned w0max = maxw - w1 - w2;
			unsigned w1max = maxw - w2;
			for (unsigned i0 = w0; i0 <= w0max && i0 < v0.size(); ++i0)
			for (unsigned j0 = 0; j0 < v0[i0].size(); ++j0)
			{
				sdr temp0 = v0[i0][j0];
				for (unsigned i1 = w1; i0+i1 <= w1max && i1 < v1.size(); ++i1)
				for (unsigned j1 = 0; j1 < v1[i1].size(); ++j1)
				{
					sdr temp1 = temp0;
					temp1.mask ^= v1[i1][j1].mask << 11;
					temp1.sign ^= v1[i1][j1].sign << 11;
					for (unsigned i2 = w2; i0+i1+i2 <= maxw && i2 < v2.size(); ++i2)
					{
						cit = v2[i2].begin();
						citend = v2[i2].end();
						for (; cit != citend; ++cit)
						{
							sdr temp2 = temp1;
							temp2.mask ^= cit->mask << 21;
							temp2.sign ^= cit->sign << 21;
							result.push_back(temp2);
						}
					}
				}
			}
		}

	}

	unsigned count_sdrs(uint32 n, unsigned w, bool signpos)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		uint32 l = n & 0xFFFF;
		uint32 h1 = n - l;
		uint32 h2 = h1 + 0x10000;
		unsigned h1w = hwnaf(h1);
		unsigned h2w = hwnaf(h2);
		unsigned hw = h1w;
		if (h2w<hw) hw = h2w;

		std::vector< std::pair<unsigned,unsigned> >& vl = hashclash_scn[l];
		std::vector< std::pair<unsigned,unsigned> >& vh1 = hashclash_scn[h1>>16];
		std::vector< std::pair<unsigned,unsigned> >& vh2 = hashclash_scn[h2>>16];
		unsigned tot = 0;

		for (unsigned i = 0; i < vl.size() && i+hw <= w; ++i)
		{
			if (signpos) {
				if (w-i < vh1.size())
					tot += vl[i].first * vh1[w-i].first;
				if (w-i < vh2.size())
					tot += vl[i].second * vh2[w-i].first;
			} else {
				if (w-i < vh1.size())
					tot += vl[i].first * vh1[w-i].second;
				if (w-i < vh2.size())
					tot += vl[i].second * vh2[w-i].second;
			}
		}
		return tot;
	}

	unsigned count_sdrs(sdr n, unsigned maxw, unsigned rot)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		sdr low = n, high = n;
		high.mask >>= 32-rot; high.sign >>= 32-rot;
		low.mask &= (~uint32(0))>>rot; low.sign &= low.mask;

		uint32 highdiff = high.adddiff();
		uint32 lowdiff = low.adddiff();
		bool lowpos = (lowdiff & 0x80000000)==0;
		bool highpos = (highdiff & 0x80000000)==0;

		unsigned lowcnt[33];
		unsigned highcnt[33];
		unsigned lowhw = hwnaf(lowdiff);
		unsigned highhw = hwnaf(highdiff);
		lowdiff <<= rot;
		highdiff <<= 32-rot;
		for (unsigned i = lowhw; i+highhw <= maxw; ++i)
			lowcnt[i] = count_sdrs(lowdiff, i, lowpos);
		for (unsigned i = highhw; i+lowhw <= maxw; ++i)
			highcnt[i] = count_sdrs(highdiff, i, highpos);
		unsigned tot = 0;
		for (unsigned wl = lowhw; wl+highhw <= maxw; ++wl)
		{
			unsigned m = 0;
			for (unsigned wh = highhw; wh+wl <= maxw; ++wh)
				m += highcnt[wh];
			tot += lowcnt[wl] * m;
		}
		return tot;
	}

	void table_sdrs(std::vector<sdr>& result, uint32 n, unsigned w, bool signpos)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		result.clear();
		if (w < hwnaf(n)) return;
		std::vector< triple<uint32,uint32,uint32> > breakn(4);
		std::vector< triple<vec_vec_sdr_t*, vec_vec_sdr_t*, vec_vec_sdr_t*> > breaknsdr(4);
		uint32 m0 = n & 0x7FF;
		uint32 n0 = n - m0;
		uint32 m1 = n0 & 0x3FFFFF;
		uint32 m2 = n0 - m1;
		breakn[0]=make_triple(m0,m1,m2);
		if (signpos)
			breaknsdr[0]=make_triple(
				&hashclash_sc[m0].positive,
				&hashclash_sc[m1>>11].positive,
				&hashclash_sc[m2>>21].positive);
		else
			breaknsdr[0]=make_triple(
				&hashclash_sc[m0].positive,
				&hashclash_sc[m1>>11].positive,
				&hashclash_sc[m2>>21].negative);

		m2 += 0x400000;
		breakn[2]=make_triple(m0,m1,m2);
		if (signpos)
			breaknsdr[2]=make_triple(
				&hashclash_sc[m0].positive,
				&hashclash_sc[m1>>11].negative,
				&hashclash_sc[m2>>21].positive);
		else
			breaknsdr[2]=make_triple(
				&hashclash_sc[m0].positive,
				&hashclash_sc[m1>>11].negative,
				&hashclash_sc[m2>>21].negative);

		n0 += 0x800;
		m1 = n0 & 0x3FFFFF;
		m2 = n0 - m1;
		breakn[1]=make_triple(m0,m1,m2);
		if (signpos)
			breaknsdr[1]=make_triple(
				&hashclash_sc[m0].negative,
				&hashclash_sc[m1>>11].positive,
				&hashclash_sc[m2>>21].positive);
		else
			breaknsdr[1]=make_triple(
				&hashclash_sc[m0].negative,
				&hashclash_sc[m1>>11].positive,
				&hashclash_sc[m2>>21].negative);

		m2 += 0x400000;
		breakn[3]=make_triple(m0,m1,m2);
		if (signpos)
			breaknsdr[3]=make_triple(
				&hashclash_sc[m0].negative,
				&hashclash_sc[m1>>11].negative,
				&hashclash_sc[m2>>21].positive);
		else
			breaknsdr[3]=make_triple(
				&hashclash_sc[m0].negative,
				&hashclash_sc[m1>>11].negative,
				&hashclash_sc[m2>>21].negative);
		std::vector<sdr>::const_iterator cit, citend;
		for (unsigned l = 0; l < breakn.size(); ++l)
		{
			m0 = breakn[l].first;
			m1 = breakn[l].second;
			m2 = breakn[l].third;
			vec_vec_sdr_t& v0 = *breaknsdr[l].first;
			vec_vec_sdr_t& v1 = *breaknsdr[l].second;
			vec_vec_sdr_t& v2 = *breaknsdr[l].third;
			unsigned w0 = 0; while (w0 < v0.size() && 0 == v0[w0].size()) ++w0;
			unsigned w1 = 0; while (w1 < v1.size() && 0 == v1[w1].size()) ++w1;
			unsigned w2 = 0; while (w2 < v2.size() && 0 == v2[w2].size()) ++w2;
			unsigned w0max = w - w1 - w2;
			unsigned w1max = w - w2;
			for (unsigned i0 = w0; i0 <= w0max && i0 < v0.size(); ++i0)
			for (unsigned j0 = 0; j0 < v0[i0].size(); ++j0)
			{
				sdr temp0 = v0[i0][j0];
				for (unsigned i1 = w1; i0+i1 <= w1max && i1 < v1.size(); ++i1)
				if (w-i0-i1 < v2.size())
				for (unsigned j1 = 0; j1 < v1[i1].size(); ++j1)
				{
					sdr temp1 = temp0;
					temp1.mask ^= v1[i1][j1].mask << 11;
					temp1.sign ^= v1[i1][j1].sign << 11;
					cit = v2[w - i0 - i1].begin();
					citend = v2[w - i0 - i1].end();
					for (; cit != citend; ++cit)
					{
						sdr temp2 = temp1;
						temp2.mask ^= cit->mask << 21;
						temp2.sign ^= cit->sign << 21;
						result.push_back(temp2);
					}
				}
			}
		}
	}

	void table_sdrs(std::vector<sdr>& result, sdr n, unsigned maxw, unsigned rot)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		result.clear();
		if (maxw < hwnaf(n)) return;
		sdr low = n, high = n;
		high.mask >>= 32-rot; high.sign >>= 32-rot;
		low.mask &= (~uint32(0))>>rot; low.sign &= low.mask;

		uint32 highdiff = high.adddiff();
		uint32 lowdiff = low.adddiff();
		bool lowpos = (lowdiff & 0x80000000)==0;
		bool highpos = (highdiff & 0x80000000)==0;

		std::vector< std::vector<sdr> > lowsdrs(33);
		std::vector< std::vector<sdr> > highsdrs(33);
		unsigned lowhw = hwnaf(lowdiff);
		unsigned highhw = hwnaf(highdiff);
		lowdiff <<= rot;
		highdiff <<= 32-rot;
		for (unsigned i = lowhw; i <= maxw-highhw; ++i)
			table_sdrs(lowsdrs[i], lowdiff, i, lowpos);
		for (unsigned i = highhw; i <= maxw-lowhw; ++i)
			table_sdrs(highsdrs[i], highdiff, i, highpos);
		for (unsigned wl = lowhw; wl <= maxw-highhw; ++wl)
			for (unsigned l0 = 0; l0 < lowsdrs[wl].size(); ++l0)
			{
				sdr temp0 = lowsdrs[wl][l0];
				if (temp0.hw() != wl) throw;
				temp0.mask >>= rot;
				temp0.sign >>= rot;
				for (unsigned wh = highhw; wh+wl <= maxw; ++wh)
					for (unsigned h0 = 0; h0 < highsdrs[wh].size(); ++h0)
					{
						sdr temp1 = highsdrs[wh][h0];
						if (temp1.hw() != wh) throw;
						temp1.mask ^= temp0.mask;
						temp1.sign ^= temp0.sign;
						result.push_back(temp1);
					}
			}
	}

	boost::mutex hashclash_init_scn_mutex;
	void hashclash_init_scn()
	{
		boost::lock_guard<boost::mutex> lock(hashclash_init_scn_mutex);
		if (hashclash_scn.size()) return;
		std::vector< std::vector< std::pair<unsigned,unsigned> > > hashclash_scn2(0);
/*		try {
			load_gz(hashclash_scn2, "hashclash_scn", binary_archive);
			hashclash_scn.swap(hashclash_scn2);
		} catch(...) {
			hashclash_scn2.clear();
		}
		if (hashclash_scn.size()) return;
*/
		sdr temp(0);
		hashclash_scn2.resize(1<<16);
		for (temp.mask = 0; temp.mask < 0x10000; ++temp.mask)
			for (temp.sign = 0; temp.sign <= temp.mask; ++temp.sign)
			{
				if (temp.sign & (~temp.mask)) continue;
				unsigned w = temp.hw();
				uint32 n = temp.adddiff();
				if (n & 0x80000000) // negative
				{
					n += 0x10000;
					if (hashclash_scn2[n].size() < w+1)
						hashclash_scn2[n].resize(w+1);
					++hashclash_scn2[n][w].second;
				} else // positive
				{
					if (hashclash_scn2[n].size() < w+1)
						hashclash_scn2[n].resize(w+1);
					++hashclash_scn2[n][w].first;
				}
			}
/*
		try {
			save_gz(hashclash_scn2, "hashclash_scn", binary_archive);
		} catch (...)
		{}
*/
		hashclash_scn.swap(hashclash_scn2);
	}

	struct hashclash_sdr__init {
		hashclash_sdr__init()
		{
			init_hwtable();
			init_sc();
		}

		void init_hwtable()
		{
			for (uint32 n = 0; n < 0x800; ++n)
			{
				unsigned w = 0;
				uint32 k = n;
				while (k) {
					w += k & 1;
					k >>= 1;
				}
				hw_table[n] = w;
			}
		}

		void init_sc()
		{
			hashclash_sc.resize(0x800);
			sdr temp;
			for (temp.mask = 0; temp.mask < 0x800; ++temp.mask)
				for (temp.sign = 0; temp.sign < 0x800; ++temp.sign)
				{
					if (temp.mask != (temp.sign | temp.mask)) continue;
					uint32 n = temp.adddiff();
					unsigned w = temp.hw();
					if (n & 0x80000000) // negative
					{
						n += 0x800;
						if (hashclash_sc[n].negative.size() < w+1)
							hashclash_sc[n].negative.resize(w+1);
						hashclash_sc[n].negative[w].push_back(temp);
					} else // positive
					{
						if (hashclash_sc[n].positive.size() < w+1)
							hashclash_sc[n].positive.resize(w+1);
						hashclash_sc[n].positive[w].push_back(temp);
					}
				}
		}
	};
	hashclash_sdr__init hashclash_sdr__init__now;

	void hashclash_sdr_hpp_init()
	{
		hashclash_sdr__init here;
	}

} // namespace


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

/**
 * Given camera parameters and noise estimates, this program
 * estimates the error that global localization via either
 * RANSAC or least-squares will have.
 **/
#include <common/init.h>
#include <common/thread.h>
#include <camera/camera_model.h>
#include <sparse_mapping/reprojection.h>

#include <Eigen/Geometry>
#include <ceres/ceres.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/features2d/features2d.hpp>

DEFINE_bool(ransac, false,
              "Use RANSAC algorithm. If not use ceres solver.");

// Observation parameters
DEFINE_uint64(num_observations, 20,
             "Number of landmarks to observe.");
DEFINE_double(map_error, 0.01,
             "Std. dev. error of the mapped position, in m, for each dimension.");
DEFINE_double(feature_error, 0,
             "Std. dev. error from feature detection, in pixels, for each dimension.");
DEFINE_double(mismatch_prob, 0.0,
             "The probability of an observation matching to the wrong landmark.");

// Camera parameters
DEFINE_double(fov_x, 170.0,
              "Horizontal field of view in degrees.");
DEFINE_int32(xres, 640,
             "Horizontal resolution.");
DEFINE_int32(yres, 480,
             "Vertical resolution.");

// Environment parameters
DEFINE_double(visible_distance, 8.0,
             "Distance features are visible to camera.");
DEFINE_double(env_width, 6.0,
             "Width of the rectangular prism camera is inside.");
DEFINE_double(env_height, 6.0,
             "Height of the rectangular prism camera is inside.");
DEFINE_double(env_length, 20.0,
             "Length of the rectangular prism camera is inside.");
DEFINE_double(env_clutter, 0.5,
             "Distance within landmarks are located from the walls.");

// Initial error parameters
DEFINE_double(guess_angle_error, 0.04 * M_PI,
             "Std. dev. angle error of initial transform guess.");
DEFINE_double(guess_distance_error, 0.5,
             "Std. dev. distance error of initial transform guess.");

// Number of trials
DEFINE_int32(num_trials, 250,
             "Number of trials to gather statistics on");

// Ceres parameters
DEFINE_int32(max_num_iterations, 10000,
             "Maximum number of iterations for solver");

// RANSAC parameters
DEFINE_int32(ransac_inlier_tolerance, 3,
              "Maximum error in pixels for a projection to be considered correct.");
DEFINE_int32(ransac_iterations, 100,
              "Number of iterations of RANSAC.");

unsigned int rand_seed;
double rand_d() {
  return static_cast<double>(rand_r(&rand_seed)) / RAND_MAX;
}

Eigen::Vector3d EnvironmentIntersection(Eigen::Vector3d origin, Eigen::Vector3d dir) {
  double W = FLAGS_env_width / 2, H = FLAGS_env_height / 2, L = FLAGS_env_length / 2, P = FLAGS_env_clutter;
  // check each plane for intersection
  // x = -W plane
  // origin + dir * t = (-W, a, b)
  double t = (-W - origin.x()) / dir.x();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.z() >= -L && p.z() <= L && p.y() >= -H && p.y() <= H)
      return p + rand_d() * P * Eigen::Vector3d(0, 0, 1);
  }
  // x = W
  t = (W - origin.x()) / dir.x();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.z() >= -L && p.z() <= L && p.y() >= -H && p.y() <= H)
      return p + rand_d() * P * Eigen::Vector3d(0, 0, -1);
  }
  // y = -H
  t = (-H - origin.y()) / dir.y();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.x() >= -W && p.x() <= W && p.z() >= -L && p.z() <= L)
      return p + rand_d() * P * Eigen::Vector3d(0, 1, 0);
  }
  // y = H
  t = (H - origin.y()) / dir.y();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.x() >= -W && p.x() <= W && p.z() >= -L && p.z() <= L)
      return p + rand_d() * P * Eigen::Vector3d(0, -1, 0);
  }
  // z = -L
  t = (-L - origin.z()) / dir.z();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.x() >= -W && p.x() <= W && p.y() >= -H && p.y() <= H)
      return p + rand_d() * P * Eigen::Vector3d(0, 0, 1);
  }
  // z = L
  t = (L - origin.z()) / dir.z();
  Eigen::Vector3d p = origin + t * dir;
  return p + rand_d() * P * Eigen::Vector3d(0, 0, -1);
}

Eigen::Vector3d RandomLandmark(std::default_random_engine gen) {
  double W = FLAGS_env_width / 2, H = FLAGS_env_height / 2, L = FLAGS_env_length / 2, P = FLAGS_env_clutter;
  std::uniform_int_distribution<int> face(0, 5);
  std::uniform_real_distribution<double> uniform(-1.0, 1.0);
  std::uniform_real_distribution<double> clutter(0.0, P);
  int f = face(gen);
  if (f == 0)
    return Eigen::Vector3d(uniform(gen) * W, uniform(gen) * H, L - clutter(gen));
  else if (f == 1)
    return Eigen::Vector3d(uniform(gen) * W, uniform(gen) * H, -L + clutter(gen));
  else if (f == 2)
    return Eigen::Vector3d(uniform(gen) * W, H - clutter(gen), uniform(gen) * L);
  else if (f == 3)
    return Eigen::Vector3d(uniform(gen) * W, -H + clutter(gen), uniform(gen) * L);
  else if (f == 4)
    return Eigen::Vector3d(W - clutter(gen), uniform(gen) * H, uniform(gen) * L);
  return Eigen::Vector3d(-W + clutter(gen), uniform(gen) * H, uniform(gen) * L);
}

void GenerateLandmarkObservations(const camera::CameraModel & camera,
                                  std::vector<Eigen::Vector3d>* pid_to_xyz,
                                  std::vector<Eigen::Vector2d>* observations) {
  std::default_random_engine gen;
  std::normal_distribution<double> map_error(0.0, FLAGS_map_error);
  std::normal_distribution<double> feature_error(0.0, FLAGS_feature_error);
  std::uniform_real_distribution<double> uniform(0.0, 1.0);
  while (pid_to_xyz->size() < FLAGS_num_observations) {
    Eigen::Vector3d ray = camera.Ray(rand_d() * FLAGS_xres, rand_d() * FLAGS_yres);
    Eigen::Vector3d landmark = EnvironmentIntersection(camera.GetPosition(), ray);
    if (!camera.IsInFov(landmark) ||
        (landmark - camera.GetPosition()).norm() > FLAGS_visible_distance)
      continue;
    Eigen::Vector3d map_landmark =
      landmark + Eigen::Vector3d(map_error(gen), map_error(gen), map_error(gen));
    // Purposely introducing rounding error here.
    Eigen::Vector2i image_coords = (camera.ImageCoordinates(landmark).array() + 0.5).cast<int>();
    Eigen::Vector2d obs(image_coords.x() + feature_error(gen), image_coords.y() + feature_error(gen));

    if (FLAGS_mismatch_prob > 0.0 && uniform(gen) <= FLAGS_mismatch_prob)
      map_landmark = RandomLandmark(gen);
    pid_to_xyz->push_back(map_landmark);
    observations->push_back(obs);
  }
}

void EstimateRandomCameraError(double* dist_error, double* angle_error) {
  // initialize random camera model with specified parameters
  double C = 4 * FLAGS_env_clutter;
  double W = FLAGS_env_width - 2 * C, H = FLAGS_env_height - 2 * C, L = FLAGS_env_length - 2 * C;
  Eigen::Vector3d true_camera_pos(rand_d() * W - W / 2, rand_d() * H - H / 2, rand_d() * L - L / 2);
  Eigen::Matrix3d true_camera_rotation;
  true_camera_rotation = Eigen::AngleAxisd(rand_d() * M_2_PI, Eigen::Vector3d::UnitX())
                       * Eigen::AngleAxisd(rand_d() * M_2_PI, Eigen::Vector3d::UnitY())
                       * Eigen::AngleAxisd(rand_d() * M_2_PI, Eigen::Vector3d::UnitZ());
  camera::CameraModel camera(true_camera_pos, true_camera_rotation,
                     FLAGS_fov_x * M_PI / 180.0, FLAGS_xres, FLAGS_yres);

  // create landmarks in field of view and simulate observations
  std::vector<Eigen::Vector3d> pid_to_xyz;  // landmark locations
  std::vector<Eigen::Vector2d> observations;  // observed camera coordinates of each landmark
  GenerateLandmarkObservations(camera, &pid_to_xyz, &observations);

  // create initial pose estimate
  std::default_random_engine gen;
  std::normal_distribution<double> guess_dist_err(0.0, FLAGS_guess_distance_error);
  std::normal_distribution<double> guess_angle_err(0.0, FLAGS_guess_angle_error);
  Eigen::Vector3d guess_pos(true_camera_pos);
  guess_pos += Eigen::Vector3d(guess_dist_err(gen), guess_dist_err(gen), guess_dist_err(gen));
  Eigen::Matrix3d guess_rotation(true_camera_rotation);
  guess_rotation = guess_rotation * Eigen::AngleAxisd(guess_angle_err(gen), Eigen::Vector3d::UnitX())
                    * Eigen::AngleAxisd(guess_angle_err(gen), Eigen::Vector3d::UnitY())
                    * Eigen::AngleAxisd(guess_angle_err(gen), Eigen::Vector3d::UnitZ());
  camera::CameraModel guess(guess_pos, guess_rotation, camera.GetParameters());

  if (!FLAGS_ransac) {
    // Solve the problem
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::ITERATIVE_SCHUR;
    options.num_threads = FLAGS_num_threads;
    options.max_num_iterations = FLAGS_max_num_iterations;
    options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary summary;
    sparse_mapping::EstimateCamera(&guess, &pid_to_xyz, observations, options, &summary);
  } else {
    sparse_mapping::RansacEstimateCamera(pid_to_xyz, observations, FLAGS_ransac_iterations,
                                         FLAGS_ransac_inlier_tolerance, &guess);
  }

  Eigen::Vector3d orig_angle = camera.GetRotation() * Eigen::Vector3d::UnitX();
  Eigen::Vector3d observed_angle = guess.GetRotation() * Eigen::Vector3d::UnitX();

  // set errors
  *dist_error = (true_camera_pos - guess.GetPosition()).norm();
  *angle_error = acos(observed_angle.dot(orig_angle));
}

int main(int argc, char** argv) {
  common::InitFreeFlyerApplication(&argc, &argv);
  rand_seed = time(NULL);

  double dist_error_mean = 0.0, dist_error_mean_2 = 0.0;
  double angle_error_mean = 0.0, angle_error_mean_2 = 0.0;
  for (int i = 0; i < FLAGS_num_trials; i++) {
    double dist_error = 0.0, angle_error = 0.0;
    EstimateRandomCameraError(&dist_error, &angle_error);
    dist_error_mean += dist_error;
    dist_error_mean_2 += dist_error * dist_error;
    angle_error_mean += angle_error;
    angle_error_mean_2 += angle_error * angle_error;
  }
  dist_error_mean    /= FLAGS_num_trials;
  angle_error_mean   /= FLAGS_num_trials;
  dist_error_mean_2  /= FLAGS_num_trials;
  angle_error_mean_2 /= FLAGS_num_trials;
  double dist_std_dev = sqrt(dist_error_mean_2 - dist_error_mean * dist_error_mean);
  double angle_std_dev = sqrt(angle_error_mean_2 - angle_error_mean * angle_error_mean);
  std::cout << "Distance Error: " << (dist_error_mean * 100.0) << " +/- " <<
    (dist_std_dev * 100.0) << " cm" << "\n";
  std::cout << "Angle Error: " << (angle_error_mean * 180.0 / M_PI) << " +/- " <<
    (angle_std_dev * 180.0 / M_PI) << " degrees" << "\n";

  return 0;
}


// Copyright (c) 2019-2020 Ricardo Tonet
// Use of this source code is governed by the MIT license, see LICENSE

/**
 * \file trajectory_planner.cpp
 * \brief Defines TrajectoryPlanner class for robot arm trajectory planning.
 */

#include <smalldrop_toolpath/trajectory_planner.h>
#include <smalldrop_state/exceptions.h>

#include <Eigen/Dense>

namespace smalldrop
{
namespace smalldrop_toolpath
{
/*****************************************************************************************
 * Public methods & constructors/destructors
 *****************************************************************************************/

/**
 * \copybrief TrajectoryPlanner::TrajectoryPlanner(const double duration, const double frequency, const PLAN_MODE
 * plan_mode)
 */
TrajectoryPlanner::TrajectoryPlanner(const double duration, const double frequency, const PLAN_MODE plan_mode)
  : duration_(duration), frequency_(frequency), plan_mode_(plan_mode), max_speed_(50)
{
}

/**
 * \copybrief TrajectoryPlanner::TrajectoryPlanner(const double duration, const double frequency, const PLAN_MODE plan_mode, const double max_speed)
 */
TrajectoryPlanner::TrajectoryPlanner(const double duration, const double frequency, const PLAN_MODE plan_mode, const double max_speed)
  : duration_(duration), frequency_(frequency), plan_mode_(plan_mode), max_speed_(max_speed)
{
}

/**
 * \copybrief TrajectoryPlanner::plan(const Path& path)
 */
Trajectory TrajectoryPlanner::plan(const Path& path)
{
  // Check if speed exceeds upper bound
  if (duration_ > 0 && path.length()/duration_ > max_speed_)
    throw smalldrop_state::TrajectoryMaxSpeedExceededException();

  poses_t trajectory;
  poses_t poses = path.poses();
  unsigned int npoints = poses.size();
  double t_interval = duration_ / ((double)npoints-1);

  Trajectory t(trajectory, 0, duration_);

  if (duration_ > 0 && frequency_ > 0)
  {
    for (size_t i = 0; i < npoints - 1; i++)
    {
      double t = 0;
      while (t <= t_interval + 0.0001)
      {
        switch (plan_mode_)
        {
          case PLAN_MODE::LSPB:
            trajectory.push_back(lspb(poses[i], poses[i + 1], LSPB_ACCEL, 0, t_interval, t));
            break;
          default:  // PLAN_MODE::POLY3
            trajectory.push_back(poly3(poses[i], poses[i + 1], 0, t_interval, t));
            break;
        }
        t += 1 / frequency_;
      }
    }

    Trajectory traj(trajectory, path.length(), duration_);
    t = traj;
  }

  return t;
}

/**
 * \copybrief TrajectoryPlanner::plan(const paths_t paths)
 */
Trajectory TrajectoryPlanner::plan(const paths_t paths)
{
  // Check if speed exceeds upper bound
  if (duration_ > 0 && getFullPathLength(paths)/duration_ > max_speed_)
    throw smalldrop_state::TrajectoryMaxSpeedExceededException();

  poses_t trajectory;
  double path_duration = duration_ / paths.size();

  Trajectory t(trajectory, 0, duration_);

  if (duration_ > 0 && frequency_ > 0)
  {
    for (size_t i = 0; i < paths.size(); i++)
    {
      poses_t poses = paths[i].poses();
      unsigned int npoints = poses.size();
      double t_interval = path_duration / ((double)npoints-1);

      for (size_t j = 0; j < npoints - 1; j++)
      {
        double t = 0;
        while (t <= t_interval + 0.0001)
        {
          switch (plan_mode_)
          {
            case PLAN_MODE::LSPB:
              trajectory.push_back(lspb(poses[j], poses[j + 1], LSPB_ACCEL, 0, t_interval, t));
              break;
            default:  // PLAN_MODE::POLY3
              trajectory.push_back(poly3(poses[j], poses[j + 1], 0, t_interval, t));
              break;
          }
          t += 1 / frequency_;
        }
      }
    }

    Trajectory traj(trajectory, getFullPathLength(paths), duration_);
    t = traj;
  }

  return t;
}

/**
 * \copybrief TrajectoryPlanner::plan(const Toolpath& toolpath)
 */
Trajectory TrajectoryPlanner::plan(const ToolPath& toolpath)
{
  // Check if speed exceeds upper bound
  if (duration_ > 0 && toolpath.length()/duration_ > max_speed_)
    throw smalldrop_state::TrajectoryMaxSpeedExceededException();

  poses_t trajectory;
  path_actions_t trajectory_actions;
  poses_t poses = toolpath.poses();
  unsigned int npoints = poses.size();
  double t_interval = duration_ / ((double)npoints-1);

  Trajectory t(trajectory, trajectory_actions, 0, duration_);

  if (duration_ > 0 && frequency_ > 0)
  {
    for (size_t i = 0; i < npoints - 1; i++)
    {
      double t = 0;
      while (t <= t_interval + 0.0001)
      {
        switch (plan_mode_)
        {
          case PLAN_MODE::LSPB:
            trajectory.push_back(lspb(poses[i], poses[i + 1], LSPB_ACCEL, 0, t_interval, t));
            break;
          default:  // PLAN_MODE::POLY3
            trajectory.push_back(poly3(poses[i], poses[i + 1], 0, t_interval, t));
            break;
        }
        t += 1 / frequency_;
      }
    }

    Trajectory traj(trajectory, toolpath.actions(), toolpath.length(), duration_);
    t = traj;
  }

  return t;
}

/**
 * \copybrief Trajectory TrajectoryPlanner::plan(const toolpaths_t toolpaths)
 */
Trajectory TrajectoryPlanner::plan(const toolpaths_t toolpaths)
{
  // Check if speed exceeds upper bound
  if (duration_ > 0 && getFullPathLength(toolpaths)/duration_ > max_speed_)
    throw smalldrop_state::TrajectoryMaxSpeedExceededException();

  poses_t trajectory;
  path_actions_t trajectory_actions;
  double path_duration = duration_ / toolpaths.size();

  Trajectory t(trajectory, trajectory_actions, 0, duration_);

  if (duration_ > 0 && frequency_ > 0)
  {
    for (size_t i = 0; i < toolpaths.size(); i++)
    {
      poses_t poses = toolpaths[i].poses();
      path_actions_t actions = toolpaths[i].actions();
      unsigned int npoints = poses.size();
      double t_interval = path_duration / ((double)npoints-1);

      for (size_t j = 0; j < npoints - 1; j++)
      {
        PRINT_ACTION action = actions[j];
        double t = 0;
        while (t <= t_interval + 0.0001)
        {
          switch (plan_mode_)
          {
            case PLAN_MODE::LSPB:
              trajectory.push_back(lspb(poses[j], poses[j + 1], LSPB_ACCEL, 0, t_interval, t));
              trajectory_actions.push_back(action);
              break;
            default:  // PLAN_MODE::POLY3
              trajectory.push_back(poly3(poses[j], poses[j + 1], 0, t_interval, t));
              trajectory_actions.push_back(action);
              break;
          }
          t += 1 / frequency_;
        }
      }
    }

    Trajectory traj(trajectory, trajectory_actions, getFullPathLength(toolpaths), duration_);
    t = traj;
  }

  return t;
}

/**
 * \copybrief TrajectoryPlanner::setDuration()
 */
void TrajectoryPlanner::setDuration(const double duration)
{
  duration_ = duration;
}

/**
 * \copybrief TrajectoryPlanner::setFrequency()
 */
void TrajectoryPlanner::setFrequency(const double frequency)
{
  frequency_ = frequency;
}

/**
 * \copybrief TrajectoryPlanner::setPlanMode()
 */
void TrajectoryPlanner::setPlanMode(const PLAN_MODE plan_mode)
{
  plan_mode_ = plan_mode;
}

/**
 * \copybrief TrajectoryPlanner::setMaxSpeed(const double max_speed)
 */
void TrajectoryPlanner::setMaxSpeed(const double max_speed)
{
  max_speed_ = max_speed;
}

/*****************************************************************************************
 * Private methods
 *****************************************************************************************/

/**
 * \copybrief double viaVelocity(const double pos_before, const double pos, const double pos_after, const double
 * t_before, const double t, const double t_after) const
 */
double TrajectoryPlanner::viaVelocity(const double pos_before, const double pos, const double pos_after,
                                      const double t_before, const double t, const double t_after) const
{
  double velocity_i, velocity_ii;

  velocity_i = (pos - pos_before) / (t - t_before);  // Mean velocity before via point
  velocity_ii = (pos_after - pos) / (t_after - t);   // Mean velocity after via point

  if ((velocity_i * velocity_ii) < 0)  // Mean velocities change signal at via point
    return 0.0;                        // Velocity at via point is zero
  else
    return 0.5 * (velocity_ii + velocity_i);  // Velocity at via point is the average of the mean velocities
}

/**
 * \copybrief pose_t poly3(const pose_t pose_i, const pose_t pose_f, const double t0, const double tf, const double t)
 * const
 */
pose_t TrajectoryPlanner::poly3(const pose_t pose_i, const pose_t pose_f, const double t0, const double tf,
                                const double t) const
{
  std::vector<double> a0 = { pose_i.position.x, pose_i.position.y, pose_i.position.z };
  std::vector<double> a1 = { 0, 0, 0 };
  std::vector<double> a2 = { (3 / pow(tf - t0, 2)) * (pose_f.position.x - pose_i.position.x),
                             (3 / pow(tf - t0, 2)) * (pose_f.position.y - pose_i.position.y),
                             (3 / pow(tf - t0, 2)) * (pose_f.position.z - pose_i.position.z) };
  std::vector<double> a3 = { -(2 / pow(tf - t0, 3)) * (pose_f.position.x - pose_i.position.x),
                             -(2 / pow(tf - t0, 3)) * (pose_f.position.y - pose_i.position.y),
                             -(2 / pow(tf - t0, 3)) * (pose_f.position.z - pose_i.position.z) };

  // Apply slerp
  Eigen::Quaterniond qres;
  Eigen::Quaterniond q0(pose_i.orientation.w, pose_i.orientation.x, pose_i.orientation.y, pose_i.orientation.z);
  Eigen::Quaterniond qf(pose_f.orientation.w, pose_f.orientation.x, pose_f.orientation.y, pose_f.orientation.z);
  qres = q0.slerp((t - t0) / (tf - t0), qf);

  pose_t pose;
  pose.position.x = a0[0] + a1[0] * (t - t0) + a2[0] * pow(t - t0, 2) + a3[0] * pow(t - t0, 3);
  pose.position.y = a0[1] + a1[1] * (t - t0) + a2[1] * pow(t - t0, 2) + a3[1] * pow(t - t0, 3);
  pose.position.z = a0[2] + a1[2] * (t - t0) + a2[2] * pow(t - t0, 2) + a3[2] * pow(t - t0, 3);
  pose.orientation.x = qres.x();
  pose.orientation.y = qres.y();
  pose.orientation.z = qres.z();
  pose.orientation.w = qres.w();

  return pose;
}

/**
 * \copybrief pose_t poly3c(const pose_t pose_i, const pose_t pose_f, const std::vector<double> velocity0, const
 * std::vector<double> velocityf, const double t0, const double tf, const double t) const
 */
pose_t TrajectoryPlanner::poly3c(const pose_t pose_i, const pose_t pose_f, const std::vector<double> velocity0,
                                 const std::vector<double> velocityf, const double t0, const double tf,
                                 const double t) const
{
  std::vector<double> a0 = { pose_i.position.x, pose_i.position.y, pose_i.position.z };
  std::vector<double> a1 = { velocity0[0], velocity0[1], velocity0[2] };
  std::vector<double> a2 = { (3 / pow(tf - t0, 2)) * (pose_f.position.x - pose_i.position.x) -
                                 (2 / (tf - t0)) * velocity0[0] - (1 / (tf - t0)) * velocityf[0],
                             (3 / pow(tf - t0, 2)) * (pose_f.position.y - pose_i.position.y) -
                                 (2 / (tf - t0)) * velocity0[1] - (1 / (tf - t0)) * velocityf[1],
                             (3 / pow(tf - t0, 2)) * (pose_f.position.z - pose_i.position.z) -
                                 (2 / (tf - t0)) * velocity0[2] - (1 / (tf - t0)) * velocityf[2] };
  std::vector<double> a3 = { -(2 / pow(tf - t0, 3)) * (pose_f.position.x - pose_i.position.x) +
                                 (1 / pow(tf - t0, 2)) * (velocityf[0] + velocity0[0]),
                             -(2 / pow(tf - t0, 3)) * (pose_f.position.y - pose_i.position.y) +
                                 (1 / pow(tf - t0, 2)) * (velocityf[1] + velocity0[1]),
                             -(2 / pow(tf - t0, 3)) * (pose_f.position.z - pose_i.position.z) +
                                 (1 / pow(tf - t0, 2)) * (velocityf[2] + velocity0[2]) };

  // Apply slerp
  Eigen::Quaterniond qres;
  Eigen::Quaterniond q0(pose_i.orientation.w, pose_i.orientation.x, pose_i.orientation.y, pose_i.orientation.z);
  Eigen::Quaterniond qf(pose_f.orientation.w, pose_f.orientation.x, pose_f.orientation.y, pose_f.orientation.z);
  qres = q0.slerp((t - t0) / (tf - t0), qf);

  pose_t pose;
  pose.position.x = a0[0] + a1[0] * (t - t0) + a2[0] * pow(t - t0, 2) + a3[0] * pow(t - t0, 3);
  pose.position.y = a0[1] + a1[1] * (t - t0) + a2[1] * pow(t - t0, 2) + a3[1] * pow(t - t0, 3);
  pose.position.z = a0[2] + a1[2] * (t - t0) + a2[2] * pow(t - t0, 2) + a3[2] * pow(t - t0, 3);
  pose.orientation.x = qres.x();
  pose.orientation.y = qres.y();
  pose.orientation.z = qres.z();
  pose.orientation.w = qres.w();

  return pose;
}

/**
 * \copybrief pose_t poly3cVias(const poses_t poses, const std::vector<double> times, const double t) const
 */
pose_t TrajectoryPlanner::poly3cVias(const poses_t poses, const std::vector<double> times, const double t) const
{
  if (poses.size() != times.size())
  {
    throw std::runtime_error("The poses vector should be the same size as times vector.");
  }

  std::vector<double> velocity0;
  std::vector<double> velocityf;
  velocity0.resize(3, 0);
  velocityf.resize(3, 0);

  pose_t pose;
  for (size_t i = 1; i < times.size(); i++)
  {
    if (t >= times[i - 1] && t <= times[i])
    {
      if (i == 1)
      {
        // velocity0 is already {0, 0, 0}
        velocityf[0] = viaVelocity(poses[i - 1].position.x, poses[i].position.x, poses[i + 1].position.x, times[i - 1],
                                   times[i], times[i + 1]);
        velocityf[1] = viaVelocity(poses[i - 1].position.y, poses[i].position.y, poses[i + 1].position.y, times[i - 1],
                                   times[i], times[i + 1]);
        velocityf[2] = viaVelocity(poses[i - 1].position.z, poses[i].position.z, poses[i + 1].position.z, times[i - 1],
                                   times[i], times[i + 1]);
      }
      else if (i == times.size() - 1)
      {
        velocity0[0] = viaVelocity(poses[i - 1].position.x, poses[i].position.x, poses[i + 1].position.x, times[i - 1],
                                   times[i], times[i + 1]);
        velocity0[1] = viaVelocity(poses[i - 1].position.y, poses[i].position.y, poses[i + 1].position.y, times[i - 1],
                                   times[i], times[i + 1]);
        velocity0[2] = viaVelocity(poses[i - 1].position.z, poses[i].position.z, poses[i + 1].position.z, times[i - 1],
                                   times[i], times[i + 1]);
        // velocityf is already {0, 0, 0}
      }
      else
      {
        velocity0[0] = viaVelocity(poses[i - 2].position.x, poses[i - 1].position.x, poses[i].position.x, times[i - 1],
                                   times[i], times[i + 1]);
        velocity0[1] = viaVelocity(poses[i - 2].position.y, poses[i - 1].position.y, poses[i].position.y, times[i - 1],
                                   times[i], times[i + 1]);
        velocity0[2] = viaVelocity(poses[i - 2].position.z, poses[i - 1].position.z, poses[i].position.z, times[i - 1],
                                   times[i], times[i + 1]);

        velocityf[0] = viaVelocity(poses[i - 1].position.x, poses[i].position.x, poses[i + 1].position.x, times[i - 1],
                                   times[i], times[i + 1]);
        velocityf[1] = viaVelocity(poses[i - 1].position.y, poses[i].position.y, poses[i + 1].position.y, times[i - 1],
                                   times[i], times[i + 1]);
        velocityf[2] = viaVelocity(poses[i - 1].position.z, poses[i].position.z, poses[i + 1].position.z, times[i - 1],
                                   times[i], times[i + 1]);
      }

      pose = poly3c(poses[i - 1], poses[i], velocity0, velocityf, times[i - 1], times[i], t);
      break;
    }
  }

  return pose;
}

/**
 * \copybrief pose_t lspb(const pose_t pose_i, const pose_t pose_f, const double accel, const double t0, const double
 * tf, const double t) const
 */
pose_t TrajectoryPlanner::lspb(const pose_t pose_i, const pose_t pose_f, const double accel, const double t0,
                               const double tf, const double t) const
{
  // It is assumed that accel param is always positive

  if (accel < (4 * abs(pose_f.position.x - pose_i.position.x)) / pow(tf - t0, 2) ||
      accel < (4 * abs(pose_f.position.y - pose_i.position.y)) / pow(tf - t0, 2) ||
      accel < (4 * abs(pose_f.position.z - pose_i.position.z)) / pow(tf - t0, 2))
  {
    throw std::runtime_error("Invalid acceleration! The value needs to be larger.");
  }

  // Change accel sign depending on growth direction of position (eg. x0 > xf => accel0 < 0)
  std::vector<double> accel_vec;
  accel_vec.resize(3, 0);
  accel_vec[0] = (pose_i.position.x > pose_f.position.x) ? -accel : accel;
  accel_vec[1] = (pose_i.position.y > pose_f.position.y) ? -accel : accel;
  accel_vec[2] = (pose_i.position.z > pose_f.position.z) ? -accel : accel;

  pose_t pose;
  std::vector<double> velocity, acceleration;
  velocity.resize(3, 0);
  acceleration.resize(3, 0);
  std::vector<double> delta_tb;
  delta_tb.resize(3, 0);
  delta_tb[0] =
      (tf - t0) / 2 - sqrt(pow(tf - t0, 2) / 4 - abs(pose_f.position.x - pose_i.position.x) / abs(accel_vec[0]));
  delta_tb[1] =
      (tf - t0) / 2 - sqrt(pow(tf - t0, 2) / 4 - abs(pose_f.position.y - pose_i.position.y) / abs(accel_vec[1]));
  delta_tb[2] =
      (tf - t0) / 2 - sqrt(pow(tf - t0, 2) / 4 - abs(pose_f.position.z - pose_i.position.z) / abs(accel_vec[2]));
  std::vector<double> tb;
  tb.resize(3, 0);
  tb[0] = t0 + delta_tb[0];
  tb[1] = t0 + delta_tb[1];
  tb[2] = t0 + delta_tb[2];
  std::vector<double> velocity_b;
  velocity_b.resize(3, 0);
  velocity_b[0] = accel_vec[0] * delta_tb[0];
  velocity_b[1] = accel_vec[1] * delta_tb[1];
  velocity_b[2] = accel_vec[2] * delta_tb[2];
  std::vector<double> poseb;
  poseb.resize(3, 0);
  poseb[0] = pose_i.position.x + 0.5 * accel_vec[0] * pow(delta_tb[0], 2);
  poseb[1] = pose_i.position.y + 0.5 * accel_vec[1] * pow(delta_tb[1], 2);
  poseb[2] = pose_i.position.z + 0.5 * accel_vec[2] * pow(delta_tb[2], 2);
  std::vector<double> posebf;
  posebf.resize(3, 0);
  posebf[0] = poseb[0] + velocity_b[0] * ((tf - t0) - 2 * delta_tb[0]);
  posebf[1] = poseb[1] + velocity_b[1] * ((tf - t0) - 2 * delta_tb[1]);
  posebf[2] = poseb[2] + velocity_b[2] * ((tf - t0) - 2 * delta_tb[2]);

  // Apply slerp
  Eigen::Quaterniond qres;
  Eigen::Quaterniond q0(pose_i.orientation.w, pose_i.orientation.x, pose_i.orientation.y, pose_i.orientation.z);
  Eigen::Quaterniond qf(pose_f.orientation.w, pose_f.orientation.x, pose_f.orientation.y, pose_f.orientation.z);
  qres = q0.slerp((t - t0) / (tf - t0), qf);
  pose.orientation.x = qres.x();
  pose.orientation.y = qres.y();
  pose.orientation.z = qres.z();
  pose.orientation.w = qres.w();

  for (size_t i = 0; i < 3; i++)
  {
    if (t >= t0 && t <= tb[i])
    {
      acceleration[i] = accel_vec[i];
      velocity[i] = accel_vec[i] * (t - t0);
      switch (i)
      {
        case 1:
          pose.position.y = pose_i.position.y + 0.5 * acceleration[i] * pow(t - t0, 2);
          break;
        case 2:
          pose.position.z = pose_i.position.z + 0.5 * acceleration[i] * pow(t - t0, 2);
          break;
        default:
          pose.position.x = pose_i.position.x + 0.5 * acceleration[i] * pow(t - t0, 2);
          break;
      }
    }
    else if (t >= (tf - delta_tb[i]) && t <= tf + 0.0001)
    {
      acceleration[i] = -accel_vec[i];
      velocity[i] = velocity_b[i] + acceleration[i] * (t - (tf - delta_tb[i]));
      switch (i)
      {
        case 1:
          pose.position.y = posebf[i] + velocity_b[i] * (t - (tf - delta_tb[i])) +
                            0.5 * acceleration[i] * pow(t - (tf - delta_tb[i]), 2);
          break;
        case 2:
          pose.position.z = posebf[i] + velocity_b[i] * (t - (tf - delta_tb[i])) +
                            0.5 * acceleration[i] * pow(t - (tf - delta_tb[i]), 2);
          break;
        default:
          pose.position.x = posebf[i] + velocity_b[i] * (t - (tf - delta_tb[i])) +
                            0.5 * acceleration[i] * pow(t - (tf - delta_tb[i]), 2);
          break;
      }
    }
    else
    {
      acceleration[i] = 0;
      velocity[i] = velocity_b[i];
      switch (i)
      {
        case 1:
          pose.position.y = poseb[i] + velocity_b[i] * (t - tb[i]);
          break;
        case 2:
          pose.position.z = poseb[i] + velocity_b[i] * (t - tb[i]);
          break;
        default:
          pose.position.x = poseb[i] + velocity_b[i] * (t - tb[i]);
          break;
      }
    }
  }

  return pose;
}

/**
 * \copybrief TrajectoryPlanner::getFullPathLength(const paths_t path)
 */
double TrajectoryPlanner::getFullPathLength(const paths_t paths)
{
  double total_length = 0;
  for (size_t i = 0; i < paths.size(); i++)
    total_length += paths[i].length();

  return total_length;
}

/**
 * \copybrief TrajectoryPlanner::getFullPathLength(const toolpaths_t path)
 */
double TrajectoryPlanner::getFullPathLength(const toolpaths_t toolpaths)
{
  double total_length = 0;
  for (size_t i = 0; i < toolpaths.size(); i++)
    total_length += toolpaths[i].length();

  return total_length;
}

/**
 * \copybrief TrajectoryPlanner::getFullPathSize(const paths_t path)
 */
double TrajectoryPlanner::getFullPathSize(const paths_t paths)
{
  double total_size = 0;
  for (size_t i = 0; i < paths.size(); i++)
    total_size += paths[i].poses().size();

  return total_size;
}

/**
 * \copybrief TrajectoryPlanner::getFullPathSize(const toolpaths_t toolpaths)
 */
double TrajectoryPlanner::getFullPathSize(const toolpaths_t toolpaths)
{
  double total_size = 0;
  for (size_t i = 0; i < toolpaths.size(); i++)
    total_size += toolpaths[i].poses().size();

  return total_size;
}

}  // namespace smalldrop_toolpath

}  // namespace smalldrop


//
// this is for testing the timing of the given individual integrals
//

#include "libgen.h"
#include <boost/algorithm/string.hpp>   // string handling
#include <boost/lexical_cast.hpp>
#include <boost/timer.hpp>
using namespace boost;

extern void hgp_os_eri_sp_sp_sp_sp(const UInt& inp2, const UInt& jnp2, const Double& pMax,
		const Double& omega, const Double* icoe, const Double* iexp, const Double* ifac,
		const Double* P, const Double* A, const Double* B, const Double* jcoe, const Double* jexp,
		const Double* jfac, const Double* Q, const Double* C, const Double* D, Double* abcd);

extern void hgp_os_eri_sp_sp_sp_sp_d1(const UInt& inp2, const UInt& jnp2, const Double& pMax,
		const Double& omega, const Double* icoe, const Double* iexp, const Double* iexpdiff,
		const Double* ifac, const Double* P, const Double* A, const Double* B, const Double* jcoe,
		const Double* jexp, const Double* jexpdiff, const Double* jfac, const Double* Q, const Double* C,
		const Double* D, Double* abcd);

Int main(int argc, char* argv[])
{
	/////////////////////////////////////////////////////////////////////////////
	// setting the shell data
	// all of shell data are in default to be composite shell
	// which contains two sub-shells
	/////////////////////////////////////////////////////////////////////////////

	// shell 1
	Int inp = 2;
	vector<Double> iexp(inp);
	vector<Double> icoe(2*inp);
	iexp[0] = 0.1812885;
	iexp[1] = 0.0442493;
	icoe[0] = 0.1193324;
	icoe[1] = 0.1608542;
	icoe[2] = 0.8434564;
	icoe[3] = 0.0689991;
	Double A[3];
	A[0]    = 1.0;
	A[1]    = 0.0;
	A[2]    = 0.0;

	// shell 2
	Int jnp = 2;
	vector<Double> jexp(jnp);
	vector<Double> jcoe(2*jnp);
	jexp[0] = 0.1439130;
	jexp[1] = 0.0914760;
	jcoe[0] = 6.139703E-02;
	jcoe[1] = 3.061130E-01;
	jcoe[2] = 1.154890;
	jcoe[3] = 0.1891265;
	Double B[3];
	B[0]    = 0.0;
	B[1]    = 1.0;
	B[2]    = 0.0;

	// shell 3
	Int knp = 2;
	vector<Double> kexp(knp);
	vector<Double> kcoe(2*knp);
	kexp[0] = 0.03628970;
	kexp[1] = 0.14786010;
	kcoe[0] = 0.09996723;
	kcoe[1] = 0.39951283;
	kcoe[2] = 0.70011547;
	kcoe[3] = 0.15591627;
	Double C[3];
	C[0]    = 0.0;
	C[1]    = 0.0;
	C[2]    = 1.0;

	// shell 4
	Int lnp = 2;
	vector<Double> lexp(lnp);
	vector<Double> lcoe(2*lnp);
	lexp[0] = 0.01982050;
	lexp[1] = 0.16906180;
	lcoe[0] = 0.06723;
	lcoe[1] = 0.51283;
	lcoe[2] = 0.30011547;
	lcoe[3] = 0.5591627;
	Double D[3];
	D[0]    = 0.0;
	D[1]    = 1.0;
	D[2]    = 1.0;

	/////////////////////////////////////////////////////////////////////////////
	// setting the shell data
	// all of shell data are in default to be composite shell
	// which contains two sub-shells
	/////////////////////////////////////////////////////////////////////////////
	//
	// form the data for hgp calculation
	// firstly it's the bra side
	//
	Double AB2 = (A[0]-B[0])*(A[0]-B[0])+(A[1]-B[1])*(A[1]-B[1])+(A[2]-B[2])*(A[2]-B[2]);
	Int inp2 = inp*jnp;
	vector<Double> iexp2(inp2,ZERO);
	vector<Double> fbra(inp2,ZERO);
	vector<Double> P(3*inp2,ZERO);
	vector<Double> iexpdiff(inp2,ZERO);
	Int count = 0;
	for(Int jp=0; jp<jnp; jp++) {
		for(Int ip=0; ip<inp; ip++) {

			// prefactors etc.
			Double ia   = iexp[ip];
			Double ja   = jexp[jp];
			Double alpla= ia+ja;
			Double diff = ia-ja;
			Double ab   = -ia*ja/alpla;
			Double pref = exp(ab*AB2)*pow(PI/alpla,1.5E0);
			iexp2[count]= ONE/alpla;
			fbra[count] = pref;
			iexpdiff[count]= diff;

			// form P point according to the
			// Gaussian pritimive product theorem
			Double adab = ia/alpla;
			Double bdab = ja/alpla;
			Double Px   = A[0]*adab + B[0]*bdab;
			Double Py   = A[1]*adab + B[1]*bdab;
			Double Pz   = A[2]*adab + B[2]*bdab;
			P[3*count+0]= Px;
			P[3*count+1]= Py;
			P[3*count+2]= Pz;
			count++;
		}
	}

	// now it's ket side data
	Double CD2 = (C[0]-D[0])*(C[0]-D[0])+(C[1]-D[1])*(C[1]-D[1])+(C[2]-D[2])*(C[2]-D[2]);
	Int jnp2 = knp*lnp;
	vector<Double> jexp2(jnp2,ZERO);
	vector<Double> jexpdiff(jnp2,ZERO);
	vector<Double> fket(jnp2,ZERO);
	vector<Double> Q(3*jnp2,ZERO);
	count = 0;
	for(Int lp=0; lp<lnp; lp++) {
		for(Int kp=0; kp<knp; kp++) {

			// prefactors etc.
			Double ia   = kexp[kp];
			Double ja   = lexp[lp];
			Double alpla= ia+ja;
			Double diff = ia-ja;
			Double ab   = -ia*ja/alpla;
			Double pref = exp(ab*CD2)*pow(PI/alpla,1.5E0);
			jexp2[count]= ONE/alpla;
			fket[count] = pref;
			jexpdiff[count]= diff;

			// form P point according to the
			// Gaussian pritimive product theorem
			Double adab = ia/alpla;
			Double bdab = ja/alpla;
			Double Qx   = C[0]*adab + D[0]*bdab;
			Double Qy   = C[1]*adab + D[1]*bdab;
			Double Qz   = C[2]*adab + D[2]*bdab;
			Q[3*count+0]= Qx;
			Q[3*count+1]= Qy;
			Q[3*count+2]= Qz;
			count++;
		}
	}

	/////////////////////////////////////////////////////////////////////////////
	// now it's the formal timing compare
	/////////////////////////////////////////////////////////////////////////////

	// set the angular momentum of the results
	// here the user need to do that
	Int lmin1 = 0;
	Int lmax1 = 1;
	Int lmin2 = 0;
	Int lmax2 = 1;
	Int lmin3 = 0;
	Int lmax3 = 1;
	Int lmin4 = 0;
	Int lmax4 = 1;
	Int nBas1 = ((lmax1+1)*(lmax1+2)*(lmax1+3)-lmin1*(lmin1+1)*(lmin1+2))/6;
	Int nBas2 = ((lmax2+1)*(lmax2+2)*(lmax2+3)-lmin2*(lmin2+1)*(lmin2+2))/6;
	Int nBas3 = ((lmax3+1)*(lmax3+2)*(lmax3+3)-lmin3*(lmin3+1)*(lmin3+2))/6;
	Int nBas4 = ((lmax4+1)*(lmax4+2)*(lmax4+3)-lmin4*(lmin4+1)*(lmin4+2))/6;

	// make the coefficients pair
	// bra side
	Int nL1 = lmax1-lmin1+1;
	Int nL2 = lmax2-lmin2+1;
	count = 0;
	vector<Double> braCoePair(inp*jnp*nL1*nL2,ZERO);
	for(Int j=0; j<nL2; j++) {
		for(Int i=0; i<nL1; i++) {
			for(Int jp=0; jp<jnp; jp++) {
				for(Int ip=0; ip<inp; ip++) {
					Double ic = icoe[ip+i*inp];
					Double jc = jcoe[jp+j*jnp];
					braCoePair[count] = ic*jc;
					count++;
				}
			}
		}
	}

	// ket side
	nL1 = lmax3-lmin3+1;
	nL2 = lmax4-lmin4+1;
	count = 0;
	vector<Double> ketCoePair(knp*lnp*nL1*nL2,ZERO);
	for(Int j=0; j<nL2; j++) {
		for(Int i=0; i<nL1; i++) {
			for(Int jp=0; jp<lnp; jp++) {
				for(Int ip=0; ip<knp; ip++) {
					Double ic = kcoe[ip+i*inp];
					Double jc = lcoe[jp+j*jnp];
					ketCoePair[count] = ic*jc;
					count++;
				}
			}
		}
	}

	// now set up the result vectors
	// N is the total number of running times
	Int N = 1000000;
	Double pMax   = 1.0E0;
	Double omega  = 0.0E0;
	vector<Double> result1(nBas1*nBas2*nBas3*nBas4);
	timer t1;
	for(Int i=0; i<N; i++) {
		hgp_os_eri_sp_sp_sp_sp(inp2,jnp2,pMax,omega,
				&braCoePair.front(),&iexp2.front(),&fbra.front(),&P.front(),A,B,
				&ketCoePair.front(),&jexp2.front(),&fket.front(),&Q.front(),C,D,
				&result1.front());
	}
	printf("hgp sp_sp_sp_sp energy time consuming: %-14.7f\n", t1.elapsed());

	// this is another integral file
	vector<Double> result2(nBas1*nBas2*nBas3*nBas4*9); // dimension is responsible by the user
	timer t2;
	for(Int i=0; i<N; i++) {
		hgp_os_eri_sp_sp_sp_sp_d1(inp2,jnp2,pMax,omega,
				&braCoePair.front(),&iexp2.front(),&iexpdiff.front(),&fbra.front(),&P.front(),A,B,
				&ketCoePair.front(),&jexp2.front(),&jexpdiff.front(),&fket.front(),&Q.front(),C,D,
				&result2.front());
	}
	printf("hgp sp_sp_sp_sp gradient time consuming: %-14.7f\n", t2.elapsed());

	return 0;

}


//*****************************************************************************
// Class: CLaricobiusNigrinus
//
//
// Description: the CLaricobiusNigrinus represents a group of LNF insect. scale by m_ScaleFactor
//*****************************************************************************
// 15/10/2019   Rémi Saint-Amant    Creation
//*****************************************************************************

#include "LaricobiusNigrinusEquations.h"
#include "LaricobiusNigrinus.h"
//#include <boost/math/distributions/weibull.hpp>
#include <boost/math/distributions/logistic.hpp>

using namespace std;
using namespace WBSF::HOURLY_DATA;
using namespace WBSF::LNF;

namespace WBSF
{

	//*********************************************************************************
	//CLaricobiusNigrinus class

	//*****************************************************************************
	// Object creator
	//
	// Input: See CIndividual creator
	//
	// Note: m_RDR (relative Development Rate)  member is init with random values.
	//*****************************************************************************
	CLaricobiusNigrinus::CLaricobiusNigrinus(CHost* pHost, CTRef creationDate, double age, TSex sex, bool bFertil, size_t generation, double scaleFactor) :
		CIndividual(pHost, creationDate, age, sex, bFertil, generation, scaleFactor)
	{
		//reset creation date
		int year = creationDate.GetYear();
		m_creationDate = GetCreationDate(year);
		m_adult_emergence = GetAdultEmergence(year);

		m_adult_longevity = Equations().GetAdultLongevity(m_sex);
		m_F = (m_sex == FEMALE) ? Equations().GetFecondity(m_adult_longevity) : 0;
	}

	CTRef CLaricobiusNigrinus::GetCreationDate(int year)const
	{
		CTRef creationDate;
		double creationCDD = Equations().GetCreationCDD();

		const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		CTRef begin = CTRef(year, JANUARY, DAY_01);
		CTRef end = CTRef(year, JUNE, DAY_30);

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !creationDate.IsInit(); TRef++)
		{
			const CWeatherDay& wDay = weather_station.GetDay(TRef);
			double DD = GetStand()->m_DD.GetDD(wDay);
			CDD += DD;
			if (CDD >= creationCDD)
			{
				creationDate = wDay.GetTRef();
			}
		}

		ASSERT(creationDate.IsInit());

		return creationDate;
	}

	CTRef CLaricobiusNigrinus::GetAdultEmergence(int year)const
	{
		const CWeatherStation& weather_station = GetStand()->GetModel()->m_weather;
		CTPeriod p = weather_station.GetEntireTPeriod(CTM::DAILY);

		CTRef adult_emergence;
		double adult_emerging_CDD = Equations().GetAdultEmergingCDD();

		CTRef begin = GetStand()->m_diapause_end;
		CTRef end = p.End();
		if(weather_station[year].HaveNext())
			end = min(p.End(), CTRef(begin.GetYear() + 1, JUNE, DAY_30));

		double CDD = 0;
		for (CTRef TRef = begin; TRef <= end && !adult_emergence.IsInit(); TRef++)
		{
			const CWeatherDay& wday = weather_station.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];
			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);

			double DD = max(0.0, T - Equations().m_EAS[Τᴴ]);
			CDD += DD;
			if (CDD >= adult_emerging_CDD)
			{
				adult_emergence = wday.GetTRef();
			}
		}

		return adult_emergence;
	}

	CLaricobiusNigrinus& CLaricobiusNigrinus::operator=(const CLaricobiusNigrinus& in)
	{
		if (&in != this)
		{
			CIndividual::operator=(in);

			m_adult_emergence = in.m_adult_emergence;
			m_adult_longevity = in.m_adult_longevity;// Equations().GetAdultLongevity(m_sex) / 2;
			m_F = in.m_F;
		}

		return *this;
	}

	//destructor
	CLaricobiusNigrinus::~CLaricobiusNigrinus(void)
	{}

	double CLaricobiusNigrinus::AdjustTLab(const string& name, size_t s, CTRef TRef, double T)
	{
		if (name == "BlacksburgLab")
		{
			if (s == -1)
			{
				if (TRef.GetJDay() < CTRef(0, MARCH, DAY_25).GetJDay())
					s = EGG;
				else if (TRef.GetJDay() < CTRef(0, APRIL, DAY_15).GetJDay())
					s = LARVAE;
				else if (TRef.GetJDay() < CTRef(0, MAY, DAY_25).GetJDay())
					s = PUPAE;
				else
					s = AESTIVAL_DIAPAUSE_ADULT;
			}

			//if we are in Blacksburg lab situation, take temperature depend of year
			//lab rearing: change temperature Lamb(2005) thesis
			if (s == LARVAE)
				T = 13;
			else if (s == PREPUPAE || s == PUPAE)
				T = 15;
			else if (s == AESTIVAL_DIAPAUSE_ADULT && TRef.GetYear() == 2003)
				T = 15;
			else if (s == AESTIVAL_DIAPAUSE_ADULT && TRef.GetYear() == 2004)
			{
				if (TRef < CTRef(2004, SEPTEMBER, DAY_27))
					T = 19;
				else
					T = 13;
			}
		}

		return T;
	}

	double CLaricobiusNigrinus::AdjustDLLab(const string& name, size_t s, CTRef TRef, double day_length)
	{
		/*if (name == "VictoriaLab")
		{
			day_length = 12;
		}
		else */
		if (name == "BlacksburgLab")
		{
			//if we are in Blacksburg lab situation, take 12 hours
			if (TRef.GetYear() == 2003)
				day_length = 14;
			else if (TRef < CTRef(2004, SEPTEMBER, DAY_27))
				day_length = 16;
			else
				day_length = 10;
		}

		return day_length;
	}

	void CLaricobiusNigrinus::OnNewDay(const CWeatherDay& weather)
	{
		CIndividual::OnNewDay(weather);

		if (weather.GetTRef() == m_creationDate)
		{
			m_age = EGG;
		}
	}

	//*****************************************************************************
	// Develops all stages for one time step
	// Input:	weather: weather of the hour
	//			timeStep: timeStep [h]
	//*****************************************************************************
	void CLaricobiusNigrinus::Live(const CHourlyData& weather, size_t timeStep)
	{
		assert(IsAlive());
		assert(m_status == HEALTHY);

		CLNFHost* pHost = GetHost();
		CLNFStand* pStand = GetStand();

		double nb_steps = (24.0 / timeStep);
		size_t h = weather.GetTRef().GetHour();
		size_t s = GetStage();

		double T = weather[H_TAIR];
		T = AdjustTLab(weather.GetWeatherStation()->m_name, s, weather.GetTRef(), T);

		double day_length = weather.GetLocation().GetDayLength(weather.GetTRef()) / 3600.0;//[h]
		day_length = AdjustDLLab(weather.GetWeatherStation()->m_name, s, weather.GetTRef(), day_length);

		if (s < AESTIVAL_DIAPAUSE_ADULT)
		{
			//Time step development rate
			double r = Equations().GetRate(s, T) / nb_steps;

			double corr_r = (s == EGG || s == LARVAE) ? Equations().m_RDR[s][0] : 1;

			//Relative development rate for this individual
			//double rr = (s == EGG || s == LARVAE) ? m_RDR[s] : 1;

			//Time step development rate for this individual
			r *= corr_r;
			ASSERT(r >= 0 && r < 1);

			//Adjust age
			m_age += r;

			if (!m_dropToGroundDate.IsInit() && m_age > LARVAE + 0.9)//drop to the soil when 90% competed (guess)
				m_dropToGroundDate = weather.GetTRef().as(CTM::DAILY);
		}
		else if (s == AESTIVAL_DIAPAUSE_ADULT)
		{
			CTRef TRef = weather.GetTRef().as(CTM::DAILY);
			if (TRef == m_adult_emergence)
				m_age = ACTIVE_ADULT;
		}
		else//ACTIVE_ADULT
		{
			double r = (1.0 / m_adult_longevity) / nb_steps;
			ASSERT(r >= 0 && r < 1);

			m_age += r;
		}
	}

	//*****************************************************************************
	// Develops all stages, including adults
	// Input:	weather: the weather of the day
	//*****************************************************************************
	void CLaricobiusNigrinus::Live(const CWeatherDay& weather)
	{
		CIndividual::Live(weather);

		ASSERT(IsCreated(weather.GetTRef()));

		if (!IsCreated(weather.GetTRef()))
			return;

		size_t nbSteps = GetTimeStep().NbSteps();
		for (size_t step = 0; step < nbSteps&&m_age < DEAD_ADULT; step++)
		{
			size_t h = step * GetTimeStep();
			Live(weather[h], GetTimeStep());
		}

		if (weather.GetTRef() == m_creationDate || HasChangedStage())
			m_reachDate[GetStage()] = weather.GetTRef();
	}

	void CLaricobiusNigrinus::Brood(const CWeatherDay& weather)
	{
		assert(IsAlive() && m_sex == FEMALE);

		if (GetStage() == ACTIVE_ADULT)
		{
			//no brood process done

			//brooding
			//m_broods = m_F;
			//m_totalBroods = m_F;
			//m_F = 0;
		}
	}

	// kills by old age and frost
	// Output:  Individual's state is updated to follow update
	void CLaricobiusNigrinus::Die(const CWeatherDay& weather)
	{
		//attrition mortality. Killed at the end of time step
		if (GetStage() == DEAD_ADULT)
		{
			//Old age
			m_status = DEAD;
			m_death = OLD_AGE;
		}
		else
		{
			size_t s = GetStage();

			//Preliminary assessment of the cold tolerance of Laricobius nigrinus, a winter - active predator of the hemlock woolly adelgid from western canada
			//Leland M.Humble
			static const double COLD_TOLERENCE_T[NB_STAGES] = { -27.5,-22.1, -99.0,-99.0,-19.0,-19.0 };
			//Toland:L. nigrinus was -13.6 oC (± 0.5) with temperatures that ranged from -6 oC to -21 oC.
			if (weather[H_TMIN][MEAN] < COLD_TOLERENCE_T[s])
			{
				m_status = DEAD;
				m_death = FROZEN;
			}
		}

	}

	//*****************************************************************************
	// GetStat gather information of this object
	//
	// Input: stat: the statistic object
	// Output: The stat is modified
	//*****************************************************************************
	void CLaricobiusNigrinus::GetStat(CTRef d, CModelStat& stat)
	{
		if (IsCreated(d))
		{
			size_t s = GetStage();
			ASSERT(s <= DEAD_ADULT);

			if (IsAlive() || (s == DEAD_ADULT))
				stat[S_EGG + s] += m_scaleFactor;

			if (m_status == DEAD && m_death == FROZEN)
				stat[S_DEAD_FROST] += m_scaleFactor;

			if (HasChangedStage())
				stat[S_M_EGG + s] += m_scaleFactor;

			//if (s == ACTIVE_ADULT)
			//{
			//	stat[S_ADULT_ABUNDANCE] += m_scaleFactor * m_adult_abundance;
			//}
		}
	}

	void CLaricobiusNigrinus::Pack(const CIndividualPtr& pBug)
	{
		assert(m_sex == pBug->GetSex());

		CLaricobiusNigrinus* in = (CLaricobiusNigrinus*)(pBug.get());
		CIndividual::Pack(pBug);
	}

	double CLaricobiusNigrinus::GetInstar(bool includeLast)const
	{
		return (IsAlive() || m_death == OLD_AGE) ? GetStage() : CBioSIMModelBase::VMISS;
	}

	//*********************************************************************************************************************

	//*********************************************************************************
	//CLNFHost

	CLNFHost::CLNFHost(CStand* pStand) :
		CHost(pStand)
	{
	}

	void CLNFHost::Live(const CWeatherDay& weather)
	{
		CHost::Live(weather);
	}

	void CLNFHost::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CHost::GetStat(d, stat, generation);
	}

	//*************************************************
	//CLNFStand

	void CLNFStand::init(int year, const CWeatherYears& weather)
	{
		m_diapause_end = ComputeDiapauseEnd(weather[year]);
	}

	CTRef CLNFStand::ComputeDiapauseEnd(const CWeatherYear& weather)const
	{
		CTPeriod p = weather.GetEntireTPeriod(CTM::DAILY);

		double sumDD = 0;

		for (size_t ii = (m_equations.m_ADE[ʎ0] - 1); ii <= (m_equations.m_ADE[ʎ1] - 1); ii++)
		{
			CTRef TRef = p.Begin() + ii;
			const CWeatherDay& wday = weather.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];

			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);
			double DD = min(0.0, T - m_equations.m_ADE[ʎb]);//DD is negative
			sumDD += DD;
		}

		boost::math::logistic_distribution<double> begin_dist(m_equations.m_ADE[ʎ2], m_equations.m_ADE[ʎ3]);
		int begin = (int)Round((m_equations.m_ADE[ʎ1] - 1) + m_equations.m_ADE[ʎa] * cdf(begin_dist, sumDD), 0);

		return p.Begin() + begin;
	}

	void CLNFStand::GetStat(CTRef d, CModelStat& stat, size_t generation)
	{
		CStand::GetStat(d, stat, generation);

		const CWeatherStation& weather_station = GetModel()->m_weather;
		const CWeatherDay& wday = weather_station.GetDay(d);

		//use year of diapause to compute correctly the adult emergence cdd
		int year = m_diapause_end.GetYear();
		CTRef begin = CTRef(year, JANUARY, DAY_01);
		CTRef end = CTRef(year, DECEMBER, DAY_31);

		if (d >= begin && d<= end)
		{
			//Egg creation DD (allen 1976)
			m_egg_creation_CDD += m_DD.GetDD(wday);
			stat[S_EGG_CREATION_CDD] = m_egg_creation_CDD;

			//diapause end negative DD
			double T = wday[H_TNTX][MEAN];
			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);
			//T = max(m_equations.m_ADE[ʎa], T);
			double NDD = min(0.0, T - m_equations.m_ADE[ʎb]);//DD is negative

			int ii = d-begin;
			//if( ii>=(172-1) && ii<=int(m_equations.m_ADE[ʎ0]-1))
			if (ii >= int(m_equations.m_ADE[ʎ0] - 1) && ii <= int(m_equations.m_ADE[ʎ1] - 1))
				m_diapause_end_NCDD += NDD;

			stat[S_DIAPAUSE_END_NCDD] = m_diapause_end_NCDD;
		}

		begin = m_diapause_end;
		end = CTRef(m_diapause_end.GetYear()+1, JANUARY, DAY_31);
		if (d >= begin && d <= end)
		{
			//adult emergence (growing DD)
			double T = wday[H_TNTX][MEAN];
			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);

			double GDD = max(0.0, T - m_equations.m_EAS[Τᴴ]);
			m_adult_emergence_CDD += GDD;
			stat[S_ADULT_EMERGENCE_CDD] = m_adult_emergence_CDD;
		}
	}

}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2006, 2008 Ferdinando Ametrano
 Copyright (C) 2010 Andre Miemiec

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

/*! \file irregularswap.hpp
    \brief Irregular fixed-rate vs Libor swap
*/

#ifndef quantlib_irregular_swap_hpp
#define quantlib_irregular_swap_hpp

#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/instruments/swap.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/schedule.hpp>
#include <boost/optional.hpp>

namespace QuantLib {

    class IborIndex;

    //! Irregular swap: fixed vs floating leg
    class IrregularSwap : public Swap {
      public:
        enum Type { Receiver = -1, Payer = 1 };
        class arguments;
        class results;
        class engine;
        IrregularSwap(
            Type type,
            const Leg& fixLeg,
            const Leg& floatLeg);
        //! \name Inspectors
        //@{
        Type type() const;

        const Leg& fixedLeg() const;
        const Leg& floatingLeg() const;
        //@}

        //! \name Results
        //@{
        Real fixedLegBPS() const;
        Real fixedLegNPV() const;
        Rate fairRate() const;

        Real floatingLegBPS() const;
        Real floatingLegNPV() const;
        Spread fairSpread() const;
        //@}
        // other
        void setupArguments(PricingEngine::arguments* args) const override;
        void fetchResults(const PricingEngine::results*) const override;

      private:
        void setupExpired() const override;
        Type type_;

        // results
        mutable Rate fairRate_;
        mutable Spread fairSpread_;
    };

    //! %Arguments for irregular-swap calculation
    class IrregularSwap::arguments : public Swap::arguments {
      public:
        arguments() : type(Receiver){}
        Type type;

        std::vector<Date> fixedResetDates;
        std::vector<Date> fixedPayDates;
        std::vector<Real> fixedCoupons;
        std::vector<Real> fixedNominals;

        std::vector<Date> floatingResetDates;
        std::vector<Date> floatingFixingDates;
        std::vector<Date> floatingPayDates;
        std::vector<Time> floatingAccrualTimes;
        std::vector<Real> floatingNominals;
        std::vector<Spread> floatingSpreads;
        std::vector<Real> floatingCoupons;

        void validate() const override;
    };

    //! %Results from irregular-swap calculation
    class IrregularSwap::results : public Swap::results {
      public:
        Rate fairRate;
        Spread fairSpread;
        void reset() override;
    };

    class IrregularSwap::engine : public GenericEngine<IrregularSwap::arguments,
                                                       IrregularSwap::results> {};

    // inline definitions

    inline IrregularSwap::Type IrregularSwap::type() const {
        return type_;
    }

    inline const Leg& IrregularSwap::fixedLeg() const {
        return legs_[0];
    }

    inline const Leg& IrregularSwap::floatingLeg() const {
        return legs_[1];
    }

    std::ostream& operator<<(std::ostream& out,
                             IrregularSwap::Type t);

}

#endif


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

#ifndef BOOST_MATH_INTERPOLATORS_CUBIC_HERMITE_HPP
#define BOOST_MATH_INTERPOLATORS_CUBIC_HERMITE_HPP
#include <memory>
#include <boost/math/interpolators/detail/cubic_hermite_detail.hpp>

namespace boost::math::interpolators {

template<class RandomAccessContainer>
class cubic_hermite {
public:
    using Real = typename RandomAccessContainer::value_type;

    cubic_hermite(RandomAccessContainer && x, RandomAccessContainer && y, RandomAccessContainer && dydx)
    : impl_(std::make_shared<detail::cubic_hermite_detail<RandomAccessContainer>>(std::move(x), std::move(y), std::move(dydx)))
    {}

    Real operator()(Real x) const {
        return impl_->operator()(x);
    }

    Real prime(Real x) const {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const cubic_hermite & m)
    {
        os << *m.impl_;
        return os;
    }

    void push_back(Real x, Real y, Real dydx) {
        impl_->push_back(x, y, dydx);
    }

private:
    std::shared_ptr<detail::cubic_hermite_detail<RandomAccessContainer>> impl_;
};

}
#endif


// Std
#include <algorithm>
#include <deque>
#include <unordered_map>
// Boost
#include <boost/graph/graphviz.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/visitors.hpp>
// Our stuff
#include "chemgraph.hpp"
#include "ctab.hpp"
#include "log.hpp"
#include "gauss.hpp"

using namespace std;
using namespace boost;

ChemGraph toGraph(CTab& tab)
{
    ChemGraph graph;
    //add_vertex
    for_each(tab.atoms.begin(), tab.atoms.end(), [&graph](const AtomEntry& a)
    {
        add_vertex(AtomVertex(a.code), graph);
    });
    auto vrange = vertices(graph);
    //add edges
    for_each(tab.bounds.begin(), tab.bounds.end(), [&graph, &vrange](const BoundEntry& b)
    {
        add_edge(vrange.first[b.a1] - 1, vrange.first[b.a2] - 1, Bound(b.type), graph);
    });
    return graph;
}

int getValence(ChemGraph& graph, ChemGraph::vertex_descriptor vertex)
{
    auto edges = out_edges(vertex, graph);
    int valence = 0;
    for (auto p = edges.first; p != edges.second; p++)
    {
        valence += graph[*p].type;
    }
    return valence;
}

ChemGraph& addHydrogen(ChemGraph& graph)
{
    std::unordered_map<int, int> valences;
    valences.insert(make_pair(C.code(), 4));
    valences.insert(make_pair(N.code(), 3));
    valences.insert(make_pair(O.code(), 2));
    auto vtx = vertices(graph);
    for(auto p = vtx.first; p != vtx.second; p++)
    {
        //Note: no extra hydrogens for negative ions
        if(valences.find(graph[*p].code.code()) != valences.end()
            && graph[*p].code.charge() >= 0)
        {
            int normal_valence = valences[graph[*p].code.code()];
            // FIXME: counts 1.5 as 4 but that is "works for me"
            int cur_val = getValence(graph, *p);
            // fit with hydrogens if not enough valence
            for(int i=cur_val; i<normal_valence; i++)
            {
                size_t v = add_vertex(AtomVertex(H), graph);
                add_edge(*p, v, Bound(1), graph);
            }
        }
    }
    return graph;
}

class CodeWriter {
public:
    CodeWriter(ChemGraph& graph):g(graph){}
    template <class VertexOrEdge>
    void operator()(ostream& out, const VertexOrEdge& v) const {
      out << "[label=\"" << g[v].code.symbol() << " [" << v << "]" << "\"]";
    }
private:
    ChemGraph& g;
};

void dumpGraph(ChemGraph& graph, ostream& out)
{
    write_graphviz(out, graph, CodeWriter(graph));
}

ostream& operator<<(ostream& stream, const Cycle& cycle)
{
    for (auto & e : cycle.edges)
    {
        stream << e.first << "--" << e.second << '\n';
    }
    return stream;
}

inline set<pair<size_t, size_t>> chainToEdgeSet(const vector<size_t>& v){
    assert(v.size() > 1);
    set<pair<size_t,size_t>> out;
    for(size_t i=1; i<v.size(); i++){
        if(v[i-1] < v[i])
            out.insert(make_pair(v[i-1], v[i]));
        else
            out.insert(make_pair(v[i], v[i-1]));
    }
    if(v.front() < v.back())
        out.insert(make_pair(v.front(), v.back()));
    else
        out.insert(make_pair(v.back(), v.front()));
    return out;
}

void logCycle(vector<pair<vd,vd>>& c)
{
    for (auto & e : c)
    {
        LOG(DEBUG) << e.first << "--" << e.second << '\n';
    }
    LOG(DEBUG) << endline;
}

Cycle::Cycle(vector<pair<vd, vd>> edges_):edges(edges_)
{
    chain = cycleToChain(edges, [](const pair<vd,vd>&p){ return p; });
}

Cycle::Cycle(vector<vd> chain_):chain(chain_)
{
    auto eset = chainToEdgeSet(chain_);
    edges.resize(eset.size());
    copy(eset.begin(), eset.end(), edges.begin());
}

bool Cycle::intersects(const Cycle& that)const
{
   return intersection(that).size() > 0; // can be optimized futher
}

vector<pair<vd,vd>> Cycle::intersection(const Cycle& c)const
{
    vector<pair<vd,vd>> ret;
    auto& c1 = edges;
    auto& c2 = c.edges;
    set_intersection(c1.begin(), c1.end(), c2.begin(), c2.end(),
        back_inserter(ret));
    return ret;
}

Cycle& Cycle::markAromatic(ChemGraph& g)
{
    LOG(TRACE) << "CHAIN: ";
    for (int k : chain)
        LOG(TRACE) << k << " - ";
    LOG(TRACE) << endline;
    int piEl = 0;
    for_each(chain.begin(), chain.end(), [&g, &piEl](int n){
        LOG(TRACE) << "Atom # " << n << " " << g[n].code.symbol() << " pi E = " << g[n].piE << endline;
        if (g[n].piE > 0)
            piEl += g[n].piE;
        //TODO: add debug trace for < 0
    });
    aromatic_ = ((piEl - 2) % 4 == 0); // Hukkel rule 4n + 2
    LOG(TRACE) << "PI E " << piEl << " aromatic? : " << aromatic_ << endline;
    //assign cyclic-only DC
    if (aromatic_)
    {
        for (auto n : chain)
        {
            g[n].inAromaCycle = true;
        }
        for (auto e : edges)
        {
            auto ed = edge(e.first, e.second, g);
            g[ed.first].type = AROMATIC;
        }
    }
    return *this;
}

struct BfsNoop{
    void onVertex(size_t v){}
};

// assuming constant edge weight - no priority queue required
template<class Policy=BfsNoop>
struct ShortestPaths : Policy{
public:
    using G = ChemGraph;

    ShortestPaths(G& graph, size_t start, const vector<bool>& m=vector<bool>()):
    g(graph), visited(num_vertices(graph)), edgeTo(num_vertices(graph)), s(start), mask(m){
        queue.push_back(start);
        visited[start] = true;
        edgeTo[start] = (size_t)1<<(sizeof(size_t)*8-1);
        bfs();
    }
    // apply functor to each node along the shortest path from v to starting point
    // except the starting point itself
    template<class Fn>
    void apply(size_t v, Fn&& fn, bool includeStart=false){
        if(!visited[v]) // cut apply for unexplored vertices
            return;
        size_t w = v;
        while(w != s){
            fn(w);
            w = edgeTo[w];
        }
        if(includeStart)
            fn(s);
    }
    // get shortest path to s
    vector<size_t> path(size_t v, bool includeStart=false){
        vector<size_t> vec;
        apply(v, [&vec](size_t w){ vec.push_back(w); }, includeStart);
        return vec;
    }

    // get prior vertex on path to this one
    size_t prev(size_t p){
        return edgeTo[p];
    }
private:
    void bfs(){
        while(!queue.empty()){
            size_t v = queue.front();
            queue.pop_front();
            // push all not visited
            auto adj = adjacent_vertices(v, g);
            for(auto p = adj.first; p != adj.second; p++ ){
                auto w = *p;
                if(!visited[w] && (mask.empty() || mask[w])){
                    edgeTo[w] = v;
                    visited[w] = true;
                    queue.push_back(w);
                }
            }
        }
    }
    G& g;
    vector<bool> visited;
    vector<size_t> edgeTo;
    deque<size_t> queue;
    size_t s;
    const vector<bool>& mask;

};

ShortestPaths<> shortestPaths(ChemGraph& g, size_t start, const vector<bool>& mask){
    return ShortestPaths<>(g, start, mask);
}

// depth-first search to detect all cycles
// ploicy-based design, final processing is deffered to the inherited policy
template<class Policy>
struct CycleDetect : Policy{
public:
    using G = ChemGraph;
    CycleDetect(G& graph, size_t start):Policy(), g(graph), visited(num_vertices(graph)){
        dfs(start);
    }

private:
    void dfs(size_t v){
        visited[v] = true;
        auto prev = path.size() ? path.back() : -1;
        path.push_back(v);
        auto adj = adjacent_vertices(v, g);
        for(auto p = adj.first; p != adj.second; p++ ){
            auto w = *p;
            auto e = edge(v, w, g);
            // TODO: turn to predicate or just use Boost DFS
            if(g[e.first].type >= STEREO)
                continue;
            if(!visited[w]){
                dfs(w); //pushes w on path
                path.pop_back();
            }
            else if(w != prev){ // visited and not previous one
                auto i = find(path.begin(), path.end(), w);
                if(i != path.end()) //cycle
                    Policy::onCycle(i, path.end());
            }
        }
    }
    G& g;
    vector<bool> visited;
    vector<int> path;
};

struct FetchCycles{
    vector<vector<size_t>> cycles;
    template<class I>
    void onCycle(I beg, I end){
        cycles.emplace_back(beg, end);
    }
};

//some cycle basis not even Horton's cycle basis
vector<vector<size_t>> cycleBasis(ChemGraph& g, size_t start=0){
    return CycleDetect<FetchCycles>(g, start).cycles;
}

vector<Cycle> minimalCycleBasis(ChemGraph& g){
    vector<vector<size_t>> isolatedCycles; //
    vector<vector<size_t>> basisCandidates; // that are not isolated
    auto const V = num_vertices(g);
    vector<vd> inCycle(V); // >0 if v is on some cycle
    vector<bool> inCycleSystem(V); // if v is in some cycle system
    LOG(DEBUG)<<"IN CYCLE: "<<inCycle<<endline;
    // find isolated cycles and enumerate vertices belonging to some cycle
    {
        vector<vector<size_t>> someCycles = cycleBasis(g);
        LOG(DEBUG) << "G SIZE:" << V << endline << "CYCLES ARE:" << endline;
        for(auto &c : someCycles)
            LOG(DEBUG) << c << endline;
        // count number of times a cycles passes through a vertex
        for(auto& c : someCycles){
            for(size_t v : c){
                inCycle[v]++;
                inCycleSystem[v] = 1; // consider every as non-isoalted
            }
        }

        // test each cycle
        for(auto& c : someCycles){
            int total = accumulate(c.begin(), c.end(), 0,
                [&inCycle](int sum, size_t v){
                return sum + inCycle[v];
            });
            // no other cycles passing though isolated cycle's vertices
            // therefore sum == length of cycle
            if(total == (int)c.size()){
                isolatedCycles.push_back(c);
                for(auto v : c) // filter out isolated cycles from cycle systems
                    inCycleSystem[v] = 0;
            }

        }
    }
    LOG(DEBUG)<<"IN CYCLE: "<<inCycle<<endline;

    // from now on consider only vertices from some cycles
    // find connection points - vertices with > 2 adjacent
    vector<size_t> connPts;
    for(size_t v=0; v<num_vertices(g); v++){
        if(inCycle[v] <= 1)
            continue; //vertex in an isolated cycle
        auto adj = adjacent_vertices(v, g);
        size_t neib = count_if(adj.first, adj.second,
            [&inCycle](vd a){
                return inCycle[a] > 0;
        });
        if(neib > 2){
            connPts.push_back(v);
        }
    }
    LOG(DEBUG) << "Conn pts.:" << connPts <<endline;
    // Modified Horton's algorithm (1987)
    // Each cycle in minimal cycle base
    // has form : Puw + Pvw + {u, v} (where Pxy is shortest path from x -> y)
    // Puw & Pvw = {w} (the paths have no common edges)
    // Find candidates using these principles

    // Combined with knowledge of connection points, all points being
    // on some cycle & no isolated cycles we can examine only connection points
    // for shortest-path trees
    {
        for(size_t con : connPts){
            auto spf = shortestPaths(g, con, inCycleSystem);
            auto eds = edges(g);
            for(auto ep = eds.first ; ep != eds.second; ep++){
                auto a = target(*ep, g);
                auto b = source(*ep, g);
                if(!inCycleSystem[a] || !inCycleSystem[b])
                    continue;
                // drop these along the paths
                if(spf.prev(a) == b || spf.prev(b) == a)
                    continue;
                auto pa = spf.path(a);
                auto pb = spf.path(b);
                LOG(DEBUG) << "Two shortest paths from " << con << ": A "<< a
                    << "  B " << b << endline;
                LOG(DEBUG) << pa << endline;
                LOG(DEBUG) << pb << endline;
                // both paths w/o starting point 'con'
                size_t missingLink = con;
                if(pa.empty() || pb.empty())
                    continue;
                // if have common suffix - drop
                // they must pass through some connection point
                // that we are going to process anyway
                if(pa.back() == pb.back())
                    continue;
                // add missing link
                pa.push_back(missingLink);
                // and follow 2nd path
                for_each(pb.rbegin(), pb.rend(), [&pa](size_t v){
                    pa.push_back(v);
                });
                basisCandidates.push_back(pa);
            }
        }
    }
    // now perform Gaussian ellimination of candidate cycles
    sort(basisCandidates.begin(), basisCandidates.end(),
        [](const vector<size_t>& v1, const vector<size_t>& v2){
            return v1.size() < v2.size();
    });
    vector<set<pair<size_t,size_t>>> basis; // basis accumulated as sets of edges
    vector<size_t> basisIdx; // indices of these candidates that are in final basis
    for(size_t i=0; i<basisCandidates.size(); i++){
        auto s = chainToEdgeSet(basisCandidates[i]);
        if(!elimination(s, basis)){
            basis.push_back(s);
            basisIdx.push_back(i);
        }
    }
    LOG(DEBUG) << "Isolated cycles:\n";
    for(auto & c : isolatedCycles){
        LOG(DEBUG) << c << endline;
    }
    LOG(DEBUG)<<"Cycle base candidates:\n";
    for(auto & c : basisCandidates){
        LOG(DEBUG) << c << endline;
    }
    using std::move;
    vector<Cycle> results;
    copy(isolatedCycles.begin(), isolatedCycles.end(), back_inserter(results));
    for(size_t i : basisIdx){
        results.push_back(Cycle{basisCandidates[i]});
    }
    LOG(DEBUG)<<"Minimal cycle base :\n";
    for(auto& v : results){
        LOG(DEBUG) << v << endline;
    }
    return results;
}