#include "pch.h"
#include "mechanics.h"
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
#include <sstream>
#include <math.h>

template <typename T> int sgn(T val) {
	return (T(0) < val) - (val < T(0));
}

class Suspension
{

	// variables
private:
	// suffix ref stands for ref, so reference/initial coordinates can be easily
	// differentiated from coordinates of the same points during wheel movement
	// hardpoints
	// LCA
	Eigen::Vector3f lca1ref;
	Eigen::Vector3f lca2ref;
	Eigen::Vector3f lca3ref;

	Eigen::Vector3f uca1ref;
	Eigen::Vector3f uca2ref;
	Eigen::Vector3f uca3ref;

	Eigen::Vector3f tr1ref;
	Eigen::Vector3f tr2ref;

	Eigen::Vector3f wcnref;
	Eigen::Vector3f spnref;

	float wRadius;     // wheel radius
	float wVert;       // wheel vertical movement
	float wSteer;      // wheel steering movement
	int vertIncr;      // number of increments between reference position and upmost and downmost
	int steerIncr;     // number of increments between reference position and leftmost and rightmost
	float precision;   // precision- at what value has the iterator converged, in percentage- 0...1

	float wheelbase;
	float cogHeight;

	float driveBias;
	float brakeBias;

	int suspPos;      // front or rear suspension 0 for front, 1 for rear
	int drivePos;     // outboard or inboard drive 0 for outboard, 1 for inboard
	int brakePos;     // outboard or inboard brakes 0 for outboard, 1 for inboard

	// derived values

	Eigen::Vector3f lca12;
	Eigen::Vector3f uca12;
	Eigen::MatrixXf lca3Glob;// (vertIncr * 2 + 1, 3);
	Eigen::MatrixXf uca3Glob;// (vertIncr * 2 + 1, 3);
	Eigen::MatrixXf tr1Glob;// (vertIncr * 2 + 1, 3);
	Eigen::MatrixXf tr2Glob;// (vertIncr * 2 + 1, 3);
	Eigen::MatrixXf wcnGlob;// (vertIncr * 2 + 1, 3);
	Eigen::MatrixXf spnGlob;// (vertIncr * 2 + 1, 3);
	Eigen::MatrixXf cpGlob;// (vertIncr * 2 + 1, 3);

public:

	Suspension(
		float* hps,
		float wRadiusin,
		float wheelbasein, float cogHeightin, float driveBiasin,
		float brakeBiasin, int suspPosin, int drivePosin, int brakePosin,
		float wVertin, float wSteerin,
		int vertIncrin, int steerIncrin, float precisionin) :
		lca3Glob(vertIncrin * 2 + 1, 3), uca3Glob(vertIncrin * 2 + 1, 3),
		tr1Glob(steerIncrin * 2 + 1, 3),
		tr2Glob((vertIncrin * 2 + 1) * (steerIncrin * 2 + 1), 3), wcnGlob((vertIncrin * 2 + 1) * (steerIncrin * 2 + 1), 3),
		spnGlob((vertIncrin * 2 + 1) * (steerIncrin * 2 + 1), 3), cpGlob((vertIncrin * 2 + 1) * (steerIncrin * 2 + 1), 3)
	{
		lca1ref << hps[0], hps[1], hps[2];
		lca2ref << hps[3], hps[4], hps[5];
		lca3ref << hps[6], hps[7], hps[8];

		uca1ref << hps[9], hps[10], hps[11];
		uca2ref << hps[12], hps[13], hps[14];
		uca3ref << hps[15], hps[16], hps[17];

		tr1ref << hps[18], hps[19], hps[20];
		tr2ref << hps[21], hps[22], hps[23];

		wcnref << hps[24], hps[25], hps[26];
		spnref << hps[27], hps[28], hps[29];

		wRadius = wRadiusin;

		wheelbase = wheelbasein;
		cogHeight = cogHeightin;
		driveBias = driveBiasin;

		brakeBias = brakeBiasin;

		suspPos = suspPosin;
		drivePos = drivePosin;
		brakePos = brakePosin;

		wVert = wVertin;
		wSteer = wSteerin;
		vertIncr = vertIncrin;
		steerIncr = steerIncrin;
		precision = precisionin;

	}

	// FUNCTIONS
private:

	// place as inputs variables rLCA, rUCA, uca12, lca12, etc. so those can be only temporary
	// values and not use up stack memory

	void CalculateConstants(
		Eigen::Matrix3f& _rotLCA, Eigen::Matrix3f& _rotUCA,
		float& _rLCA, float& _rUCA, float& _rCA,
		Eigen::ArrayXf& _zLocLca,
		float& _rST, float& _t_param, float& _rTR,
		Eigen::Vector3f& _wcnlocTRk, Eigen::Vector3f& _spnlocTRk)
	{

		// create line connecting lca1 and lca2
		Eigen::ParametrizedLine<float, 3> lca1lca2 = Eigen::ParametrizedLine<float, 3>::Through(lca1ref, lca2ref);

		// local LCA plane for determining max z value of wheel parameters
		Eigen::Vector4f abcd;
		Eigen::Vector3f _tr2prref;

		// z local maximum value
		float zLocHi;
		// z local minimum value
		float zLocLo;

		lca12 = lca1lca2.projection(lca3ref);
		_rLCA = lca1lca2.distance(lca3ref);

		// create rotation matrix for LCA cs
		_rotLCA <<
			(lca1ref - lca2ref).normalized(),
			(lca12 - lca3ref).normalized(),
			((lca1ref - lca2ref).cross(lca12 - lca3ref)).normalized();

		// calculate parameters for plane in LCA cs ax+by+cz+d=0
		abcd <<
			_rotLCA.row(2)(0),
			_rotLCA.row(2)(1),
			_rotLCA.row(2)(2),
			-_rotLCA.row(2) * (_rotLCA.transpose() * Eigen::Vector3f{ -lca12(0),-lca12(1),lca3ref(2) - wVert - lca12(2) });
		// calculates z value for upmost movement of wheel for intersection of plane and circle in LCA
		zLocHi =
			(-abcd(2) * abcd(3) +
				abcd(1) * sqrt(abcd(2) * abcd(2) * _rLCA * _rLCA +
					abcd(1) * abcd(1) * _rLCA * _rLCA -
					abcd(3) * abcd(3))) /
			(abcd(1) * abcd(1) + abcd(2) * abcd(2));

		// reuses previous parameters for LCA plane, only 4th parameter is changed
		abcd(3) =
			-_rotLCA.row(2) *
			_rotLCA.transpose() *
			Eigen::Vector3f{ -lca12(0), -lca12(1), lca3ref(2) + wVert - lca12(2) };

		// calculates z value for downmost movement of wheel for intersection of plane and circle in LCA
		zLocLo =
			(-abcd(2) * abcd(3) +
				abcd(1) * sqrt(abcd(2) * abcd(2) * _rLCA * _rLCA +
					abcd(1) * abcd(1) * _rLCA * _rLCA -
					abcd(3) * abcd(3))) /
			(abcd(1) * abcd(1) + abcd(2) * abcd(2));

		// wheel travel from rebound to bump
		_zLocLca <<
			Eigen::VectorXf::LinSpaced(vertIncr, zLocLo, zLocLo / vertIncr),
			0,
			Eigen::VectorXf::LinSpaced(vertIncr, zLocHi / vertIncr, zLocHi);

		// create line connecting uca1 and uca2
		Eigen::ParametrizedLine<float, 3> uca1uca2 = Eigen::ParametrizedLine<float, 3>::Through(uca1ref, uca2ref);

		uca12 = uca1uca2.projection(uca3ref);
		_rUCA = uca1uca2.distance(uca3ref);

		_rCA = (uca3ref - lca3ref).norm();

		// create rotation matrix for UCA cs
		_rotUCA <<
			(uca1ref - uca2ref).normalized(),
			(uca12 - uca3ref).normalized(),
			((uca1ref - uca2ref).cross(uca12 - uca3ref)).normalized();

		// calculate TR2 projection point and rTR
		Eigen::ParametrizedLine<float, 3> lca3uca3 = Eigen::ParametrizedLine<float, 3>::Through(lca3ref, uca3ref);

		_tr2prref = lca3uca3.projection(tr2ref);

		_rST = (tr2ref - _tr2prref).norm();

		_rTR = (tr2ref - tr1ref).norm();

		_t_param = (_tr2prref(2) - lca3ref(2)) / (uca3ref(2) - lca3ref(2));

		Eigen::Matrix3f _rotTRk; // TR rotation matrix defined by TR2ref

		Eigen::Vector3f _xCol{ _tr2prref - tr2ref };
		Eigen::Vector3f _zCol{ lca3ref - uca3ref };
		Eigen::Vector3f _yCol{ _zCol.cross(_xCol) };

		_rotTRk.col(0) << _xCol / _xCol.norm();
		_rotTRk.col(1) << _yCol / _yCol.norm();
		_rotTRk.col(2) << _zCol / _zCol.norm();

		_wcnlocTRk << _rotTRk.transpose() * (wcnref - _tr2prref);
		_spnlocTRk << _rotTRk.transpose() * (spnref - _tr2prref);
	}

public:

