#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();
}


#include <catch2/catch.hpp>

#include <array>

#include <units/isq/si/speed.h>
#include <units/isq/si/time.h>
#include <units/isq/si/length.h>
#include <units/isq/si/area.h>
#include <units/generic/dimensionless.h>

#include <boost/hana/tuple.hpp>

#include <codys/codys.hpp>

using PositionTag = class Position_;
using PositionUnit = units::isq::si::length<units::isq::si::metre>;
using PositionUnitSq = units::isq::si::area<units::isq::si::square_metre>;
using Dimensionless = units::dimensionless<units::one>;
using Position = codys::State<PositionTag, PositionUnit>;
using VelocityUnit = units::isq::si::speed<units::isq::si::metre_per_second>;
using Velocity = codys::State<class Vel_, VelocityUnit>;

using BasicMotions = codys::System<Position, Velocity>;

TEST_CASE("System return right state indices", "[System]")
{
  STATIC_REQUIRE(BasicMotions::idx_of<Position>() == 0);
  STATIC_REQUIRE(BasicMotions::idx_of<Velocity>() == 1);
}

TEST_CASE("State gives self as depends_on", "[State]")
{
  STATIC_REQUIRE(std::is_same_v<Position::depends_on, boost::hana::tuple<Position>>);
}

TEST_CASE("State has right unit as type", "[State]")
{
  STATIC_REQUIRE(std::is_same_v<Position::Unit, PositionUnit>);
}

TEST_CASE("Derivative has right operand as type", "[Derivative]")
{
  static constexpr auto derivative = codys::dot<Position>(Velocity{});
  STATIC_REQUIRE(std::is_same_v<decltype(derivative)::Operand, Position>);
}

TEST_CASE("Operator Plus yields sum of operands", "[Operator]")
{
  static constexpr auto plus = Position{} + Position{};
  static constexpr std::array values{1.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result = plus.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result == values[0] + values[0]);
}

TEST_CASE("Operator Plus has same unit as operands", "[Operator]")
{
  static constexpr auto plus = Position{} + Position{};
  STATIC_REQUIRE(std::is_same_v<decltype(plus)::Unit, PositionUnit>);
}

TEST_CASE("Operator Plus has concatination of denepends on", "[Operator]")
{
  static constexpr auto plus = Position{} + Position{};
  static constexpr auto ref_dependands = boost::hana::tuple<Position,Position>();
  STATIC_REQUIRE(std::is_same_v<decltype(plus)::depends_on, std::remove_cvref_t<decltype(ref_dependands)>>);
}

TEST_CASE("Operator Minus yields substraction of operands", "[Operator]")
{
  static constexpr auto minus = Position{} - Position{};
  static constexpr std::array values{1.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result = minus.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result == values[0] - values[0]);
}

TEST_CASE("Operator Minus has same unit as operands", "[Operator]")
{
  static constexpr auto minus = Position{} - Position{};
  STATIC_REQUIRE(std::is_same_v<decltype(minus)::Unit, PositionUnit>);
}

TEST_CASE("Operator Minus has concatination of denepends on", "[Operator]")
{
  static constexpr auto minus = Position{} - Position{};
  static constexpr auto ref_dependands = boost::hana::tuple<Position,Position>();
  STATIC_REQUIRE(std::is_same_v<decltype(minus)::depends_on, std::remove_cvref_t<decltype(ref_dependands)>>);
}

TEST_CASE("Operator Multiply yields multiplication of operands", "[Operator]")
{
  static constexpr auto multiply = Position{} * Position{};
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result = multiply.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result == values[0] * values[0]);
}

TEST_CASE("Operator Multiply has unit resulting from multiplying operands", "[Operator]")
{
  static constexpr auto multiply = Position{} * Position{};
  STATIC_REQUIRE(std::is_same_v<decltype(multiply)::Unit, PositionUnitSq>);
}

TEST_CASE("Operator Multiply has concatination of denepends on", "[Operator]")
{
  static constexpr auto multiply = Position{} * Position{};
  static constexpr auto ref_dependands = boost::hana::tuple<Position,Position>();
  STATIC_REQUIRE(std::is_same_v<decltype(multiply)::depends_on, std::remove_cvref_t<decltype(ref_dependands)>>);
}

TEST_CASE("Operator Divide yields division of operands", "[Operator]")
{
  static constexpr auto divide = Position{} / Position{};
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result = divide.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result == values[0] / values[0]);
}

TEST_CASE("Operator Divide has unit resulting from dividing operands", "[Operator]")
{
  static constexpr auto divide = Position{} / Position{};
  STATIC_REQUIRE(std::is_same_v<decltype(divide)::Unit, Dimensionless>);
}

TEST_CASE("Operator Divide has concatination of denepends on", "[Operator]")
{
  static constexpr auto divide = Position{} / Position{};
  static constexpr auto ref_dependands = boost::hana::tuple<Position,Position>();
  STATIC_REQUIRE(std::is_same_v<decltype(divide)::depends_on, std::remove_cvref_t<decltype(ref_dependands)>>);
}

TEST_CASE("Operator Add to Scalar yields sum of operands", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto scalar_number = 10.0;
  static constexpr auto displacement = scalar_number * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_sum = displacement + state;
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result = scalar_sum.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result == scalar_number + values[0]);
}

TEST_CASE("Operator Add to Scalar is commutative", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto scalar_number = 10.0;
  static constexpr auto displacement = scalar_number * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_sum1 = displacement + state;
  static constexpr auto scalar_sum2 = state + displacement;
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result1 = scalar_sum1.template evaluate<TestSystem, 1>(values);
  static constexpr auto result2 = scalar_sum2.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result1 == result2);
}

TEST_CASE("Operator Add to Scalar does not change unit", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto displacement = 10 * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_sum = displacement + state;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_sum)::Unit, PositionUnit>);
}

TEST_CASE("Operator Add to Scalar does not change dependencies", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto displacement = 10 * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_sum = displacement + state;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_sum)::depends_on, decltype(state)::depends_on>);
}

TEST_CASE("Operator Minus with Scalar yields substraction of operands", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto scalar_number = 10.0;
  static constexpr auto displacement = scalar_number * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_substraction = displacement - state;
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result = scalar_substraction.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result == scalar_number - values[0]);
}

TEST_CASE("Operator Minus with Scalar is commutative", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto scalar_number = 10.0;
  static constexpr auto displacement = scalar_number * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_substraction1 = displacement - state;
  static constexpr auto scalar_substraction2 = state - displacement;
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result1 = scalar_substraction1.template evaluate<TestSystem, 1>(values);
  static constexpr auto result2 = scalar_substraction2.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result1 == -result2);
}

TEST_CASE("Operator Minus with Scalar does not change unit", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto displacement = 10 * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_substraction = displacement - state;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_substraction)::Unit, PositionUnit>);
}

TEST_CASE("Operator Minus with Scalar does not change dependencies", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto displacement = 10 * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_substraction = displacement - state;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_substraction)::depends_on, decltype(state)::depends_on>);
}

TEST_CASE("Operator Multiply With Scalar yields multiplication of operands", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto scalar_number = 10.0;
  static constexpr auto time = scalar_number * s;
  static constexpr auto state = Velocity{};
  static constexpr auto scalar_multiply = time * state;
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Velocity>;

  static constexpr auto result = scalar_multiply.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result == scalar_number * values[0]);
}

TEST_CASE("Operator Multiply With Scalar is commutative", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto scalar_number = 10.0;
  static constexpr auto time = scalar_number * s;
  static constexpr auto state = Velocity{};
  static constexpr auto scalar_multiply1 = time * state;
  static constexpr auto scalar_multiply2 = state * time;
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Velocity>;

  static constexpr auto result1 = scalar_multiply1.template evaluate<TestSystem, 1>(values);
  static constexpr auto result2 = scalar_multiply2.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result1 == result2);
}

TEST_CASE("Operator Multiply With Scalar has unit resulting from multiplying operands", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto time = 10 * s;
  static constexpr auto state = Velocity{};
  static constexpr auto scalar_multiply = time * state;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_multiply)::Unit, PositionUnit>);
}

TEST_CASE("Operator Multiply With Scalar does not change dependencies", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto time = 10 * s;
  static constexpr auto state = Velocity{};
  static constexpr auto scalar_multiply = time * state;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_multiply)::depends_on, decltype(state)::depends_on>);
}

TEST_CASE("Operator Divide by Scalar yields division of operands", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto scalar_number = 10.0;
  static constexpr auto time = scalar_number * s;
  static constexpr auto state = Position{};
  static constexpr auto scalar_division = state / time;
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result = scalar_division.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result == values[0] / scalar_number);
}

TEST_CASE("Operator Divide by Scalar yields unit from division", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto time = 10 * s;
  static constexpr auto state = Position{};
  static constexpr auto scalar_division = state / time;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_division)::Unit, VelocityUnit>);
}