	void CalculateMovement()
	{
		Eigen::Matrix3f rotLCA;
		Eigen::Matrix3f rotUCA;
		Eigen::MatrixXf tr2prGlob;
		float rLCA;
		float rUCA;
		float rCA;   // distance between LCA3 and UCA3
		float rST;   // distance between TR2 and UCA3LCA3 axis
		float rTR;   // distance between TR2 and TR1 axis
		float t_param;   // parameter to determine position of TR2pr on uca3lca3 line
		// coordinates of wcn and spn in TR cs defined by tr2ref
		Eigen::Vector3f wcnlocTRk;
		Eigen::Vector3f spnlocTRk;

		Eigen::ArrayXf zLCA3LocLCA(vertIncr * 2 + 1);

		Suspension::CalculateConstants(rotLCA, rotUCA, rLCA, rUCA, rCA, zLCA3LocLCA, rST, t_param, rTR, wcnlocTRk, spnlocTRk);

		Eigen::MatrixXf lca3LocLCA(vertIncr * 2 + 1, 3);

		// populating positions of local LCA3 in a matrix
		lca3LocLCA.col(0) << Eigen::VectorXf::Zero(vertIncr * 2 + 1);
		lca3LocLCA.col(1) << -(rLCA * rLCA - zLCA3LocLCA * zLCA3LocLCA).sqrt();
		lca3LocLCA.col(2) << zLCA3LocLCA;

		// global positions of LCA3 for whole wheel movement
		lca3Glob = (lca3LocLCA * rotLCA.transpose()).array().rowwise() + lca12.array().transpose();

		// global position of UCA3 for whole wheel movement
		Eigen::MatrixXf uca3LocUCA(vertIncr * 2 + 1, 3);
		Eigen::MatrixXf lca3LocUCA(vertIncr * 2 + 1, 3);

		lca3LocUCA = lca3Glob.rowwise() - uca12.transpose();
		lca3LocUCA = lca3LocUCA * rotUCA;

		// temporary values for calculating UCA3 in UCA cs, correspond to chunks of expression in word
		Eigen::ArrayXf temp1UCA3 =
			-rCA * rCA + rUCA * rUCA +
			lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() +
			lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp2UCA3 =
			2 * (lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
				lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array());

		Eigen::ArrayXf temp3UCA3 =
			-rCA * rCA * rCA * rCA + 2 * rCA * rCA * rUCA * rUCA +
			2 * rCA * rCA * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() +
			2 * rCA * rCA * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			2 * rCA * rCA * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp4UCA3 =
			-rUCA * rUCA * rUCA * rUCA -
			2 * rUCA * rUCA * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() +
			2 * rUCA * rUCA * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() +
			2 * rUCA * rUCA * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp5UCA3 =
			-lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() -
			2 * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() -
			2 * lca3LocUCA.col(0).array() * lca3LocUCA.col(0).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp6UCA3 =
			-2 * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array() -
			lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() * lca3LocUCA.col(1).array() -
			lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array() * lca3LocUCA.col(2).array();

		Eigen::ArrayXf temp7UCA3 = (temp3UCA3 + temp4UCA3 + temp5UCA3 + temp6UCA3).sqrt();

		uca3LocUCA.col(0) << Eigen::VectorXf::Zero(vertIncr * 2 + 1);
		uca3LocUCA.col(1) << (lca3LocUCA.col(1).array() * temp1UCA3 - lca3LocUCA.col(2).array() * temp7UCA3) / temp2UCA3;
		uca3LocUCA.col(2) << (lca3LocUCA.col(2).array() * temp1UCA3 + lca3LocUCA.col(1).array() * temp7UCA3) / temp2UCA3;

		uca3Glob = (uca3LocUCA * rotUCA.transpose()).array().rowwise() + uca12.array().transpose();

		// calculating TR2 positions

		tr1Glob.col(0) <<
			Eigen::VectorXf::LinSpaced(2 * steerIncr + 1, tr1ref(0), tr1ref(0));
		tr1Glob.col(1) <<
			Eigen::VectorXf::LinSpaced(steerIncr, tr1ref(1) - wSteer, tr1ref(1) - wSteer / steerIncr),
			tr1ref(1),
			Eigen::VectorXf::LinSpaced(steerIncr, tr1ref(1) + wSteer / steerIncr, tr1ref(1) + wSteer);
		tr1Glob.col(2) <<
			Eigen::VectorXf::LinSpaced(2 * steerIncr + 1, tr1ref(2), tr1ref(2));

		tr2prGlob = (uca3Glob - lca3Glob) * t_param + lca3Glob;

		for (int i = 0; i < vertIncr * 2 + 1; i++)
		{
			Eigen::Matrix3f rotTR;
			Eigen::Vector3f tr2locTR;

			Eigen::Vector3f xCol{ 30, 0, -30 * (uca3Glob(i, 0) - lca3Glob(i, 0)) / (uca3Glob(i, 2) - lca3Glob(i, 2)) };
			Eigen::Vector3f zCol{ (lca3Glob.row(i) - uca3Glob.row(i)).transpose() };
			Eigen::Vector3f yCol{ zCol.cross(xCol) };

			rotTR.col(0) << xCol / xCol.norm();
			rotTR.col(1) << yCol / yCol.norm();
			rotTR.col(2) << zCol / zCol.norm();
			for (int j = 0; j < steerIncr * 2 + 1; j++)
			{
				// calculate local position of TR1
				Eigen::Vector3f tr1locTR;

				tr1locTR = -tr2prGlob.row(i) + tr1Glob.row(j);
				tr1locTR = rotTR.transpose() * tr1locTR;

				// calculate local position of TR2
				float temp1TR2 =
					rST * rST - rTR * rTR +
					tr1locTR(0) * tr1locTR(0) +
					tr1locTR(1) * tr1locTR(1) +
					tr1locTR(2) * tr1locTR(2);

				float temp2TR2 = 2 * (tr1locTR(0) * tr1locTR(0) + tr1locTR(1) * tr1locTR(1));

				float temp3TR2 =
					-rST * rST * rST * rST + 2 * rTR * rTR * rST * rST +
					2 * rST * rST * tr1locTR(0) * tr1locTR(0) +
					2 * rST * rST * tr1locTR(1) * tr1locTR(1) -
					2 * rST * rST * tr1locTR(2) * tr1locTR(2);

				float temp4TR2 =
					-rTR * rTR * rTR * rTR +
					2 * rTR * rTR * tr1locTR(0) * tr1locTR(0) +
					2 * rTR * rTR * tr1locTR(1) * tr1locTR(1) +
					2 * rTR * rTR * tr1locTR(2) * tr1locTR(2);

				float temp5TR2 =
					-tr1locTR(0) * tr1locTR(0) * tr1locTR(0) * tr1locTR(0) -
					2 * tr1locTR(0) * tr1locTR(0) * tr1locTR(1) * tr1locTR(1) -
					2 * tr1locTR(0) * tr1locTR(0) * tr1locTR(2) * tr1locTR(2);

				float temp6TR2 =
					-2 * tr1locTR(1) * tr1locTR(1) * tr1locTR(2) * tr1locTR(2) -
					tr1locTR(1) * tr1locTR(1) * tr1locTR(1) * tr1locTR(1) -
					tr1locTR(2) * tr1locTR(2) * tr1locTR(2) * tr1locTR(2);

				float temp7TR2 = sgn(wcnref[0] - tr2ref[0]) * std::sqrt(temp3TR2 + temp4TR2 + temp5TR2 + temp6TR2);

				tr2locTR(0) = (tr1locTR(0) * temp1TR2 - tr1locTR(1) * temp7TR2) / temp2TR2;
				tr2locTR(1) = (tr1locTR(1) * temp1TR2 + tr1locTR(0) * temp7TR2) / temp2TR2;
				tr2locTR(2) = 0;

				tr2Glob.row(i * (2 * steerIncr + 1) + j) << (rotTR * tr2locTR).transpose() + tr2prGlob.row(i);

				// calculating WCN and SPN
				Eigen::Matrix3f _rotTRk; // TR rotation matrix defined by TR2ref

				Eigen::Vector3f _xCol{ tr2prGlob.row(i) - tr2Glob.row(i * (2 * steerIncr + 1) + j) };
				Eigen::Vector3f _zCol{ lca3Glob.row(i) - uca3Glob.row(i) };
				Eigen::Vector3f _yCol{ _zCol.cross(_xCol) };

				_rotTRk.col(0) << _xCol / _xCol.norm();
				_rotTRk.col(1) << _yCol / _yCol.norm();
				_rotTRk.col(2) << _zCol / _zCol.norm();

				wcnGlob.row(i * (2 * steerIncr + 1) + j) << (_rotTRk * wcnlocTRk).transpose() + tr2prGlob.row(i);
				spnGlob.row(i * (2 * steerIncr + 1) + j) << (_rotTRk * spnlocTRk).transpose() + tr2prGlob.row(i);

				// CP calculation
				float temp1cp{ -20 }; // this is actually vector 0,0,-20
				Eigen::MatrixXf temp2cp((vertIncr * 2 + 1) * (steerIncr * 2 + 1), 3);
				Eigen::MatrixXf temp3cp((vertIncr * 2 + 1) * (steerIncr * 2 + 1), 3);

				temp2cp << spnGlob - wcnGlob;

				temp3cp.col(0) << -temp2cp.col(0).array() * temp2cp.col(2).array() * temp1cp;
				temp3cp.col(1) << -temp2cp.col(1).array() * temp2cp.col(2).array() * temp1cp;
				temp3cp.col(2) <<
					temp2cp.col(1).array() * temp2cp.col(1).array() * temp1cp +
					temp2cp.col(0).array() * temp2cp.col(0).array() * temp1cp;

				temp3cp.rowwise().normalize();

				cpGlob << -temp3cp * wRadius + wcnGlob;
			}
		}
	}

	void LogToConsole()
	{

		std::cout << "lca3\n " << lca3Glob << "\n";
		std::cout << "\n";
		std::cout << "uca3\n " << uca3Glob << "\n";
		std::cout << "\n";

		std::cout << "tr1\n " << tr1Glob << "\n";
		std::cout << "\n";

		std::cout << "tr2\n " << tr2Glob << "\n";
		std::cout << "\n";

		std::cout << "wcn\n " << wcnGlob << "\n";
		std::cout << "\n";

		std::cout << "spn\n " << spnGlob << "\n";
		std::cout << "\n";

		std::cout << "cp\n " << cpGlob << "\n";
		std::cout << "\n";

	}

	float ObjFuncModule(float peakWidth, float flatness, float variable, float target)
	{
		return (float)exp(-1 / peakWidth * pow(abs(variable - target), flatness));
	}

	float GetObjFuncScore(float* peakWidth, float* flatness, float* variables, float* targets, float* weightFactors)
	{

		float objFuncScore = 1.0f;

		for (int i = 0; i < 21; i++)
			objFuncScore -= weightFactors[i] * ObjFuncModule(peakWidth[i], flatness[i], variables[i], targets[i]);

		return objFuncScore;
	}

	float GetCamberAngle(int vertPos, int steerPos)
	{
		float camberAngle;
		int L = vertPos * (2 * steerIncr + 1) + steerPos;
		int R = (2 * vertIncr - vertPos) * (2 * steerIncr + 1) + 2 * steerIncr - steerPos;

		Eigen::Vector3f wheelAxis{
			-wcnGlob.row(L)(0) + cpGlob.row(L)(0),
			-wcnGlob.row(L)(1) + cpGlob.row(L)(1),
			-wcnGlob.row(L)(2) + cpGlob.row(L)(2)
		};

		Eigen::Vector3f groundNormal{
			0,
			-cpGlob.row(R)(2) + cpGlob.row(L)(2),
			-cpGlob.row(R)(1) - cpGlob.row(L)(1)
		};

		// calculate plane parallel to ground going through SPN point with respect to which camber is measured
		float temp1_wcnpr =
			spnGlob.row(L)(1) * groundNormal(1)
			+ spnGlob.row(L)(2) * groundNormal(2)
			- wcnGlob.row(L)(1) * groundNormal(1)
			- wcnGlob.row(L)(2) * groundNormal(2);
		float temp2_wcnpr =
			groundNormal(1) * groundNormal(1) +
			groundNormal(2) * groundNormal(2);

		Eigen::Vector3f wcnpr{
			wcnGlob.row(L)(0),
			wcnGlob.row(L)(1) + groundNormal(1) * temp1_wcnpr / temp2_wcnpr,
			wcnGlob.row(L)(2) + groundNormal(2) * temp1_wcnpr / temp2_wcnpr
		};

		float camber =
			(wcnpr - (Eigen::Vector3f)wcnGlob.row(L)).norm() /
			((Eigen::Vector3f)spnGlob.row(L) -
				(Eigen::Vector3f)wcnGlob.row(L)).norm();

		// tests if camber is negative, if it is it returns negative angle
		if ((wcnpr - (Eigen::Vector3f)wcnGlob.row(L))(2) > 0)
		{
			camberAngle = -asin(camber) * 180 / 3.14159f;
			return camberAngle;
		}

		// if camber is not negative, returns positive angle
		else
		{
			camberAngle = asin(camber) * 180 / 3.14159f;
			return camberAngle;
		}
	}

	float GetToeAngle(int vertPos, int steerPos)
	{
		int position = vertPos * (2 * steerIncr + 1) + steerPos;
		// positive toe angle for toe in and negative for toe out
		Eigen::Vector3d wheelAxis{
			(double)(wcnGlob.row(position)(0) - spnGlob.row(position)(0)),
			(double)(wcnGlob.row(position)(1) - spnGlob.row(position)(1)),
			(double)(wcnGlob.row(position)(2) - spnGlob.row(position)(2))
		};
			//(Eigen::Vector3d)(wcnGlob.row(position) - spnGlob.row(position));
		Eigen::Vector3d refAxis{
			0.0,
			(double)(wcnGlob.row(position)(1) - spnGlob.row(position)(1)),
			(double)(wcnGlob.row(position)(2) - spnGlob.row(position)(2))
		};

		if (wcnGlob.row(position)(0) < spnGlob.row(position)(0)) // toe out case
			return float(-acos(refAxis.norm() / wheelAxis.norm()) * 180.0 / 3.14159);

		else // toe in case
			return float(acos(refAxis.norm() / wheelAxis.norm()) * 180.0 / 3.14159);
	}

	float GetCasterAngle(int vertPos)
	{
		int position = vertPos;
		float casterAngle;
		float caster =
			atan2f(
				(lca3Glob.row(position)(0) - uca3Glob.row(position)(0))
				, (-uca3Glob.row(position)(2) + lca3Glob.row(position)(2)));

		casterAngle = caster * 180 / 3.14159f;
		return casterAngle;
	}

	float GetRollCentreHeight(int vertPos, int steerPos)
	{
		float rollCentreHeight;

		int Lv = vertPos;							// Left wheel only vertical position, UCA3, LCA3 points
		int Rv = 2 * vertIncr - vertPos;		// Right wheel only vertical position, UCA3, LCA3 points

		int Lvs = vertPos * (2 * steerIncr + 1) + steerPos;									  // Left wheel position including vertical and steering movement
		int Rvs = (2 * vertIncr - vertPos) * (2 * steerIncr + 1) + 2 * steerIncr - steerPos;  // Right wheel position including vertical and steering movement

		float slopePrecision{ 0.001f }; // if difference between slopes is less than this value, than they are considered parallel

		Eigen::Vector3f lca3L{ lca3Glob.row(Lv) };
		Eigen::Vector3f uca3L{ uca3Glob.row(Lv) };
		Eigen::Vector3f cpL{ cpGlob.row(Lvs) };

		Eigen::Vector3f lca1R{ lca1ref(0), -lca1ref(1), lca1ref(2) };
		Eigen::Vector3f lca2R{ lca2ref(0), -lca2ref(1), lca2ref(2) };
		Eigen::Vector3f lca3R{ lca3Glob.row(Rv)(0), -lca3Glob.row(Rv)(1), lca3Glob.row(Rv)(2) };

		Eigen::Vector3f uca1R{ uca1ref(0), -uca1ref(1), uca1ref(2) };
		Eigen::Vector3f uca2R{ uca2ref(0), -uca2ref(1), uca2ref(2) };
		Eigen::Vector3f uca3R{ uca3Glob.row(Rv)(0), -uca3Glob.row(Rv)(1), uca3Glob.row(Rv)(2) };
		Eigen::Vector3f cpR{ cpGlob.row(Rvs)(0), -cpGlob.row(Rvs)(1), cpGlob.row(Rvs)(2) };

		float aLCAL;
		float aUCAL;
		float aLCAR;
		float aUCAR;

		float bLCAL;
		float bUCAL;
		float bLCAR;
		float bUCAR;

		// LCA and UCA plane intersection with plane parallel to YZ plane with x coord CPR/2+CPL/2
		// lambda function that defines first point of intersection line
		auto intersectionLineCalc = [cpL, cpR](float& aCa, float& bCa, const Eigen::Vector3f& ca1, const Eigen::Vector3f& ca2, const Eigen::Vector3f& ca3)
		{
			// plane coefficients Ax + By + Cz + D = 0, plane defined by control arm
			float A = (-ca1[1] + ca2[1]) * (-ca1[2] + ca3[2]) - (-ca1[1] + ca3[1]) * (-ca1[2] + ca2[2]);
			float B = -(-ca1[0] + ca2[0]) * (-ca1[2] + ca3[2]) + (-ca1[0] + ca3[0]) * (-ca1[2] + ca2[2]);
			float C = (-ca1[0] + ca2[0]) * (-ca1[1] + ca3[1]) - (-ca1[0] + ca3[0]) * (-ca1[1] + ca2[1]);
			float D = -ca1[0] * A - ca1[1] * B - ca1[2] * C;

			// intersection plane defined  as x + D2 = 0
			float D2 = -(cpL[0] + cpR[0]) / 2;

			aCa = -B / C;
			bCa = (A * D2 - D) / C;
		};

		intersectionLineCalc(aLCAL, bLCAL, lca1ref, lca2ref, lca3L);
		intersectionLineCalc(aUCAL, bUCAL, uca1ref, uca2ref, uca3L);
		intersectionLineCalc(aLCAR, bLCAR, lca1R, lca2R, lca3R);
		intersectionLineCalc(aUCAR, bUCAR, uca1R, uca2R, uca3R);

		float aICL;
		float aICR;
		float bICL;
		float bICR;

		// CALCULATE LEFT SIDE
		// case if LEFT LCA and UCA are parallel
		if (abs(aLCAL - aUCAL) / abs(aLCAL) < precision)
		{
			aICL = aLCAL;
			bICL = cpL(2) - aLCAL * cpL(1);
		}
		// case if LEFT LCA and UCA are NOT parallel
		else
		{
			float ICLz = (aLCAL * bUCAL - aUCAL * bLCAL) / (aLCAL - aUCAL);
			float ICLy = (-bLCAL + bUCAL) / (aLCAL - aUCAL);

			aICL = (ICLz - cpL(2)) / (ICLy - cpL(1));
			bICL = -cpL(1) * aICL + cpL(2);
		}

		// CALCULATE RIGHT SIDE
		// case if RIGHT LCA and UCA are parallel
		if (abs(aLCAR - aUCAR) / abs(aLCAR) < precision)
		{
			aICR = aLCAR;
			bICR = cpR(2) - aLCAR * cpR(1);
		}
		// case if RIGHT LCA and UCA are NOT parallel
		else
		{
			float ICRz = (aLCAR * bUCAR - aUCAR * bLCAR) / (aLCAR - aUCAR);
			float ICRy = (-bLCAR + bUCAR) / (aLCAR - aUCAR);

			aICR = (ICRz - cpR(2)) / (ICRy - cpR(1));
			bICR = -cpR(1) * aICR + cpR(2);
		}

		// instantenous centre lines are parallel
		if (abs(aICR - aICL) / abs(aICR) < precision)
		{
			rollCentreHeight = 0;
			return rollCentreHeight;
		}

		// instantenous centre lines are NOT parallel
		else
		{
			float RCy = (bICL - bICR) / (aICR - aICL);
			float RCz = aICL * RCy + bICL;

			rollCentreHeight =
				((cpR(1) - cpL(1)) * (cpL(2) - RCz) -
					(cpL(1) - RCy) * (cpR(2) - cpL(2))) /
				sqrt(pow((cpR(2) - cpL(2)), 2) + pow((cpR(1) - cpL(1)), 2));
			return rollCentreHeight;
		}
	}

	float GetCasterTrail(int vertPos, int steerPos)
	{
		float casterTrail;

		int Lv = vertPos;							// Left wheel only vertical position, UCA3, LCA3 points
		int Rv = 2 * vertIncr - vertPos;		// Right wheel only vertical position, UCA3, LCA3 points

		int Lvs = vertPos * (2 * steerIncr + 1) + steerPos;									  // Left wheel position including vertical and steering movement
		int Rvs = (2 * vertIncr - vertPos) * (2 * steerIncr + 1) + 2 * steerIncr - steerPos;  // Right wheel position including vertical and steering movement

		//int Lv = vertPos;							// Left wheel only vertical position, UCA3, LCA3 points
		//int Rv = cpGlob.rows() - 1 - vertPos;		// Right wheel only vertical position, UCA3, LCA3 points

		//int Lvs = vertPos * (2 * steerIncr + 1) + steerPos;							// Left wheel position including vertical and steering movement
		//int Rvs = (cpGlob.rows() - 1 - vertPos) * (2 * steerIncr + 1) + steerPos;		// Right wheel position including vertical and steering movement

		Eigen::Vector3f cpL{ cpGlob.row(Lvs) };
		Eigen::Vector3f cpR{ cpGlob.row(Rvs) };
		Eigen::Vector3f wcn{ wcnGlob.row(Lvs) };
		Eigen::Vector3f spn{ spnGlob.row(Lvs) };
		Eigen::Vector3f lca3L{ lca3Glob.row(Lv) };
		Eigen::Vector3f uca3L{ uca3Glob.row(Lv) };

		Eigen::Vector3f grndNormal{
			0,
			-cpR(2) + cpL(2),
			-cpR(1) - cpL(1)
		};

		float wcnpr_temp1 =
			grndNormal(0) * cpL(0) - grndNormal(0) * wcn(0) +
			grndNormal(1) * cpL(1) - grndNormal(1) * wcn(1) +
			grndNormal(2) * cpL(2) - grndNormal(2) * wcn(2);

		float wcnpr_temp2 =
			grndNormal(0) * grndNormal(0) +
			grndNormal(1) * grndNormal(1) +
			grndNormal(2) * grndNormal(2);

		Eigen::Vector3f wcnpr{
			grndNormal(0) * wcnpr_temp1 / wcnpr_temp2 + wcn(0),
			grndNormal(1) * wcnpr_temp1 / wcnpr_temp2 + wcn(1),
			grndNormal(2) * wcnpr_temp1 / wcnpr_temp2 + wcn(2)
		};

		float spnpr_temp1 =
			grndNormal(0) * cpL(0) - grndNormal(0) * wcn(0) +
			grndNormal(1) * cpL(1) - grndNormal(1) * wcn(1) +
			grndNormal(2) * cpL(2) - grndNormal(2) * wcn(2);

		float spnpr_temp2 =
			grndNormal(0) * grndNormal(0) +
			grndNormal(1) * grndNormal(1) +
			grndNormal(2) * grndNormal(2);

		Eigen::Vector3f spnpr{
			grndNormal(0) * spnpr_temp1 / spnpr_temp2 + spn(0),
			grndNormal(1) * spnpr_temp1 / spnpr_temp2 + spn(1),
			grndNormal(2) * spnpr_temp1 / spnpr_temp2 + spn(2)
		};

		float l3u3intrs_temp1 =
			-grndNormal(0) * cpL(0) + grndNormal(0) * lca3L(0)
			- grndNormal(1) * cpL(1) + grndNormal(1) * lca3L(1)
			- grndNormal(2) * cpL(2) + grndNormal(2) * lca3L(2);

		float l3u3intrs_temp2 =
			grndNormal(0) * lca3L(0) - grndNormal(0) * uca3L(0) +
			grndNormal(1) * lca3L(1) - grndNormal(1) * uca3L(1) +
			grndNormal(2) * lca3L(2) - grndNormal(2) * uca3L(2);

		Eigen::Vector3f l3u3intrs{
			lca3L(0) - (lca3L(0) - uca3L(0)) * l3u3intrs_temp1 / l3u3intrs_temp2,
			lca3L(1) - (lca3L(1) - uca3L(1)) * l3u3intrs_temp1 / l3u3intrs_temp2,
			lca3L(2) - (lca3L(2) - uca3L(2)) * l3u3intrs_temp1 / l3u3intrs_temp2
		};

		float caster_trail =
			(l3u3intrs - spnpr).cross(l3u3intrs - wcnpr).norm() / (wcnpr - spnpr).norm();

		// positive caster trail
		if ((l3u3intrs - spnpr).cross(l3u3intrs - wcnpr)(2) > 0)
		{
			casterTrail = caster_trail;

			return casterTrail;
		}

		// negative caster trail
		else
		{
			casterTrail = -caster_trail;
			return casterTrail;
		}
	}

	float GetScrubRadius(int vertPos, int steerPos)
	{
		float scrubRadius;

		int Lv = vertPos;							// Left wheel only vertical position, UCA3, LCA3 points
		int Rv = 2 * vertIncr - vertPos;		// Right wheel only vertical position, UCA3, LCA3 points

		int Lvs = vertPos * (2 * steerIncr + 1) + steerPos;									  // Left wheel position including vertical and steering movement
		int Rvs = (2 * vertIncr - vertPos) * (2 * steerIncr + 1) + 2 * steerIncr - steerPos;  // Right wheel position including vertical and steering movement

		//int Lv = vertPos;							// Left wheel only vertical position, UCA3, LCA3 points
		//int Rv = cpGlob.rows() - 1 - vertPos;		// Right wheel only vertical position, UCA3, LCA3 points

		//int Lvs = vertPos * (2 * steerIncr + 1) + steerPos;							// Left wheel position including vertical and steering movement
		//int Rvs = (cpGlob.rows() - 1 - vertPos) * (2 * steerIncr + 1) + steerPos;		// Right wheel position including vertical and steering movement

		Eigen::Vector3f cpL{ cpGlob.row(Lvs) };
		Eigen::Vector3f cpR{ cpGlob.row(Rvs) };
		Eigen::Vector3f wcn{ wcnGlob.row(Lvs) };
		Eigen::Vector3f spn{ spnGlob.row(Lvs) };
		Eigen::Vector3f lca3L{ lca3Glob.row(Lv) };
		Eigen::Vector3f uca3L{ uca3Glob.row(Lv) };

		Eigen::Vector3f grndNormal{
			0,
			-cpR(2) + cpL(2),
			-cpR(1) - cpL(1)
		};

		float wcnpr_temp1 =
			grndNormal(0) * cpL(0) - grndNormal(0) * wcn(0) +
			grndNormal(1) * cpL(1) - grndNormal(1) * wcn(1) +
			grndNormal(2) * cpL(2) - grndNormal(2) * wcn(2);

		float wcnpr_temp2 =
			grndNormal(0) * grndNormal(0) +
			grndNormal(1) * grndNormal(1) +
			grndNormal(2) * grndNormal(2);

		Eigen::Vector3f wcnpr{
			grndNormal(0) * wcnpr_temp1 / wcnpr_temp2 + wcn(0),
			grndNormal(1) * wcnpr_temp1 / wcnpr_temp2 + wcn(1),
			grndNormal(2) * wcnpr_temp1 / wcnpr_temp2 + wcn(2)
		};

		float spnpr_temp1 =
			grndNormal(0) * cpL(0) - grndNormal(0) * wcn(0) +
			grndNormal(1) * cpL(1) - grndNormal(1) * wcn(1) +
			grndNormal(2) * cpL(2) - grndNormal(2) * wcn(2);

		float spnpr_temp2 =
			grndNormal(0) * grndNormal(0) +
			grndNormal(1) * grndNormal(1) +
			grndNormal(2) * grndNormal(2);

		Eigen::Vector3f spnpr{
			grndNormal(0) * spnpr_temp1 / spnpr_temp2 + spn(0),
			grndNormal(1) * spnpr_temp1 / spnpr_temp2 + spn(1),
			grndNormal(2) * spnpr_temp1 / spnpr_temp2 + spn(2)
		};

		float l3u3intrs_temp1 =
			-grndNormal(0) * cpL(0) + grndNormal(0) * lca3L(0)
			- grndNormal(1) * cpL(1) + grndNormal(1) * lca3L(1)
			- grndNormal(2) * cpL(2) + grndNormal(2) * lca3L(2);

		float l3u3intrs_temp2 =
			grndNormal(0) * lca3L(0) - grndNormal(0) * uca3L(0) +
			grndNormal(1) * lca3L(1) - grndNormal(1) * uca3L(1) +
			grndNormal(2) * lca3L(2) - grndNormal(2) * uca3L(2);

		Eigen::Vector3f l3u3intrs{
			lca3L(0) - (lca3L(0) - uca3L(0)) * l3u3intrs_temp1 / l3u3intrs_temp2,
			lca3L(1) - (lca3L(1) - uca3L(1)) * l3u3intrs_temp1 / l3u3intrs_temp2,
			lca3L(2) - (lca3L(2) - uca3L(2)) * l3u3intrs_temp1 / l3u3intrs_temp2
		};

		// scrub radius
		float scrubRadius_temp1 =
			(wcn[0] - spn[0]) * (l3u3intrs[0] - cpL[0]) +
			(wcn[1] - spn[1]) * (l3u3intrs[1] - cpL[1]) +
			(wcn[2] - spn[2]) * (l3u3intrs[2] - cpL[2]);

		float scrubRadius_temp2 =
			pow((wcn[0] - spn[0]), 2) +
			pow((wcn[1] - spn[1]), 2) +
			pow((wcn[2] - spn[2]), 2);

		scrubRadius = scrubRadius_temp1 / sqrtf(scrubRadius_temp2);

		return scrubRadius;

	}

	float GetKingpinAngle(int vertPos, int steerPos)
	{
		float kingpinAngle;

		int Lv = vertPos;							// Left wheel only vertical position, UCA3, LCA3 points
		int Rv = 2 * vertIncr - vertPos;		// Right wheel only vertical position, UCA3, LCA3 points

		int Lvs = vertPos * (2 * steerIncr + 1) + steerPos;									  // Left wheel position including vertical and steering movement
		int Rvs = (2 * vertIncr - vertPos) * (2 * steerIncr + 1) + 2 * steerIncr - steerPos;  // Right wheel position including vertical and steering movement

		//int Lv = vertPos;							// Left wheel only vertical position, UCA3, LCA3 points
		//int Rv = cpGlob.rows() - 1 - vertPos;		// Right wheel only vertical position, UCA3, LCA3 points

		//int Lvs = vertPos * (2 * steerIncr + 1) + steerPos;							// Left wheel position including vertical and steering movement
		//int Rvs = (cpGlob.rows() - 1 - vertPos) * (2 * steerIncr + 1) + steerPos;		// Right wheel position including vertical and steering movement

		Eigen::Vector3f cpL{ cpGlob.row(Lvs) };
		Eigen::Vector3f cpR{ cpGlob.row(Rvs) };

		Eigen::Vector3f grndNormal{
			0,
			-cpR(2) + cpL(2),
			-cpR(1) - cpL(1)
		};

		Eigen::Vector3f l3u3pr{
			0,
			-uca3Glob.row(Lv)(1) + lca3Glob.row(Lv)(1),
			-uca3Glob.row(Lv)(2) + lca3Glob.row(Lv)(2)
		};

		// if uca3 is closer to chassis centre then kingpin angle is positive
		if (abs(uca3Glob.row(Lv)(1)) < abs(lca3Glob.row(Lv)(1)))
		{
			kingpinAngle = acos(grndNormal.dot(l3u3pr) / grndNormal.norm() / l3u3pr.norm()) * 180.0f / 3.14159f;
			return kingpinAngle;
		}
		// otherwise negative kingpin angle
		else
		{
			kingpinAngle = -acos(grndNormal.dot(l3u3pr) / grndNormal.norm() / l3u3pr.norm()) * 180.0f / 3.14159f;
			return kingpinAngle;

		}
	}