TEST_CASE("Operator Divide by Scalar does not change dependencies", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto time = 10 * s;
  static constexpr auto state = Position{};
  static constexpr auto scalar_division = state / time;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_division)::depends_on, decltype(state)::depends_on>);
}

TEST_CASE("Operator Divide Scalar by State yields division of operands", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto scalar_number = 10.0;
  static constexpr auto ref_distance = scalar_number * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_division = ref_distance / state;
  static constexpr std::array values{2.0};
  using TestSystem = codys::System<Position>;

  static constexpr auto result = scalar_division.template evaluate<TestSystem, 1>(values);
  STATIC_REQUIRE(result == scalar_number / values[0]);
}

TEST_CASE("Operator Divide Scalar by State yields unit from division", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto ref_distance = 10.0 * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_division = ref_distance / state;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_division)::Unit, Dimensionless>);
}

TEST_CASE("Operator Divide Scalar by State does not change dependencies", "[Operator]")
{
  using namespace units::isq::si::references;
  static constexpr auto ref_distance = 10.0 * m;
  static constexpr auto state = Position{};
  static constexpr auto scalar_division = ref_distance / state;
  STATIC_REQUIRE(std::is_same_v<decltype(scalar_division)::depends_on, decltype(state)::depends_on>);
}


/*
 * 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;
}


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file scheduling_jobs_on_identical_parallel_machines_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-09-06
 */

#include "test_utils/logger.hpp"
#include "test_utils/scheduling.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/greedy/scheduling_jobs_on_identical_parallel_machines/scheduling_jobs_on_identical_parallel_machines.hpp"

#include <boost/range/numeric.hpp>
#include <boost/test/unit_test.hpp>

#include <vector>

BOOST_AUTO_TEST_CASE(scheduling_jobs_on_identical_parallel_machines) {
    // sample data
    int NUMBER_OF_MACHINES = 3;
    typedef double Time;
    std::vector<Time> j = { 2.1, 3.1, 4.1, 5.1, 6.1, 7.1, 8.1 };
    std::vector<std::pair<int, decltype(j)::iterator>> result;
    paal::greedy::scheduling_jobs_on_identical_parallel_machines(
        NUMBER_OF_MACHINES, j.begin(), j.end(), back_inserter(result),
        paal::utils::identity_functor());
    check_jobs(result, j);
    std::vector<Time> sum_of_machine;
    sum_of_machine.resize(NUMBER_OF_MACHINES);
    for (auto job_machine_pair : result) {
        sum_of_machine[job_machine_pair.first] += *job_machine_pair.second;
    }

    Time maximumLoad = *boost::max_element(sum_of_machine);

    Time sum_all_loads = boost::accumulate(sum_of_machine, 0.);
    // print result
    check_result(maximumLoad, double(sum_all_loads) / NUMBER_OF_MACHINES, 4./3.);
}


/* ----------------------------------------------------------------------------
 * GTDynamics Copyright 2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file  main.cpp
 * @brief Spider trajectory optimization with pre-specified footholds.
 * @author: Alejandro Escontrela, Stephanie McCormick, Disha Das, Tarushree
 * Gandhi, Varun Agrawal
 */

#include <CppUnitLite/TestHarness.h>
#include <gtdynamics/factors/ObjectiveFactors.h>
#include <gtdynamics/universal_robot/sdf.h>
#include <gtdynamics/utils/Trajectory.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

#include <algorithm>
#include <boost/algorithm/string/join.hpp>
#include <boost/optional.hpp>
#include <fstream>
#include <iostream>
#include <utility>

using std::string;
using std::vector;

using gtsam::Point3;
using gtsam::Pose3;
using gtsam::Rot3;
using gtsam::Vector6;
using gtsam::noiseModel::Isotropic;
using gtsam::noiseModel::Unit;

using namespace gtdynamics;

// Returns a Trajectory object for a single spider walk cycle.
Trajectory getTrajectory(vector<string> links, Robot robot, size_t repeat) {
  const Point3 contact_in_com(0, 0.19, 0);
  Phase stationary(40);
  stationary.addContactPoints(links, contact_in_com);

  Phase odd(20);
  odd.addContactPoints(
      {{"tarsus_1_L1", "tarsus_3_L3", "tarsus_5_R4", "tarsus_7_R2"}},
      contact_in_com);

  Phase even(20);
  even.addContactPoints(
      {{"tarsus_2_L2", "tarsus_4_L4", "tarsus_6_R3", "tarsus_8_R1"}},
      contact_in_com);

  WalkCycle walk_cycle;
  walk_cycle.addPhase(stationary);
  walk_cycle.addPhase(even);
  walk_cycle.addPhase(stationary);
  walk_cycle.addPhase(odd);

  Trajectory trajectory(robot, walk_cycle, repeat);
  return trajectory;
}