	float GetAntiDrive(int vertPos, int steerPos)
	{
		float antiDrive;

		int Lv = vertPos;							// Left wheel only vertical position, UCA3, LCA3 points
		int Lvs = vertPos * (2 * steerIncr + 1) + steerPos;							// Left wheel position including vertical and steering movement

		Eigen::Vector3f lca3{ lca3Glob.row(Lv) };
		Eigen::Vector3f uca3{ uca3Glob.row(Lv) };
		Eigen::Vector3f cp{ cpGlob.row(Lvs) };
		Eigen::Vector3f wcn{ wcnGlob.row(Lvs) };

		float aLCA;
		float aUCA;

		float bLCA;
		float bUCA;

		// LCA and UCA plane intersection with plane parallel to YZ plane with x coord CPR/2+CPL/2
		// lambda function that defines first point of intersection line
		auto intersectionLineCalc = [cp](float& aCa, float& bCa, const Eigen::Vector3f& ca1, const Eigen::Vector3f& ca2, const Eigen::Vector3f& ca3)
		{
			// plane coefficients Ax + By + Cz + D = 0, plane defined by control arm
			float A = (-ca1[1] + ca2[1]) * (-ca1[2] + ca3[2]) - (-ca1[1] + ca3[1]) * (-ca1[2] + ca2[2]);
			float B = -(-ca1[0] + ca2[0]) * (-ca1[2] + ca3[2]) + (-ca1[0] + ca3[0]) * (-ca1[2] + ca2[2]);
			float C = (-ca1[0] + ca2[0]) * (-ca1[1] + ca3[1]) - (-ca1[0] + ca3[0]) * (-ca1[1] + ca2[1]);
			float D = -ca1[0] * A - ca1[1] * B - ca1[2] * C;

			// intersection plane defined  as x + D2 = 0
			float D2 = -cp[1];

			aCa = -A / C;
			bCa = (B * D2 - D) / C;
		};

		intersectionLineCalc(aLCA, bLCA, lca1ref, lca2ref, lca3);
		intersectionLineCalc(aUCA, bUCA, uca1ref, uca2ref, uca3);

		float tanThetaOutboard;
		float tanThetaInboard;

		// if resulting lines are parallel
		if (abs(aLCA - aUCA) / abs(aLCA) < precision)
		{
			tanThetaInboard = aLCA;
			tanThetaOutboard = aLCA;
		}
		// if resulting lines are not parallel
		else
		{
			float ICPtx = (bLCA - bUCA) / (aUCA - aLCA);
			float ICPtz = aLCA * ICPtx + bLCA;

			tanThetaOutboard = (ICPtz - wcn[2]) / (ICPtx - wcn[0]);
			tanThetaInboard = (ICPtz - cp[2]) / (ICPtx - cp[0]);
		}

		if (suspPos == 0)  // front suspension
		{
			if (drivePos == 0)                        // outboard drive
				antiDrive = tanThetaOutboard * wheelbase / cogHeight * driveBias * 100;

			else if (driveBias == 0)                  // inboard drive
				antiDrive = 0;
			else
				antiDrive = tanThetaInboard * wheelbase / cogHeight / driveBias * 100;
		}

		else  // rear suspension
		{
			if (drivePos == 0)                        // outboard drive
				antiDrive = -tanThetaOutboard * wheelbase / cogHeight * driveBias * 100;

			else if (driveBias == 0)                  // inboard drive
				antiDrive = 0;
			else
				antiDrive = -tanThetaInboard * wheelbase / cogHeight / driveBias * 100;
		}

		return antiDrive;
	}

	float GetAntiBrake(int vertPos, int steerPos)
	{
		float antiBrakes;

		int Lv = vertPos;							// Left wheel only vertical position, UCA3, LCA3 points
		int Lvs = vertPos * (2 * steerIncr + 1) + steerPos;							// Left wheel position including vertical and steering movement

		Eigen::Vector3f lca3{ lca3Glob.row(Lv) };
		Eigen::Vector3f uca3{ uca3Glob.row(Lv) };
		Eigen::Vector3f cp{ cpGlob.row(Lvs) };
		Eigen::Vector3f wcn{ wcnGlob.row(Lvs) };

		float aLCA;
		float aUCA;

		float bLCA;
		float bUCA;

		// LCA and UCA plane intersection with plane parallel to YZ plane with x coord CPR/2+CPL/2
		// lambda function that defines first point of intersection line
		auto intersectionLineCalc = [cp](float& aCa, float& bCa, const Eigen::Vector3f& ca1, const Eigen::Vector3f& ca2, const Eigen::Vector3f& ca3)
		{
			// plane coefficients Ax + By + Cz + D = 0, plane defined by control arm
			float A = (-ca1[1] + ca2[1]) * (-ca1[2] + ca3[2]) - (-ca1[1] + ca3[1]) * (-ca1[2] + ca2[2]);
			float B = -(-ca1[0] + ca2[0]) * (-ca1[2] + ca3[2]) + (-ca1[0] + ca3[0]) * (-ca1[2] + ca2[2]);
			float C = (-ca1[0] + ca2[0]) * (-ca1[1] + ca3[1]) - (-ca1[0] + ca3[0]) * (-ca1[1] + ca2[1]);
			float D = -ca1[0] * A - ca1[1] * B - ca1[2] * C;

			// intersection plane defined  as x + D2 = 0
			float D2 = -cp[1];

			aCa = -A / C;
			bCa = (B * D2 - D) / C;
		};

		intersectionLineCalc(aLCA, bLCA, lca1ref, lca2ref, lca3);
		intersectionLineCalc(aUCA, bUCA, uca1ref, uca2ref, uca3);

		float tanThetaOutboard;
		float tanThetaInboard;

		// if resulting lines are parallel
		if (abs(aLCA - aUCA) / abs(aLCA) < precision)
		{
			tanThetaInboard = aLCA;
			tanThetaOutboard = aLCA;
		}
		// if resulting lines are not parallel
		else
		{
			float ICPtx = (bLCA - bUCA) / (aUCA - aLCA);
			float ICPtz = aLCA * ICPtx + bLCA;

			tanThetaOutboard = (ICPtz - wcn[2]) / (ICPtx - wcn[0]);
			tanThetaInboard = (ICPtz - cp[2]) / (ICPtx - cp[0]);
		}

		if (suspPos == 0)  // front suspension
		{
			if (brakePos == 0)                        // outboard brakes
				antiBrakes = tanThetaOutboard * wheelbase / cogHeight * brakeBias * 100;

			else if (brakeBias == 0)                  // inboard brakes
				antiBrakes = 0;
			else
				antiBrakes = tanThetaInboard * wheelbase / cogHeight / brakeBias * 100;
		}

		else  // rear suspension
		{
			if (brakePos == 0)                        // outboard brakes
				antiBrakes = -tanThetaOutboard * wheelbase / cogHeight * brakeBias * 100;

			else if (brakeBias == 0)                  // inboard brakes
				antiBrakes = 0;
			else
				antiBrakes = -tanThetaInboard * wheelbase / cogHeight / brakeBias * 100;

		}
		return antiBrakes;
	}

	float GetHalfTrackChange(int vertPos, int steerPos)
	{
		int position = vertPos * (2 * steerIncr + 1) + steerPos;
		float halfTrackChange;
		// if current wheelbase or half track is smaller than reference than negative sign, otherwise positive
		halfTrackChange = cpGlob.row(cpGlob.rows() / 2)[1] - cpGlob.row(position)[1];
		return halfTrackChange;
	}

	float GetWheelbaseChange(int vertPos, int steerPos)
	{
		int position = vertPos * (2 * steerIncr + 1) + steerPos;

		float wheelbaseChange;
		// if current wheelbase or half track is smaller than reference than negative sign, otherwise positive
		wheelbaseChange = -cpGlob.row(cpGlob.rows() / 2)[0] + cpGlob.row(position)[0];
		return wheelbaseChange;
	}

	float GetLca3DistanceFromWheelAxis()
	{
		return CalculateDistancePointToLine(spnref, wcnref, lca3ref);

	}

	float GetUca3DistanceFromWheelAxis()
	{
		return CalculateDistancePointToLine(spnref, wcnref, uca3ref);

	}

	float GetTr2DistanceFromWheelAxis()
	{
		return CalculateDistancePointToLine(spnref, wcnref, tr2ref);

	}

	float CalculateDistancePointToLine(const Eigen::Vector3f& linePt1, const Eigen::Vector3f& linePt2, const Eigen::Vector3f& Pt)
	{
		float distance;
		distance = (Pt - linePt1).cross(Pt - linePt2).norm() / (linePt2 - linePt1).norm();
		return distance;
	}

	float GetLca3DistanceToWheelCentrePlane()
	{
		return GetSignedPointToPlaneDistance(wcnref, spnref, lca3ref);
	}

	float GetUca3DistanceToWheelCentrePlane()
	{
		return GetSignedPointToPlaneDistance(wcnref, spnref, uca3ref);
	}

	float GetTr2DistanceToWheelCentrePlane()
	{
		return GetSignedPointToPlaneDistance(wcnref, spnref, tr2ref);
	}

	float GetSignedPointToPlaneDistance(const Eigen::Vector3f linePt1, const Eigen::Vector3f& linePt2, const Eigen::Vector3f& Pt)
	{
		/*Calculates distance from point to plane and gives a sign (+ or -) for distance, positive when distanced in direction of plane normal and  negative otherwise, linePt1 is the head of normal vector and linePt2 tail*/
		float distance;
		float A = linePt1[0] - linePt2[0];
		float B = linePt1[1] - linePt2[1];
		float C = linePt1[2] - linePt2[2];
		float D = -linePt1[0] * A - linePt1[1] * B - linePt1[2] * C;

		distance = (A * Pt[0] + B * Pt[1] + C * Pt[2] + D) / sqrtf(A * A + B * B + C * C);
		return distance;
	}

	void GetOptimisationCharacteristicsArray(float* characteristicsArray)
	{

		characteristicsArray[0] = GetCamberAngle(0, 0);
		characteristicsArray[1] = GetCamberAngle(2, 0);
		characteristicsArray[2] = GetToeAngle(0, 0);
		characteristicsArray[3] = GetToeAngle(2, 0);
		characteristicsArray[4] = GetCasterAngle(1);
		characteristicsArray[5] = GetRollCentreHeight(1, 0);
		characteristicsArray[6] = GetCasterTrail(1, 0);
		characteristicsArray[7] = GetScrubRadius(1, 0);
		characteristicsArray[8] = GetKingpinAngle(1, 0);
		characteristicsArray[9] = GetAntiDrive(1, 0);
		characteristicsArray[10] = GetAntiBrake(1, 0);
		characteristicsArray[11] = GetHalfTrackChange(0, 0);
		characteristicsArray[12] = GetHalfTrackChange(2, 0);
		characteristicsArray[13] = GetWheelbaseChange(0, 0);
		characteristicsArray[14] = GetWheelbaseChange(2, 0);
		characteristicsArray[15] = GetLca3DistanceFromWheelAxis();
		characteristicsArray[16] = GetUca3DistanceFromWheelAxis();
		characteristicsArray[17] = GetTr2DistanceFromWheelAxis();
		characteristicsArray[18] = GetLca3DistanceToWheelCentrePlane();
		characteristicsArray[19] = GetUca3DistanceToWheelCentrePlane();
		characteristicsArray[20] = GetTr2DistanceToWheelCentrePlane();

	}

	void GetMovedHardpoints(float* outputLca3, float* outputUca3, float* outputTr1, float* outputTr2,
		float* outputWcn, float* outputSpn) {

		for (int i = 0; i < vertIncr * 2 + 1; i++)
		{
			outputLca3[i * 3] = lca3Glob.row(i)(0);
			outputLca3[i * 3 + 1] = lca3Glob.row(i)(1);
			outputLca3[i * 3 + 2] = lca3Glob.row(i)(2);

			outputUca3[i * 3] = uca3Glob.row(i)(0);
			outputUca3[i * 3 + 1] = uca3Glob.row(i)(1);
			outputUca3[i * 3 + 2] = uca3Glob.row(i)(2);

			for (int j = 0; j < steerIncr * 2 + 1; j++)
			{
				outputTr2[(i * (2 * steerIncr + 1) + j) * 3] = tr2Glob.row(i * (2 * steerIncr + 1) + j)(0);
				outputTr2[(i * (2 * steerIncr + 1) + j) * 3 + 1] = tr2Glob.row(i * (2 * steerIncr + 1) + j)(1);
				outputTr2[(i * (2 * steerIncr + 1) + j) * 3 + 2] = tr2Glob.row(i * (2 * steerIncr + 1) + j)(2);

				outputWcn[(i * (2 * steerIncr + 1) + j) * 3] = wcnGlob.row(i * (2 * steerIncr + 1) + j)(0);
				outputWcn[(i * (2 * steerIncr + 1) + j) * 3 + 1] = wcnGlob.row(i * (2 * steerIncr + 1) + j)(1);
				outputWcn[(i * (2 * steerIncr + 1) + j) * 3 + 2] = wcnGlob.row(i * (2 * steerIncr + 1) + j)(2);

				outputSpn[(i * (2 * steerIncr + 1) + j) * 3] = spnGlob.row(i * (2 * steerIncr + 1) + j)(0);
				outputSpn[(i * (2 * steerIncr + 1) + j) * 3 + 1] = spnGlob.row(i * (2 * steerIncr + 1) + j)(1);
				outputSpn[(i * (2 * steerIncr + 1) + j) * 3 + 2] = spnGlob.row(i * (2 * steerIncr + 1) + j)(2);
			}
		}

		for (int j = 0; j < steerIncr * 2 + 1; j++)
		{
			outputTr1[j * 3] = tr1Glob.row(j)(0);
			outputTr1[j * 3 + 1] = tr1Glob.row(j)(1);
			outputTr1[j * 3 + 2] = tr1Glob.row(j)(2);
		}
	}
};

void optimisation_obj_res(float* hardpoints, int suspPos, float wRadius,
	float wheelbase, float cogHeight, float driveBias, float brakeBias,
	int drivePos, int brakePos,
	float wVert, float* peakWidth, float* flatness, float wSteer, int vertIncr, int steerIncr, float precision, float* targetValues,
	float* weightFactors, float& obj_func_res, float* outputParams)
{
	Suspension susp{

		hardpoints,
		wRadius,
		wheelbase, cogHeight, driveBias, brakeBias,
		suspPos, drivePos, brakePos,
		wVert, wSteer,
		vertIncr, steerIncr, precision
	};
	susp.CalculateMovement();

	susp.GetOptimisationCharacteristicsArray(outputParams);
	obj_func_res = susp.GetObjFuncScore(peakWidth, flatness, outputParams, targetValues, weightFactors);
}

void suspension_movement(float* hardpoints, float wRadiusin,
	float wheelbase, float cogHeight, float frontDriveBias, float frontBrakeBias,
	int suspPos, int drivePos, int brakePos,
	float wVertin, float wSteerin, int vertIncrin, int steerIncrin, float precisionin,

	float* camberAngle,
	float* toeAngle,
	float* casterAngle,
	float* rcHeight,
	float* casterTrail,
	float* scrubRadius,
	float* kingpinAngle,
	float* antiDrive,
	float* antiBrake,
	float* halfTrackChange,
	float* wheelbaseChange,
	float* constOutputParams,

	float* outputLca3,
	float* outputUca3,
	float* outputTr1,
	float* outputTr2,
	float* outputWcn,
	float* outputSpn)
{

	Suspension susp{
		hardpoints,
		wRadiusin,
		wheelbase, cogHeight, frontDriveBias, frontBrakeBias,
		suspPos, drivePos, brakePos,
		wVertin, wSteerin,
		vertIncrin, steerIncrin, precisionin
	};

	susp.CalculateMovement();

	susp.GetMovedHardpoints(outputLca3, outputUca3, outputTr1, outputTr2, outputWcn, outputSpn);
	for (int i = 0; i < vertIncrin * 2 + 1; i++)
	{

		for (int j = 0; j < steerIncrin * 2 + 1; j++)
		{
			camberAngle[i * (2 * steerIncrin + 1) + j] = susp.GetCamberAngle(i, j);
			toeAngle[i * (2 * steerIncrin + 1) + j] = susp.GetToeAngle(i, j);
			casterAngle[i * (2 * steerIncrin + 1) + j] = susp.GetCasterAngle(i);
			rcHeight[i * (2 * steerIncrin + 1) + j] = susp.GetRollCentreHeight(i, j);
			casterTrail[i * (2 * steerIncrin + 1) + j] = susp.GetCasterTrail(i, j);
			scrubRadius[i * (2 * steerIncrin + 1) + j] = susp.GetScrubRadius(i, j);
			kingpinAngle[i * (2 * steerIncrin + 1) + j] = susp.GetKingpinAngle(i, j);
			antiDrive[i * (2 * steerIncrin + 1) + j] = susp.GetAntiDrive(i, j);
			antiBrake[i * (2 * steerIncrin + 1) + j] = susp.GetAntiBrake(i, j);
			halfTrackChange[i * (2 * steerIncrin + 1) + j] = susp.GetHalfTrackChange(i, j);
			wheelbaseChange[i * (2 * steerIncrin + 1) + j] = susp.GetWheelbaseChange(i, j);

		}
	}

	constOutputParams[0] = susp.GetLca3DistanceFromWheelAxis();
	constOutputParams[1] = susp.GetUca3DistanceFromWheelAxis();
	constOutputParams[2] = susp.GetTr2DistanceFromWheelAxis();
	constOutputParams[3] = susp.GetLca3DistanceToWheelCentrePlane();
	constOutputParams[4] = susp.GetUca3DistanceToWheelCentrePlane();
	constOutputParams[5] = susp.GetTr2DistanceToWheelCentrePlane();
}


/*
 * main.cpp
 *
 *  Created on: 26.10.2010
 *      Author: stephaniebayer
 */

#include <stdio.h>
#include <time.h>
#include <vector>
#include <fstream>

#include "G_q.h"
#include "Functions.h"
#include "ElGammal.h"
#include "Cipher_elg.h"
#include "Permutation.h"
#include "Prover.h"
#include "Prover_me.h"
#include "Prover_fft.h"
#include "Prover_toom.h"
#include "Verifier.h"
#include "Verifier_me.h"
#include "Verifier_toom.h"
#include <NTL/ZZ.h>
#include <NTL/mat_ZZ.h>

#include <NTL/matrix.h>
#include <NTL/vec_vec_ZZ.h>
NTL_CLIENT

 G_q G=G_q();// group used for the Pedersen commitment
 G_q H=G_q();// group used for the the encryption
 ElGammal El = ElGammal(); //The class for encryption and decryption
 Pedersen Ped = Pedersen(); //Object which calculates the commitments
 double time_rw_p =0;
 double time_rw_v=0;
 double time_cm =0;
 long m_r=0;//number of rows after reduction
 long mu=0; //number of rows after reduction
 long mu_h=0;//2*mu-1, number of extra elements in the reduction

 int shuffle_wo_opti(vector<vector<Cipher_elg>* >* e,vector<vector<Cipher_elg>* >* E, vector<vector<ZZ>*>* R,vector<vector<vector<long>* >* > * pi, vector<long> num, ZZ genq);
 int shuffle_w_opti_me(vector<vector<Cipher_elg>* >* e, vector<vector<Cipher_elg>* >* E, vector<vector<ZZ>*>* R,vector<vector<vector<long>* >* > * pi, vector<long> num);
 int shuffle_w_opti(vector<vector<Cipher_elg>* >* e, vector<vector<Cipher_elg>* >* E, vector<vector<ZZ>*>* R,vector<vector<vector<long>* >* > * pi, vector<long> num, ZZ genq);
 int shuffle_w_toom(vector<vector<Cipher_elg>* >* e, vector<vector<Cipher_elg>* >* E, vector<vector<ZZ>*>* R,vector<vector<vector<long>* >* > * pi, vector<long> num, ZZ genq);

 int main(){
	int i;
	vector<long> num; //Containing the number of ciphertexts and the structure of the matrix of the ciphertexts
	vector<vector<Cipher_elg>* >* c=0; // contains the original input ciphertexts
	vector<vector<Cipher_elg>* >* C=0;//Contains reencryptetd ciphers
	vector<vector<vector<long>* >* > * pi=0; //Permutation
	vector<vector<ZZ>* >* R=0; //Random elements for reencryption
	ZZ genq; //generator of Z_q
	long m, n;
	double tstart,  tstop, ttime, time_p, time_v;
	string file_name;

	time_p = 0;
	time_v = 0;
	num=vector<long>(8);
	Functions::read_config(num, genq);

	 m = num[1];
	 n = num[2];

	 Ped = Pedersen(n, G);
	 Ped.set_omega(num[3], num[7], num[4]);

	c =new vector<vector<Cipher_elg>* >(m);

	Functions::createCipher(c,num);
	tstart = (double)clock()/CLOCKS_PER_SEC;
	pi = new vector<vector<vector<long>* >* >(m);
	Permutation::perm_matrix(pi,n,m);
	R = new vector<vector<ZZ>*>(m);
	Functions::randomEl(R,num);
	C=new vector<vector<Cipher_elg>* >(m);
	Functions::reencryptCipher(C,c,pi,R,num);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	cout << "To shuffle the ciphertexts took " << ttime << " second(s)." << endl;

	if(num[5]==0){
		shuffle_wo_opti(c,C,R, pi, num, genq);
	}
	else if(num[5]==1){
		cout<<"Multi-expo version:"<<endl;
		shuffle_w_opti_me(c,C,R, pi, num);
	}
	else if(num[5]==2){
		cout<<"FFT:"<<endl;
		shuffle_w_opti(c,C,R, pi, num, genq);
	}
	else if(num[5]==3){
		cout<<"Toom-Cook and Interaction:"<<endl;
		shuffle_w_toom(c,C,R, pi, num, genq);
	}

	Functions::delete_vector(c);
	Functions::delete_vector(C);
	Functions::delete_vector(R);
	Functions::delete_vector(pi);
}