int main(int argc, char **argv) {
  // Load Stephanie's spider robot.
  auto robot =
      CreateRobotFromFile(kSdfPath + string("/spider_alt.sdf"), "spider");

  double sigma_dynamics = 1e-5;   // std of dynamics constraints.
  double sigma_objectives = 1e-6; // std of additional objectives.
  double sigma_joints = 1.85e-4;  // 1.85e-4

  // Noise models.
  auto dynamics_model_6 = Isotropic::Sigma(6, sigma_dynamics),
       dynamics_model_1 = Isotropic::Sigma(1, sigma_dynamics),
       dynamics_model_1_2 = Isotropic::Sigma(1, sigma_joints),
       objectives_model_6 = Isotropic::Sigma(6, sigma_objectives),
       objectives_model_1 = Isotropic::Sigma(1, sigma_objectives);

  // Env parameters.
  gtsam::Vector3 gravity(0, 0, -9.8);
  double mu = 1.0;

  OptimizerSetting opt(sigma_dynamics);
  DynamicsGraph graph_builder(opt, gravity);

  vector<string> links = {"tarsus_1_L1", "tarsus_2_L2", "tarsus_3_L3",
                          "tarsus_4_L4", "tarsus_5_R4", "tarsus_6_R3",
                          "tarsus_7_R2", "tarsus_8_R1"};

  // Create the trajectory, consisting of 3 walk cycles, each consisting of 4
  // phases: [stationary, odd, stationary, even].
  auto trajectory = getTrajectory(links, robot, 3);

  // Create multi-phase trajectory factor graph
  auto collocation = CollocationScheme::Euler;
  auto graph = trajectory.multiPhaseFactorGraph(graph_builder, collocation, mu);

  // Build the objective factors.
  double ground_height = 1.0;
  const Point3 step(0, 0.4, 0);
  gtsam::NonlinearFactorGraph objectives =
      trajectory.contactPointObjectives(Isotropic::Sigma(3, 1e-7), step, ground_height);

  // Get final time step.
  int K = trajectory.getEndTimeStep(trajectory.numPhases() - 1);

  // Add base goal objectives to the factor graph.
  auto base_link = robot.link("body");
  for (int k = 0; k <= K; k++) {
    objectives.add(
        LinkObjectives(base_link->id(), k)
            .pose(Pose3(Rot3(), Point3(0, 0.0, 0.5)), Isotropic::Sigma(6, 5e-5))
            .twist(gtsam::Z_6x1, Isotropic::Sigma(6, 5e-5)));
  }

  // Add link and joint boundary conditions to FG.
  trajectory.addBoundaryConditions(&objectives, dynamics_model_6,
                                   dynamics_model_6, objectives_model_6,
                                   objectives_model_1, objectives_model_1);

  // Constrain all Phase keys to have duration of 1 /240.
  const double desired_dt = 1. / 240;
  trajectory.addIntegrationTimeFactors(&objectives, desired_dt, 1e-30);

  // Add min torque objectives.
  trajectory.addMinimumTorqueFactors(&objectives, Unit::Create(1));

  // Add prior on hip joint angles (spider specific)
  auto prior_model = Isotropic::Sigma(1, 1.85e-4);
  for (auto &&joint : robot.joints())
    if (joint->name().find("hip2") == 0)
      for (int k = 0; k <= K; k++)
        objectives.add(JointObjectives(joint->id(), k).angle(2.5, prior_model));

  // Add objectives to factor graph.
  graph.add(objectives);

  // Initialize solution.
  double gaussian_noise = 1e-5;
  gtsam::Values init_vals =
      trajectory.multiPhaseInitialValues(gaussian_noise, desired_dt);

  // Optimize!
  gtsam::LevenbergMarquardtParams params;
  params.setVerbosityLM("SUMMARY");
  params.setlambdaInitial(1e10);
  params.setlambdaLowerBound(1e-7);
  params.setlambdaUpperBound(1e10);
  params.setAbsoluteErrorTol(1.0);
  gtsam::LevenbergMarquardtOptimizer optimizer(graph, init_vals, params);
  auto results = optimizer.optimize();

  // Write results to traj file
  trajectory.writeToFile("forward_traj.csv", results);

  return 0;
}


// 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;
}