int shuffle_wo_opti(vector<vector<Cipher_elg>* >* c, vector<vector<Cipher_elg>* >* C, vector<vector<ZZ>*>* R,vector<vector<vector<long>* >* > * pi, vector<long> num, ZZ genq){
	Prover* P=0;
	Verifier* V=0;
	P = new Prover(C,R,pi,num, genq);
	V = new Verifier(num);
	double tstart, tstart_t, tstop,tstop_t, ttime, time_p, time_v;
	ZZ chal_10,ans_12;
	string file_name, name;
	ofstream ost;

	time_p=0;
	time_v =0;
	time_cm =0;

	tstart_t = (double)clock()/CLOCKS_PER_SEC;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_1();
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_2(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_3(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_4(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_5(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_6(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_7(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V-> round_8(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
		tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = P->round_9(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_p+=ttime;
		tstart = (double)clock()/CLOCKS_PER_SEC;
	chal_10 = V->round_10(file_name, c, C);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_v+=ttime;

	tstop_t = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop_t-tstart_t;

	name = "shuffle_without_opti_P.txt";
	ost.open(name.c_str(),ios::app);
	ost<< time_p<<endl;
	ost.close();

	name = "shuffle_without_opti_V.txt";
	ost.open(name.c_str(),ios::app);
	ost<< time_v<<endl;
	ost.close();
//	ost << "The shuffle argument took " << ttime << " second(s)." << endl;
//	ost << "The prover needed " <<time_p<<" in total and " << "the verifier needed "<<time_v<<" in total"<<endl;
//	ost << "The commitments needed "<< time_cm<< "second(s)";
	delete P;
	delete V;
	return 1;
}

int shuffle_w_opti_me(vector<vector<Cipher_elg>* >* c, vector<vector<Cipher_elg>* >* C, vector<vector<ZZ>*>* R,vector<vector<vector<long>* >* > * pi, vector<long> num){
	Prover_me* P=0;
	Verifier_me* V=0;
	double tstart, tstart_t, tstop,tstop_t, ttime, time_p, time_v;
	ZZ chal_10,ans_12;
	string file_name, name;
	ofstream ost;
	P = new Prover_me(C,R,pi,num);
	V = new Verifier_me(num);

	time_p=0;
	time_v =0;
	tstart_t = (double)clock()/CLOCKS_PER_SEC;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_1();
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_2(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_3(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_4(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_5(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_6(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_7(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V-> round_8(file_name);
tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
		tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_9(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_p+=ttime;
		tstart = (double)clock()/CLOCKS_PER_SEC;
chal_10 = V->round_10(file_name, c, C);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_v+=ttime;
	tstop_t = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop_t-tstart_t;

	name = "shuffle_with_me_P.txt";
	ost.open(name.c_str(),ios::app);
	ost<< time_p<<endl;
	ost.close();

	name = "shuffle_with_me_V.txt";
	ost.open(name.c_str(),ios::app);
	ost<< time_v<<endl;
	ost.close();
/*	ost << "The optimized shuffle argument took " << ttime << " second(s)." << endl;
	ost << "The prover needed " <<time_p<<" in total and "<< "the verifier needed "<<time_v<<" in total"<<endl;
	ost << "The opt. commitments needed "<< time_cm<< "second(s)";
	ost.close();*/

	delete P;
	delete V;

	return 1;
}

int shuffle_w_opti(vector<vector<Cipher_elg>* >* c, vector<vector<Cipher_elg>* >* C, vector<vector<ZZ>*>* R,vector<vector<vector<long>* >* > * pi, vector<long> num, ZZ gen){
	Prover_fft* P=0;
	Verifier_me* V=0;
	double tstart, tstart_t, tstop,tstop_t, ttime, time_p, time_v;
	ZZ chal_10,ans_12;
	string file_name, name;
	ofstream ost;
	P = new Prover_fft(C,R,pi,num, gen);
	V = new Verifier_me(num);

	time_p=0;
	time_v =0;
	tstart_t = (double)clock()/CLOCKS_PER_SEC;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_1();
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_2(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_3(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_4(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_5(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_6(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_7(file_name);

	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V-> round_8(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;
		tstart = (double)clock()/CLOCKS_PER_SEC;

	file_name = P->round_9(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_p+=ttime;
		tstart = (double)clock()/CLOCKS_PER_SEC;
	chal_10 = V->round_10(file_name, c, C);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_v+=ttime;

	tstop_t = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop_t-tstart_t;

	name = "shuffle_with_FFT_P.txt";
	ost.open(name.c_str(),ios::app);
	ost<< time_p<<endl;
	ost.close();

	name = "shuffle_with_FFT_V.txt";
	ost.open(name.c_str(),ios::app);
	ost<< time_v<<endl;
	ost.close();
/*	ost << "The optimized shuffle argument took " << ttime << " second(s)." << endl;
	ost << "The prover needed " <<time_p<<" in total and "<< "the verifier needed "<<time_v<<" in total"<<endl;
	ost << "The opt. commitments needed "<< time_cm<< "second(s)";
	ost.close();*/

	delete P;
	delete V;

	return 1;
}

int shuffle_w_toom(vector<vector<Cipher_elg>* >* c, vector<vector<Cipher_elg>* >* C, vector<vector<ZZ>*>* R,vector<vector<vector<long>* >* > * pi, vector<long> num, ZZ gen){

	Prover_toom* P=0;
	Verifier_toom* V=0;
	double tstart, tstart_t, tstop,tstop_t, ttime, time_p, time_v;
	ZZ chal_10,ans_12;
	string file_name, name;
	ofstream ost;
	mu = 4;
	mu_h = 2*mu-1;
	m_r = num[1]/mu;
	P = new Prover_toom(C,R,pi,num, gen);
	V = new Verifier_toom(num);

	time_p=0;
	time_v =0;
	tstart_t = (double)clock()/CLOCKS_PER_SEC;

	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_1();
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;

	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_2(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;

	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_3(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;

	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_4(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;

	if(m_r ==4){
	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_5(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;

	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = V->round_6(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;

	tstart = (double)clock()/CLOCKS_PER_SEC;
file_name = P->round_7(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_p+=ttime;

	tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = V-> round_8(file_name);
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	time_v+=ttime;

		tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = P->round_9(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_p+=ttime;

		tstart = (double)clock()/CLOCKS_PER_SEC;
	chal_10 = V->round_10(file_name,c,C);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_v+=ttime;
	}
	else{
		tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = P->round_5_red(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_p+=ttime;

		tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = V->round_6_red(file_name,c);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_v+=ttime;

		m_r=m_r/mu;
	/*	while(m_r>mu){
			cout<<"This still needs of programming, but only happen if m=256";
			m_r=m_r/mu;
		}*/
		tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = P->round_5_red1(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_p+=ttime;

		tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = V->round_6_red1(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_v+=ttime;

		tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = P->round_7_red(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_p+=ttime;

		tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = V->round_8(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_v+=ttime;

		tstart = (double)clock()/CLOCKS_PER_SEC;
	file_name = P->round_9(file_name);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_p+=ttime;

		tstart = (double)clock()/CLOCKS_PER_SEC;
	chal_10 = V->round_10_red(file_name,c,C);
		tstop = (double)clock()/CLOCKS_PER_SEC;
		ttime= tstop-tstart;
		time_v+=ttime;
	}

	tstop_t = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop_t-tstart_t;

	name = "shuffle_with_toom_cook_P.txt";
	ost.open(name.c_str(),ios::app);
	ost<< time_p<<endl;
	ost.close();

	name = "shuffle_with_toom_cook_V.txt";
	ost.open(name.c_str(),ios::app);
	ost<< time_v<<endl;
	ost.close();
/*	ost << "The optimized shuffle argument took " << ttime << " second(s)." << endl;
	ost << "The prover needed " <<time_p<<" in total and "<< "the verifier needed "<<time_v<<" in total"<<endl;
	ost << "The opt. commitments  "<< time_cm<< "second(s)";
	ost.close();*/

	delete P;
	delete V;

	return 1;
}


// This file is part of PoseEstimation.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef FIVE_POINT_UTIL_HPP_
#define FIVE_POINT_UTIL_HPP_

#include <Eigen/Dense>

#include <random>
#include <vector>

namespace FivePoint
{

template <typename FloatType, typename RNG>
void getRandomPose(RNG &rng, FloatType *R, FloatType *t)
{
    using std::cos;
    using std::sin;
	std::uniform_real_distribution<FloatType> urd(0, static_cast<FloatType>(M_PI));
	FloatType phi   = 2 * urd(rng);
	FloatType theta = urd(rng);
	FloatType psi   = 2 * urd(rng);

	Eigen::Map<Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> >mR(R);

	mR(0, 0) = cos(psi) * cos(phi) - cos(theta) * sin(phi) * sin(psi);
	mR(0, 1) = cos(psi) * sin(phi) + cos(theta) * cos(phi) * sin(psi);
	mR(0, 2) = sin(psi) * sin(theta);
	mR(1, 0) = -sin(psi) * cos(phi) - cos(theta) * sin(phi) * cos(psi);
	mR(1, 1) = -sin(psi) * sin(phi) + cos(theta) * cos(phi) * cos(psi);
	mR(1, 2) = cos(psi) * sin(theta);
	mR(2, 0) = sin(theta) * sin(phi);
	mR(2, 1) = -sin(theta) * cos(phi);
	mR(2, 2) = cos(theta);
	t[0] = 0.0;
	t[1] = 0.0;
	t[2] = 6.0;
}

template <typename FloatType, typename RNG>
void getRandomPoints(RNG &rng, int n, std::vector<FloatType> &p)
{
    using std::cos;
    using std::sin;
	const FloatType pi = static_cast<FloatType>(M_PI);
	std::uniform_real_distribution<FloatType> urd(0, 1);

	p.resize(0);
	p.reserve(3 * n);
	for (int i = 0; i < n; i++)
	{
		FloatType theta = pi * urd(rng), phi = 2 * pi * urd(rng), R = 2 * urd(rng);
		FloatType X =  sin(theta) * sin(phi) * R;
		FloatType Y = -sin(theta) * cos(phi) * R;
		FloatType Z =  cos(theta) * R;

		p.push_back(X);
		p.push_back(Y);
		p.push_back(Z);
	}
}

template <typename FloatType>
void getRandomCorrespondences(int n, FloatType *R, FloatType *t, std::vector<FloatType> &pts1, std::vector<FloatType> &pts2)
{
	std::random_device rd;
	std::mt19937 rng(rd());
	FloatType R1[9], R2[9], t1[3], t2[3];
	getRandomPose(rng, R1, t1);
	getRandomPose(rng, R2, t2);

	std::vector<FloatType> pts;
	getRandomPoints(rng, n, pts);

	Eigen::Map<Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic> > mP(&(pts[0]), 3, n);
	Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> mR1(R1);
	Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> mR2(R2);
	Eigen::Matrix<FloatType, 3, 1> mt1(t1);
	Eigen::Matrix<FloatType, 3, 1> mt2(t2);

	Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic> mP1(3, n);
	mP1 = (mR1 * mP).colwise() + mt1;
	pts1.resize(2*n);
	Eigen::Map<Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic> > mp1(&(pts1[0]), 2, n);
	mp1.row(0) = mP1.row(0).array() / mP1.row(2).array();
	mp1.row(1) = mP1.row(1).array() / mP1.row(2).array();

	Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic> mP2(3, n);
	mP2 = (mR2 * mP).colwise() + mt2;
	pts2.resize(2*n);
	Eigen::Map<Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic> > mp2(&(pts2[0]), 2, n);
	mp2.row(0) = mP2.row(0).array() / mP2.row(2).array();
	mp2.row(1) = mP2.row(1).array() / mP2.row(2).array();

	Eigen::Map<Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> > mR(R);
	Eigen::Map<Eigen::Matrix<FloatType, 3, 1> > mt(t);
	mR = mR2*mR1.transpose();
	mt = mt2 - mR*mt1;
}
}

#endif // FIVE_POINT_UTIL_HPP_


///3
// ALGOLAB BGL Tutorial 3
// Flow example demonstrating
// - breadth first search (BFS) on the residual graph

// Compile and run with one of the following:
// g++ -std=c++11 -O2 bgl_residual_bfs.cpp -o bgl_residual_bfs ./bgl_residual_bfs
// g++ -std=c++11 -O2 -I path/to/boost_1_58_0 bgl_residual_bfs.cpp -o bgl_residual_bfs; ./bgl_residual_bfs

// Includes
// ========
// STL includes
#include <iostream>
#include <algorithm>
#include <vector>
#include <queue>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/tuple/tuple.hpp>

// BGL graph definitions
// =====================
// Graph Type with nested interior edge properties for Flow Algorithms
typedef  boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
  boost::property<boost::edge_capacity_t, long,
    boost::property<boost::edge_residual_capacity_t, long,
      boost::property<boost::edge_reverse_t, traits::edge_descriptor> > > >  graph;
// Interior Property Maps
typedef traits::vertex_descriptor vertex_desc;

typedef  boost::graph_traits<graph>::edge_descriptor      edge_desc;
typedef  boost::graph_traits<graph>::out_edge_iterator      out_edge_it;

// Custom Edge Adder Class, that holds the references
// to the graph, capacity map and reverse edge map
// ===================================================
class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

void add_if_present(edge_adder & adder, std::vector<int> &present, int n, int u, int i, int j, int c) {
  if(i >= 0 && j >= 0 && i <= (n-1) && j <= (n-1)) {
    if(present[i * n + j]) {
      adder.add_edge(u, i * n + j, c);
    }
  }
}

// Main
void testcase() {
  // build graph
  int n;
  std::cin >> n;
  graph G(n * n);
  edge_adder adder(G);
  // auto rc_map = boost::get(boost::edge_residual_capacity, G);
  const vertex_desc v_source = boost::add_vertex(G);
  const vertex_desc v_sink = boost::add_vertex(G);
  std::vector<int> present(n*n);

  int sum_present = 0;
  for(int i = 0; i < n; i++) {
    for(int j = 0; j < n; j++) {
      std::cin >> present[i * n + j];
      if(present[i * n + j]) {
        sum_present++;
        if((i + j) % 2 == 0)
          adder.add_edge(v_source, i * n + j, 1);
        else
          adder.add_edge(i * n + j, v_sink, 1);
      }
    }
  }

  for(int i = 0; i < n; i++) {
    for(int j = 0; j < n; j++) {
      int ij = i * n + j;
      if(present[ij] && (i + j) % 2 == 0) {
        add_if_present(adder, present, n, ij, (i - 1), j - 2, 1);
        add_if_present(adder, present, n, ij, (i - 1), j + 2, 1);
        add_if_present(adder, present, n, ij, (i + 1), j - 2, 1);
        add_if_present(adder, present, n, ij, (i + 1), j + 2, 1);
        add_if_present(adder, present, n, ij, (i - 2), j - 1, 1);
        add_if_present(adder, present, n, ij, (i - 2), j + 1, 1);
        add_if_present(adder, present, n, ij, (i + 2), j - 1, 1);
        add_if_present(adder, present, n, ij, (i + 2), j + 1, 1);
      //   if(i > 0 && j > 1)
      //     adder.add_edge(ij, (i - 1) * n + j - 2, 1);
      //   if(i > 0 && j < (n - 2))
      //     adder.add_edge(ij, (i - 1) * n + j + 2, 1);
      //   if(i < (n - 1) && j > 1)
      //     adder.add_edge(ij, (i + 1) * n + j - 2, 1);
      //   if(i < (n - 1) && j < (n - 2))
      //     adder.add_edge(ij, (i + 1) * n + j + 2, 1);
      //   if(i > 1 && j > 0)
      //     adder.add_edge(ij, (i - 2) * n + j - 1, 1);
      //   if(i > 1 && j < (n - 1))
      //     adder.add_edge(ij, (i - 2) * n + j + 1, 1);
      //   if(i < (n - 2) && j > 0)
      //     adder.add_edge(ij, (i + 2) * n + j - 1, 1);
      //   if(i < (n - 2) && j < (n - 1))
      //     adder.add_edge(ij, (i + 2) * n + j + 1, 1);
      }
    }
  }

  // for(int i = 0; i < n; i++) {
  //   // adder.add_edge(v_source, i, 1);
  //   adder.add_edge(i, v_sink,  std::numeric_limits<int>::max());
  // }

  int flow = boost::push_relabel_max_flow(G, v_source, v_sink);
  // auto c_map = boost::get(boost::edge_capacity, G);
  // auto rc_map = boost::get(boost::edge_residual_capacity, G);
  // Find a min cut via maxflow
  std::cout << sum_present - flow << "\n";
  // std::cerr << std::endl;
  // // Retrieve the capacity map and reverse capacity map
  // const auto c_map = boost::get(boost::edge_capacity, G);
  // const auto rc_map = boost::get(boost::edge_residual_capacity, G);

  // // Iterate over all the edges to print the flow along them
  // auto edge_iters = boost::edges(G);
  // for (auto edge_it = edge_iters.first; edge_it != edge_iters.second; ++edge_it) {
  //   const edge_desc edge = *edge_it;
  //   const long flow_through_edge = c_map[edge] - rc_map[edge];
  //   std::cerr << "edge from " << boost::source(edge, G) << " to " << boost::target(edge, G)
  //             << " runs " << flow_through_edge
  //             << " units of flow (negative for reverse direction). \n";
  // }
}

int main() {
  std::ios_base::sync_with_stdio(false);
  std::size_t t;
  for (std::cin >> t; t > 0; --t) testcase();
  return 0;
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2004 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * SLAM : Simultaneous Locatization and Mapping
 *  Kalman filter representing representation of SLAM
 */

		// Bayes++ Bayesian filtering schemes
#include "BayesFilter/bayesFlt.hpp"
		// Bayes++ SLAM
#include "SLAM.hpp"
#include "kalmanSLAM.hpp"
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

namespace SLAM_filter
{

template <class Base>
inline void zero(FM::ublas::matrix_range<Base> A)
// Zero a matrix_range
{	// Note A cannot be a reference
	typedef typename Base::value_type Base_value_type;
	FM::noalias(A) = FM::ublas::scalar_matrix<Base_value_type>(A.size1(),A.size2(), Base_value_type());
}

Kalman_SLAM::Kalman_SLAM( Kalman_filter_generator& filter_generator ) :
	SLAM(),
	fgenerator(filter_generator),
	loc(0), full(0)
{
	nL = 0;
	nM = 0;
}

Kalman_SLAM::~Kalman_SLAM()
{
	fgenerator.dispose (loc);
	fgenerator.dispose (full);
}

void Kalman_SLAM::init_kalman (const FM::Vec& x, const FM::SymMatrix& X)
{
	// TODO maintain map states
	nL = x.size();
	nM = 0;
	if (loc) fgenerator.dispose (loc);
	if (full) fgenerator.dispose (full);
		// generate a location filter for prediction
	loc = fgenerator.generate(nL);
		// generate full filter
	full = fgenerator.generate(nL);
		// initialise location states
	full->x.sub_range(0,nL) = x;
	full->X.sub_matrix(0,nL,0,nL) = X;
	full->init();
}

void Kalman_SLAM::predict( BF::Linrz_predict_model& lpred )
{
		// extract location part of full
	loc->x = full->x.sub_range(0,nL);
	loc->X = full->X.sub_matrix(0,nL,0,nL);
		// predict location, independent of map
	loc->init();
	loc->predict (lpred);
	loc->update();
		// return location to full
	full->x.sub_range(0,nL) = loc->x;
	full->X.sub_matrix(0,nL,0,nL) = loc->X;
	full->init();
}

void Kalman_SLAM::observe( unsigned feature, const Feature_observe& fom, const FM::Vec& z )
{
	// Assume features added sequentially
	if (feature >= nM) {
		error (BF::Logic_exception("Observe non existing feature"));
		return;
	}
	// TODO Implement nonlinear form
	// Create a augmented sparse observe model for full states
	BF::Linear_uncorrelated_observe_model fullm(full->x.size(), 1);
	fullm.Hx.clear();
	fullm.Hx.sub_matrix(0,nL, 0,nL) = fom.Hx.sub_matrix(0,nL, 0,nL);
	fullm.Hx(0,nL+feature) = fom.Hx(0,nL);
	fullm.Zv = fom.Zv;
	full->observe(fullm, z);
}

void Kalman_SLAM::observe_new( unsigned feature, const Feature_observe_inverse& fom, const FM::Vec& z )
// fom: must have a the special form required for SLAM::obeserve_new
{
		// size consistency, single state feature
	if (fom.Hx.size1() != 1)
		error (BF::Logic_exception("observation and model size inconsistent"));

		// make new filter with additional (uninitialized) feature state
	if (feature >= nM)
	{
		nM = feature+1;
		Kalman_filter_generator::Filter_type* nf = fgenerator.generate(nL+nM);
		FM::noalias(nf->x.sub_range(0,full->x.size())) = full->x;
		FM::noalias(nf->X.sub_matrix(0,full->x.size(),0,full->x.size())) = full->X;

		fgenerator.dispose(full);
		full = nf;
	}
		// build augmented location and observation
	FM::Vec sz(nL+z.size());
	sz.sub_range(0,nL) = full->x.sub_range(0,nL);
	sz.sub_range(nL,nL+z.size() )= z;

	// TODO use named references rather then explict Ha Hb
	FM::Matrix Ha (fom.Hx.sub_matrix(0,1, 0,nL) );
	FM::Matrix Hb (fom.Hx.sub_matrix(0,1, nL,nL+z.size()) );
	FM::Matrix tempHa (1,nL);
	FM::Matrix tempHb (1,sz.size());

		// feature covariance with existing location and features
        // X+ = [0 Ha] X [0 Ha]' + Hb Z Hb'
        // - zero existing feature covariance
	zero( full->X.sub_matrix(0,full->X.size1(), nL+feature,nL+feature+1) );
	full->X.sub_matrix(nL+feature,nL+feature+1,0,nL+nM) = FM::prod(Ha,full->X.sub_matrix(0,nL, 0,nL+nM) );
		// feature state and variance
	full->x[nL+feature] = fom.h(sz)[0];
	full->X(nL+feature,nL+feature) = ( FM::prod_SPD(Ha,full->X.sub_matrix(0,nL, 0,nL),tempHa) +
													  FM::prod_SPD(Hb,fom.Zv,tempHb)
													 ) (0,0);

	full->init ();
}

void Kalman_SLAM::observe_new( unsigned feature, const FM::Float& t, const FM::Float& T )
{
		// Make space in scheme for feature, requires the scheme can deal with resized state
	if (feature >= nM)
	{
		Kalman_filter_generator::Filter_type* nf = fgenerator.generate(nL+feature+1);
		FM::noalias(nf->x.sub_range(0,full->x.size())) = full->x;
		FM::noalias(nf->X.sub_matrix(0,full->x.size(),0,full->x.size())) = full->X;
		zero( nf->X.sub_matrix(0,nf->X.size1(), nL+nM,nf->X.size2()) );

		nf->x[nL+feature] = t;
		nf->X(nL+feature,nL+feature) = T;
		nf->init ();
		fgenerator.dispose(full);
		full = nf;
		nM = feature+1;
	}
	else
	{
		full->x[nL+feature] = t;
		full->X(nL+feature,nL+feature) = T;
		full->init ();
	}
}

void Kalman_SLAM::forget( unsigned feature, bool must_exist )
{
	full->x[nL+feature] = 0.;
			// ISSUE uBLAS has problems accessing the lower symmetry via a sub_matrix proxy, there two two parts seperately
	zero( full->X.sub_matrix(0,nL+feature, nL+feature,nL+feature+1) );
	zero( full->X.sub_matrix(nL+feature,nL+feature+1, nL+feature,full->X.size1()) );
	full->init();
}

void Kalman_SLAM::decorrelate( Bayesian_filter::Bayes_base::Float d )
// Reduce correlation by scaling cross-correlation terms
{
	std::size_t i,j;
	const std::size_t n = full->X.size1();
	for (i = 1; i < n; ++i)
	{
		FM::SymMatrix::Row Xi(full->X,i);
		for (j = 0; j < i; ++j)
		{
			Xi[j] *= d;
		}
		for (j = i+1; j < n; ++j)
		{
			Xi[j] *= d;
		}
	}
	full->init();
}

}//namespace SLAM


#include <Eigen/Dense>
#include <iostream>

using namespace std;
using namespace Eigen;

typedef Matrix<float, 1, Dynamic> MatrixType;
typedef Map<MatrixType> MapType;
typedef Map<const MatrixType> MapTypeConst; // a read-only map
const int n_dims = 5;

void main() {
	{
		int arr[8];
		for (int i = 0; i < 8; i++) {
			arr[i] = i;
		}

		cout << "Column-major: \n" << Map<Matrix<int, 2, 4>>(arr) << endl;
		cout << "Row-major: \n" << Map<Matrix<int, 2, 4, RowMajor>>(arr) << endl;
		cout << "Row-major using stride: \n" << Map<Matrix<int, 2, 4>, Unaligned, Stride<1, 4>>(arr) << endl;
	}

	{
		MatrixType m1(n_dims), m2(n_dims);
		m1.setRandom();
		m2.setRandom();

		float * p = &m2(0); // get the addres storing the data for m2
		MapType m2map(p, m2.size()); //m2map shares data with m2
		MapTypeConst m2mapconst(p, m2.size()); // a read-only accessor for m2

		cout << "m1: \n" << m1 << endl;
		cout << "m2: \n" << m2 << endl;
		cout << "Squared euclidean distance: \n" << (m1 - m2).squaredNorm() << endl;
		cout << "Squared euclidean distance, using map: \n" << (m1 - m2map).squaredNorm() << endl;
		m2map(3) = 7; // this will change m2, since they share the same array
		cout << "Updated m2: \n" << m2 << endl;
		cout << "m2 coefficient 2, constant accessor: \n" << m2mapconst(2) << endl;

	}

	{
		// changing the mapped array
		int data[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9 };
		Map<RowVectorXi> v(data, 4);
		cout << "The mapped vector v is : " << v << endl;
		new(&v) Map<RowVectorXi>(data + 4, 5);
		cout << "Now v is : " << v << endl;
	}

	{
		// declare a Map obj without first knowing the mapped array's location in memory
		//int n_matrices = 4;
		//Map<Matrix3f> A(NULL);
		//VectorXf b(n_matrices);
		//for (int i = 0; i < n_dims; i++) {
		//	new (&A) Map<Matrix3f>(get_matrix_pointer(i));
		//	b(i) = A.trace();
		//}
	}

	system("pause");
}


// Copyright Louis Dionne 2016
// Distributed under the Boost Software License, Version 1.0.

#include <boost/mpl/equal.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/vector_c.hpp>
namespace mpl = boost::mpl;

// sample(dimensions)
// base dimensions                     M  L  T  I  Θ  J  N
using mass        = mpl::vector_c<int, 1, 0, 0, 0, 0, 0, 0>;
using length      = mpl::vector_c<int, 0, 1, 0, 0, 0, 0, 0>;
using time        = mpl::vector_c<int, 0, 0, 1, 0, 0, 0, 0>;
using charge      = mpl::vector_c<int, 0, 0, 0, 1, 0, 0, 0>;
using temperature = mpl::vector_c<int, 0, 0, 0, 0, 1, 0, 0>;
using intensity   = mpl::vector_c<int, 0, 0, 0, 0, 0, 1, 0>;
using amount      = mpl::vector_c<int, 0, 0, 0, 0, 0, 0, 1>;

// composite dimensions
using velocity     = mpl::vector_c<int, 0, 1, -1, 0, 0, 0, 0>; // L/T
using acceleration = mpl::vector_c<int, 0, 1, -2, 0, 0, 0, 0>; // L/T²
using force        = mpl::vector_c<int, 1, 1, -2, 0, 0, 0, 0>; // ML/T²
// end-sample

// sample(dimensions-compare)
template <typename A, typename B>
constexpr bool compatible = mpl::equal<A, B>::value;
// end-sample

// sample(quantity)
template <typename Dimensions>
struct quantity {
  double value_;

  explicit quantity(double v) : value_(v) { }

  template <typename OtherDimensions>
  explicit quantity(quantity<OtherDimensions> other)
    : value_(other.value_)
  {
    static_assert(compatible<Dimensions, OtherDimensions>,
      "Constructing quantities with incompatible dimensions!");
  }

  explicit operator double() const { return value_; }
};
// end-sample

// sample(dimensions-compose)
template <typename D1, typename D2>
auto operator*(quantity<D1> a, quantity<D2> b) {
  using D = typename mpl::transform<D1, D2, mpl::plus<>>::type;
  return quantity<D>{static_cast<double>(a) * static_cast<double>(b)};
}

template <typename D1, typename D2>
auto operator/(quantity<D1> a, quantity<D2> b) {
  using D = typename mpl::transform<D1, D2, mpl::minus<>>::type;
  return quantity<D>{static_cast<double>(a) / static_cast<double>(b)};
}

template <typename D1, typename D2>
auto operator+(quantity<D1> a, quantity<D2> b) {
  static_assert(compatible<D1, D2>,
    "Adding quantities with incompatible dimensions!");
  return quantity<D1>{static_cast<double>(a) + static_cast<double>(b)};
}

// etc..
// end-sample

#if 0
// sample(usage)
quantity<mass>         m{10.3};
quantity<length>       d{3.6};
quantity<time>         t{2.4};
quantity<velocity>     v{d / t};
quantity<acceleration> a{...};
quantity<force>        f{m * v}; // Compiler error!
quantity<force>        f{m * a}; // Works as expected
// end-sample
#endif

int main() {
  quantity<mass>         m{10.3};
  quantity<length>       d{3.6};
  quantity<time>         t{2.4};
  quantity<velocity>     v{d / t};
  quantity<acceleration> a{3.9};
  quantity<force>        f{m * a};

  quantity<force> f2 = f + f;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <Eigen/Dense>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 10000; ++i){
        auto reader = vtkSmartPointer<vtkPolyDataReader>::New();
        reader->SetFileName("Bad.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = (double*) poly->GetPoints()->GetData()->GetVoidPointer(0);
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Reset the center of the sphere to origin by translating
        Vector3d center = points.rowwise().mean();
        points = points.colwise() - center;

        // Rotate all points so that the point in 0th column is along z-axis
        Vector3d c = points.col(0);
        double_t cos_t = c(2);
        double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
        Vector3d axis;
        axis << c(1), -c(0), 0.;
        Matrix3d rotMat, axis_cross, outer;
        axis_cross << 0. , -axis(2), axis(1),
                        axis(2), 0., -axis(0),
                        -axis(1), axis(0), 0.;

        outer.noalias() = axis*axis.transpose();

        rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1-cos_t)*outer;
        Matrix3Xd rPts(3,N);
        rPts = rotMat*points; // The points on a sphere rotated

        // Calculate the stereographic projections
        Vector3d p0;
        Map3Xd l0( &(rPts(0,1)), 3, N-1 );
        Matrix3Xd l(3,N-1), proj(3,N-1);
        p0 << 0,0,-1;
        c = rPts.col(0);
        l = (l0.colwise() - c).colwise().normalized();
        for( auto j=0; j < N-1; ++j ){
            proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
        }
        // Insert the projected points in a CGAL vertex_with_info vector
        std::vector< std::pair< Point, unsigned> > verts;
        for( auto j=0; j < N-1; ++j ){
            verts.push_back(std::make_pair(Point(proj(0,j),proj(1,j)),j+1));
        }

        Delaunay dt( verts.begin(), verts.end() );

        // Write the finite faces of the triangulation to a VTK file
        vtkNew<vtkCellArray> triangles;
        for( auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end(); ++ffi){
            triangles->InsertNextCell(3);
            for(auto j=2; j >= 0; --j)
                triangles->InsertCellPoint(ffi->vertex(j)->info());
        }

        // Iterate over infinite faces
        Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done(fc);
        if (fc != 0) {
            do{
                triangles->InsertNextCell(3);
                for(auto j=2; j >= 0; --j){
                    auto vh = fc->vertex(j);
                    auto id = dt.is_infinite(vh)? 0 : vh->info();
                    triangles->InsertCellPoint(id);
                }
            }while(++fc != done);
        }
        poly->SetPolys(triangles);

        // Write to VTK file
        vtkNew<vtkPolyDataWriter> writer;
        writer->SetFileName("StereoMesh.vtk");
        writer->SetInputData(poly);
        writer->Write();
    }
    float diff((float)clock() - (float)t1);
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PYTHAGORAS_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PYTHAGORAS_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

#include <boost/geometry/core/access.hpp>

#include <boost/geometry/strategies/distance.hpp>

#include <boost/geometry/util/calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <size_t I, typename T>
struct compute_pythagoras
{
    template <typename Point1, typename Point2>
    static inline T apply(Point1 const& p1, Point2 const& p2)
    {
        T const c1 = boost::numeric_cast<T>(get<I-1>(p1));
        T const c2 = boost::numeric_cast<T>(get<I-1>(p2));
        T const d = c1 - c2;
        return d * d + compute_pythagoras<I-1, T>::apply(p1, p2);
    }
};

template <typename T>
struct compute_pythagoras<0, T>
{
    template <typename Point1, typename Point2>
    static inline T apply(Point1 const&, Point2 const&)
    {
        return boost::numeric_cast<T>(0);
    }
};

}
#endif // DOXYGEN_NO_DETAIL

namespace comparable
{

/*!
\brief Strategy to calculate comparable distance between two points
\ingroup strategies
\tparam Point1 \tparam_first_point
\tparam Point2 \tparam_second_point
\tparam CalculationType \tparam_calculation
*/
template <typename CalculationType = void>
class pythagoras
{
public :

    template <typename Point1, typename Point2>
    struct calculation_type
        : util::calculation_type::geometric::binary
          <
              Point1,
              Point2,
              CalculationType
          >
    {};

    template <typename Point1, typename Point2>
    static inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& p1, Point2 const& p2)
    {
        BOOST_CONCEPT_ASSERT( (concept::ConstPoint<Point1>) );
        BOOST_CONCEPT_ASSERT( (concept::ConstPoint<Point2>) );

        // Calculate distance using Pythagoras
        // (Leave comment above for Doxygen)

        assert_dimension_equal<Point1, Point2>();

        return detail::compute_pythagoras
            <
                dimension<Point1>::value,
                typename calculation_type<Point1, Point2>::type
            >::apply(p1, p2);
    }
};

} // namespace comparable

/*!
\brief Strategy to calculate the distance between two points
\ingroup strategies
\tparam CalculationType \tparam_calculation

\qbk{
[heading Notes]
[note Can be used for points with two\, three or more dimensions]
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}

*/
template
<
    typename CalculationType = void
>
class pythagoras
{
public :

    template <typename P1, typename P2>
    struct calculation_type
        : util::calculation_type::geometric::binary
          <
              P1,
              P2,
              CalculationType,
              double,
              double // promote integer to double
          >
    {};

    /*!
    \brief applies the distance calculation using pythagoras
    \return the calculated distance (including taking the square root)
    \param p1 first point
    \param p2 second point
    */
    template <typename P1, typename P2>
    static inline typename calculation_type<P1, P2>::type
    apply(P1 const& p1, P2 const& p2)
    {
        // The cast is necessary for MSVC which considers sqrt __int64 as an ambiguous call
        return std::sqrt
            (
                 boost::numeric_cast<typename calculation_type<P1, P2>::type>
                    (
                        comparable::pythagoras<CalculationType>::apply(p1, p2)
                    )
            );
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType>
struct tag<pythagoras<CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename CalculationType, typename P1, typename P2>
struct return_type<distance::pythagoras<CalculationType>, P1, P2>
    : pythagoras<CalculationType>::template calculation_type<P1, P2>
{};

template <typename CalculationType>
struct comparable_type<pythagoras<CalculationType> >
{
    typedef comparable::pythagoras<CalculationType> type;
};

template <typename CalculationType>
struct get_comparable<pythagoras<CalculationType> >
{
    typedef comparable::pythagoras<CalculationType> comparable_type;
public :
    static inline comparable_type apply(pythagoras<CalculationType> const& )
    {
        return comparable_type();
    }
};

template <typename CalculationType, typename Point1, typename Point2>
struct result_from_distance<pythagoras<CalculationType>, Point1, Point2>
{
private :
    typedef typename return_type<pythagoras<CalculationType>, Point1, Point2>::type return_type;
public :
    template <typename T>
    static inline return_type apply(pythagoras<CalculationType> const& , T const& value)
    {
        return return_type(value);
    }
};

// Specializations for comparable::pythagoras
template <typename CalculationType>
struct tag<comparable::pythagoras<CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename CalculationType, typename P1, typename P2>
struct return_type<comparable::pythagoras<CalculationType>, P1, P2>
    : comparable::pythagoras<CalculationType>::template calculation_type<P1, P2>
{};

template <typename CalculationType>
struct comparable_type<comparable::pythagoras<CalculationType> >
{
    typedef comparable::pythagoras<CalculationType> type;
};

template <typename CalculationType>
struct get_comparable<comparable::pythagoras<CalculationType> >
{
    typedef comparable::pythagoras<CalculationType> comparable_type;
public :
    static inline comparable_type apply(comparable::pythagoras<CalculationType> const& )
    {
        return comparable_type();
    }
};

template <typename CalculationType, typename Point1, typename Point2>
struct result_from_distance<comparable::pythagoras<CalculationType>, Point1, Point2>
{
private :
    typedef typename return_type<comparable::pythagoras<CalculationType>, Point1, Point2>::type return_type;
public :
    template <typename T>
    static inline return_type apply(comparable::pythagoras<CalculationType> const& , T const& value)
    {
        return_type const v = value;
        return v * v;
    }
};

template <typename Point1, typename Point2>
struct default_strategy<point_tag, Point1, Point2, cartesian_tag, cartesian_tag, void>
{
    typedef pythagoras<> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_DISTANCE_PYTHAGORAS_HPP


#pragma once

#include <memory>
#include <Eigen/Dense>
#include <boost/math/distributions/normal.hpp>

namespace calotypes
{

/*! \brief Base interface for functions that operate on pairs of data. It must be
 * that kernel(a,b) = kernel(b,a) */
template <class Data>
class KernelFunction
{
public:

	typedef std::shared_ptr<KernelFunction> Ptr;
	typedef Data DataType;

	KernelFunction() {}

	/*! \brief Calculates k(a,b) = k(a - b) */
	virtual double Evaluate( const Data& a, const Data& b ) const
	{
		return Evaluate( Difference( a, b ) );
	}

	/*! \brief Calculates a - b */
	virtual double Difference( const Data& a, const Data& b ) const = 0;

	/*! \brief Calculates k(x) */
	virtual double Evaluate( double x ) const { return x; }

};

/*! \brief Pushes a kernel through a Gaussian PDF. Uses boost's implementation. */
template <class Data>
class GaussianKernelAdaptor
: public KernelFunction<Data>
{
public:

	typedef std::shared_ptr<GaussianKernelAdaptor> Ptr;

	/*! \brief Construct a Gaussian kernel with specified standard deviation. */
	GaussianKernelAdaptor( typename KernelFunction<Data>::Ptr& k, double s )
	: kernel( k ), normal( 0, s ) {}

	virtual double Difference( const Data& a, const Data& b ) const
	{
		return kernel->Difference( a, b );
	}

	virtual double Evaluate( double x ) const
	{
		return boost::math::pdf( normal, x );
	}

protected:

	typename KernelFunction<Data>::Ptr kernel;
	boost::math::normal_distribution<double> normal;
};

/*! \brief Computes the positive-definite kernel (Gram) matrix for a given kernel.
 * Assumes kernel(a,a) = 0 and kernel(a,b) = kernel(b,a). */
template <class Data>
void ComputeGramMatrix( const KernelFunction<Data>& kernel, const std::vector<Data>& data,
						Eigen::MatrixXf& K )
{
	unsigned int N = data.size();
	K = Eigen::MatrixXf( N, N );

	for( unsigned int i = 0; i < N; i++ )
	{
		K(i,i) = kernel.Evaluate( data[i], data[i] );
	}
	for( unsigned int i = 0; i < N-1; i++ )
	{
		for( unsigned int j = i+1; j < N; j++ )
		{
			K(i,j) = kernel.Evaluate( data[i], data[j] );
			K(j,i) = K(i,j);
		}
	}
}

} // end namespace calotypes


#include <iostream>
#include <sstream>
#include <vector>
#include <string>
#include <cmath>
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/variance.hpp>

using namespace std;

#include "cDataPoint.h"
#include "KMeans.h"

void KMeans::Add( const vector<double>& p )
{
    if( myLocations.size() )
    {
        if( myLocations[0]->myDim != p.size() )
        {
            throw std::runtime_error("Inconsistent data attribute count");
        }
    }
    myLocations.push_back( dp_t( new cDataPoint( p ) ) );
}

    void KMeans::Add( cDataPoint& p )
    {
        Add( p.d );
    }

double KMeans::TotalDistanceToCluster()
{
    double total = 0;
    for( int kc = 0; kc < myClusterCount; kc++ )
    {
        for( int kl = 0; kl < (int)myLocations.size(); kl++ )
        {
            if( myAssigns[kl] == kc )
            {
                // add distance from location to cluster center it is assigned to
                total += myClusters[kc].dist( myLocations[ kl ] );
            }
        }
    }
    return total;
}

std::string KMeans::text()
{
    stringstream ss;
    int kc = 0;
    for( auto& c : myClusters )
    {
        ss << "Cluster " << kc << " : ";
        for( int k = 0; k < (int)myLocations.size(); k++ )
            if( myAssigns[k] == kc )
                ss << myLocations[k]->text() << ", ";
        ss << "\n" << ClusterStats( kc ) << "\n";
        kc++;
    }
    return ss.str();
}

void KMeans::Assign()
{
    myAssigns.clear();
    for( auto& c : myClusters )
        c.clear();

    for( auto& cl : myLocations )
    {
        //cout << "assigning " << cl.Text() << "\n";
        double m = 1000000000000000000000.0;
        int closest_cluster;
        for( int si = 0; si < myClusterCount; si++ )
        {
            //cout << "test cluster " << myClusters[ si ].text() << "\n";
            double td = myClusters[ si ].dist( cl );
            if( td < m )
            {
                m = td;
                closest_cluster = si;
            }
        }
        myAssigns.push_back( closest_cluster );
        myClusters[ closest_cluster ].add( cl );
    }
}

void KMeans::MoveClustersToMean()
{
    for( int ks = 0; ks < myClusterCount; ks++ )
    {
        cDataPoint A( myLocations[0]->myDim );
        int count = 0;
        for( int kl = 0; kl < (int)myLocations.size(); kl++ )
        {
            if( ks == myAssigns[ kl ] )
            {
                A = A + *myLocations[ kl ].get();
                count++;
            }
        }
        //cout << "cluster " << ks << " has "<< count << " " << A.Text() << "\n";
        if( count )
            myClusters[ ks ].move( A / count );
    }
}

void KMeans::Init(
    int clusterCount,
    bool frandom )
{
    myClusterCount = clusterCount;

    if( myClusterCount > (int)myLocations.size() )
        throw std::runtime_error("KMeans::ClusterCount less locations than clusters");

    if( frandom )
        ClusterLocationInitRandom();
    else
        ClusterLocationInitIndex();
}
void KMeans::ClusterLocationInitIndex()
{
    myClusters.clear();
    for( int k = 0; k < myClusterCount; k++ )
    {
        int ic = k * (double)myLocations.size()/myClusterCount;
        //cout << ic <<" "<< myLocations[ic].Text() << ", ";
        myClusters.push_back( cCluster( *myLocations[ic].get() ) );
    }
}
void KMeans::ClusterLocationInitRandom()
{
    /* initialize random seed: */
    srand (time(NULL));
    myClusters.clear();
    for( int k = 0; k < myClusterCount; k++ )
    {
        myClusters.push_back( *myLocations[ rand() % myLocations.size() ].get() );
    }
}

void KMeans::Iter( int max )
{
    double dt = 0;

    for( int kiter=0; kiter < max; kiter++ )
    {
        // assign locations to nearest cluster
        Assign();

        // move clusters to mean location of locations assigned
        MoveClustersToMean();

        double d = TotalDistanceToCluster();
        if( fabs( dt - d ) < .000001 )
            break;
    }
}

std::string KMeans::ClusterStats( int cluster )
{
    std::stringstream ss;
    ss << "Cluster " << cluster << " means ";
    if( 0 > cluster || cluster > (int)myClusters.size())
        return ss.str();
    ss << myClusters[ cluster ].text();

    typedef boost::accumulators::accumulator_set<double, boost::accumulators::stats<
    boost::accumulators::tag::min,
          boost::accumulators::tag::max,
          boost::accumulators::tag::mean,
          boost::accumulators::tag::variance,
          boost::accumulators::tag::count> >
          Accumulator_t;
    vector< Accumulator_t > vac( myLocations[0]->myDim );

    for( int k = 0; k < (int)myLocations.size(); k++ )
    {
        if( cluster == myAssigns[ k ] )
        {
            for( int kd = 0; kd < (int)myLocations[0]->myDim; kd++ )
            {
                vac[ kd ]( myLocations[k]->d[kd] );
            }
        }
    }

    ss << " mins: ";
    for( int kd = 0; kd < (int)myLocations[0]->myDim; kd++ )
    {
        ss << boost::accumulators::min( vac[kd] ) << ", ";
    }
    ss << " maxs: ";
    for( int kd = 0; kd < (int)myLocations[0]->myDim; kd++ )
    {
        ss << boost::accumulators::max( vac[kd] ) << ", ";
    }
    ss << " sds: ";
    for( int kd = 0; kd < (int)myLocations[0]->myDim; kd++ )
    {
        ss << sqrt(boost::accumulators::variance( vac[kd] )) << ", ";
    }
    ss << "\n";

    return ss.str();
}

std::vector< cCluster >&
KMeans::clusters()
{
    return myClusters;
}

double cCluster::dist( dp_t o )
{
    return cDataPoint::dist( myCenter, *o.get() );
}
std::string cCluster::text() const
{
    return myCenter.text();
}
void cCluster::move( const cDataPoint& r )
{
    myCenter = r;
}
cCluster::cCluster( const cDataPoint& r )
{
    myCenter = r;
}
void cCluster::clear()
{
    myPoints.clear();
}
void cCluster::add( dp_t p )
{
    myPoints.push_back( p );
}
std::vector< dp_t >& cCluster::points()
{
    return myPoints;
}
cDataPoint& cCluster::center()
{
    return myCenter;
}