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

max_stars_repo_name: brunomraz/FS-BMK

max_issues_repo_name: brunomraz/FS-BMK

max_forks_repo_name: brunomraz/FS-BMK


text:
/*
Copyright (c) 2013, Illumina Inc.
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
. Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
. Neither the name of the Illumina, Inc. nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER 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.
*/
#include "uploader.h"
#include "scope_guard.h"
#include "errors.h"
#include "context.h"
#include "commands.h"

#include <iostream>
#include <stdint.h>
#include <stdio.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <boost/bind.hpp>
#include "compressor.h"
#include "mimes.h"

#define MIN_PART_SIZE (16*1024*1024)
#define MIN_ALLOWED_PART_SIZE (16*1024*1024)
#define MAX_PART_NUM 10000

using namespace es3;

struct es3::upload_content
{
	upload_content() : num_parts_(), num_completed_() {}

	context_ptr conn_;
    std::string upload_id_;
	s3_path remote_;

	mutex_t lock_;
	size_t num_parts_;
	size_t num_completed_;
    std::vector<std::string> etags_;
    header_map_t hmap_;
};

class part_upload_task : public sync_task
{
	size_t num_;
	upload_content_ptr content_;

	segment_ptr segment_;
public:
	part_upload_task(size_t num, upload_content_ptr content,
					 segment_ptr segment)
		: num_(num), content_(content), segment_(segment)
	{
	}

	virtual void print_to(std::ostream &str)
	{
		str << "Upload segment " << num_ << " of " << content_->remote_;
	}

	virtual void operator()(agenda_ptr agenda)
	{
		VLOG(2) << "Starting upload of a part " << num_ << " of "
				<< content_->remote_;

        bool is_multipart=content_->num_parts_>1;
        if (is_multipart)
        {
            guard_t g(content_->lock_);
            if (content_->upload_id_.empty())
            {
                s3_connection up_prep(content_->conn_);
                content_->upload_id_=up_prep.initiate_multipart(content_->remote_, content_->hmap_);
            }
        }

		struct sched_param param;
		param.sched_priority = 1+num_*90/content_->num_parts_;
		pthread_setschedparam(pthread_self(), SCHED_RR, &param);

		s3_path part_path=content_->remote_;
		s3_connection up(content_->conn_);
        std::string etag=up.upload_data(part_path, content_->upload_id_, num_+1,
                &segment_->data_[0], segment_->data_.size(),
                is_multipart?header_map_t():content_->hmap_);
		assert(!etag.empty());
		agenda->add_stat_counter("uploaded", segment_->data_.size());

		//Check if the upload is completed
		guard_t g(content_->lock_);
		content_->num_completed_++;
		content_->etags_.at(num_) = etag;

		VLOG(2) << "Uploaded part " << num_ << " of "<< content_->remote_
				<< " with etag=" << etag
				<< ", total=" << content_->num_parts_
				<< ", sent=" << content_->num_completed_ << ".";

        if (content_->num_completed_ == content_->num_parts_ && !content_->upload_id_.empty())
		{
			VLOG(2) << "Assembling "<< content_->remote_ <<".";
			//We've completed the upload!
			s3_connection up2(content_->conn_);
			up2.complete_multipart(content_->remote_, content_->upload_id_,
								   content_->etags_);
		}
	}
};

class file_pump : public sync_task,
		public boost::enable_shared_from_this<file_pump>
{
	upload_content_ptr content_;
	files_ptr files_;
	size_t cur_segment_, number_of_segments_;
	bool update_log_;
public:
	file_pump(upload_content_ptr content,
		files_ptr files, size_t cur_segment, size_t number_of_segments,
		bool update_log) :
		content_(content), files_(files),
		cur_segment_(cur_segment), number_of_segments_(number_of_segments),
		update_log_(update_log)
	{
	}

	virtual void print_to(std::ostream &str)
	{
		str << "Read segment " << cur_segment_ << " of " << content_->remote_;
	}

	virtual task_type_e get_class() const { return taskIOBound; }

	virtual size_t needs_segments() const
	{
		return number_of_segments_;
	}

	virtual void operator()(agenda_ptr agenda,
							const std::vector<segment_ptr> &segments)
	{
		size_t segment_size=agenda->segment_size();
		uint64_t start_offset = segment_size*cur_segment_;

		//First, skip to our piece
		uint64_t skipped_so_far=0, offset_within_=0;
		int cur_piece=0;
		for(int f=0;f<files_->sizes_.size();++f)
		{
			uint64_t cur_size = files_->sizes_.at(f);
			if (start_offset<(skipped_so_far+cur_size))
			{
				cur_piece = f;
				offset_within_ = start_offset-skipped_so_far;
				assert(offset_within_<cur_size);
				break;
			}
			skipped_so_far+=cur_size;
		}

		//Now read data!!!
		for(int f=0;f<number_of_segments_;++f)
		{
			segment_ptr seg = segments.at(f);
			seg->data_.resize(segment_size, 0);

			uint64_t segment_read_so_far=0;
			while(segment_read_so_far<seg->data_.size())
			{
				//Note that we're duplicating the handle because other
				//file pumps might be using it
				handle_t cur_fl(open(
									files_->files_.at(cur_piece).c_str(), O_RDONLY));
				lseek64(cur_fl.get(), offset_within_, SEEK_SET) | libc_die;
				uint64_t cur_piece_size=files_->sizes_.at(cur_piece);

				uint64_t remaining_size =
						std::min(segment_size-segment_read_so_far,
								 cur_piece_size-offset_within_);
				while(remaining_size>0)
				{
					char buf[65536*8];
					//No overflow is possible since sizeof(buf)<MAX_SIZE_T.
					size_t chunk=std::min(remaining_size,
										  uint64_t(sizeof(buf)));
					size_t res=read(cur_fl.get(), buf, chunk) | libc_die;
					assert(res!=0);
					memcpy(&seg->data_[segment_read_so_far], buf, res);

					segment_read_so_far+=res;
					offset_within_+=res;
					remaining_size-=res;
				}

				//We've run out of file piece, switch to the next one
				if (cur_piece==files_->sizes_.size()-1)
				{
					//No other pieces and this is the last segment
					seg->data_.resize(segment_read_so_far);
				} else
				{
					if (cur_piece_size==offset_within_)
					{
						cur_piece++;
						offset_within_=0;
					} else
					{
						assert(segment_read_so_far==segment_size);
					}
				}
			}
			assert(segment_read_so_far==segment_size
				   || f==number_of_segments_-1);

			if (update_log_)
				agenda->add_stat_counter("read", seg->data_.size());
			sync_task_ptr task(new part_upload_task(cur_segment_+f,
													content_, seg));
			agenda->schedule(task);
		}
	}
};

void file_uploader::operator()(agenda_ptr agenda)
{
	uint64_t file_sz=file_size(path_);
	time_t mtime=last_write_time(path_);

	struct stat stbuf={0};
	::stat(path_.c_str(), &stbuf) | libc_die;
	mode_t mode=stbuf.st_mode & 0777; //Get permissions

	//Check the modification date of the file locally and on the
	//remote side
	s3_connection up(conn_);
	file_desc mod=up.find_mtime_and_size(remote_);
	if(mod.mtime_)
	{
		if (mod.mtime_==mtime && mod.raw_size_==file_sz)
			return; //TODO: add an optional MD5 check?
	}
	//We don't check file mode here, because it doesn't really work
	//on Windows - we'll get permission loops.

	//Woohoo! We need to upload the file.
	upload_content_ptr up_data(new upload_content());
	up_data->conn_ = conn_;
	up_data->remote_ = remote_;

	VLOG(2) << "Starting upload of " << path_ << " as "
			  << remote_;

	bool do_compress = should_compress(path_, file_sz) &&
			conn_->do_compression_;
	//Prepare upload
	header_map_t hmap;
	hmap["x-amz-meta-compressed"] = do_compress ? "true" : "false";
	//hmap["Content-Type"] = "application/x-binary";
	hmap["Content-Type"] = find_mime(path_.extension().c_str());
	if (do_compress)
		hmap["Content-Encoding"] = "gzip";
	hmap["x-amz-meta-last-modified"] = int_to_string(mtime);
	hmap["x-amz-meta-size"] = int_to_string(file_sz);
	hmap["x-amz-meta-file-mode"] = int_to_string(mode);
    up_data->hmap_=hmap;

	if (do_compress)
	{
		files_finished_callback on_finish=boost::bind(
					&file_uploader::start_upload, shared_from_this(),
					agenda, up_data, _1, true);

		sync_task_ptr task(new file_compressor(path_, conn_, on_finish));
		agenda->schedule(task);
	} else
	{
		handle_t fl(open(path_.c_str(), O_RDONLY) | libc_die);
		files_ptr files(new scattered_files(path_, fl.size()));
		start_upload(agenda, up_data, files, false);
	}
}

void file_uploader::start_upload(agenda_ptr ag,
								 upload_content_ptr content,
								 files_ptr files,
								 bool compressed)
{
	uint64_t size = 0;
	for(int f=0;f<files->sizes_.size();++f)
		size+=files->sizes_.at(f);

	size_t segment_size = ag->segment_size();
	size_t number_of_segments = safe_cast<size_t>(size/segment_size +
			((size%segment_size)==0 ? 0:1));
	if (number_of_segments>MAX_PART_NUM)
		err(errFatal) << "File "<<remote_ <<" is too big";
	if (number_of_segments==0)
	{
		assert(files->sizes_.size()==1 && files->sizes_.at(0)==0);
		number_of_segments=1;
	}

	content->num_parts_ = number_of_segments;
	content->etags_.resize(number_of_segments);

	//Now create file pumps
	size_t num_per_pump = number_of_segments /
			ag->get_capability(taskIOBound) + 1;

	if (num_per_pump>ag->max_in_flight()/2)
		num_per_pump=ag->max_in_flight()/2;

	for(size_t f=0;f<number_of_segments;f+=num_per_pump)
	{
		size_t num_cur = number_of_segments-f;
		if (num_cur > num_per_pump)
			num_cur = num_per_pump;

		sync_task_ptr task(new file_pump(content, files, f, num_cur,
										 !compressed));
		ag->schedule(task);
	}
}

void remote_file_deleter::operator()(agenda_ptr agenda)
{
	VLOG(2) << "Removing " << remote_;
	s3_connection up(conn_);
	up.read_fully("DELETE", remote_);
}

ext: cpp


text:
#include "dbc++/dbcpp.hh"
#include <boost/dll/shared_library.hpp>
#include <map>
#include <mutex>

#include <spdlog/spdlog.h>

#include <spdlog/sinks/null_sink.h>

namespace dbcpp {
  /**
   * @brief Create and register a logger with the library's spdlog
   * @param logger spdlog logger to register in the library
   */
  void register_logger( std::shared_ptr< spdlog::logger > logger ) { spdlog::register_logger( logger ); }

  /**
   * @brief Create and register a logger with the library's spdlog
   * @param name logger name
   * @param sinks logger targets/sinks
   */
  std::shared_ptr< spdlog::logger > create_logger( std::string name, std::vector< spdlog::sink_ptr > sinks ) {
    std::shared_ptr< spdlog::logger > logger = spdlog::get( name );

    if ( !logger ) {
      if ( sinks.empty( ) ) {
        sinks.emplace_back( std::make_shared< spdlog::sinks::null_sink_mt >( ) );
      }

      logger = std::make_shared< spdlog::logger >( name, sinks.begin( ), sinks.end( ) );

      spdlog::register_logger( logger );
    } else {
      auto &lsinks = logger->sinks( );

      std::copy( sinks.begin( ), sinks.end( ), std::back_inserter( lsinks ) );
      std::sort( lsinks.begin( ), lsinks.end( ) );

      lsinks.erase( std::unique( lsinks.begin( ), lsinks.end( ) ), lsinks.end( ) );
    }

    return logger;
  }

#define LOG( logger, lvl, ... )                                                                                \
  do {                                                                                \
    if ( logger->should_log( spdlog::level::lvl ) ) {                                                                  \
      SPDLOG_LOGGER_CALL( logger, spdlog::level::lvl, __VA_ARGS__ );                                                   \
    }                                                                                \
  } while ( 0 )

  namespace Driver {
    struct DriverInfo {
      boost::dll::shared_library library;
      Base *                     driver;
    };

    /** Database driver map lock */
    std::mutex driverLock;
    /** Database drivers by schema name */
    std::map< std::string, DriverInfo > drivers;
    /** Driver logger */
    std::shared_ptr< spdlog::logger > logger = create_logger( "dbcpp::Driver", { } );

    /**
     * @brief Get the driver by uri schema
     * @param uri connection uri
     * @return driver
     */
    Base *getDriver( Uri const *uri ) {
      auto entry = drivers.find( uri->scheme( ) );

      if ( entry == drivers.end( ) ) {
        std::lock_guard< std::mutex > guard( driverLock );
        boost::dll::shared_library    library(
          uri->scheme( ), boost::dll::load_mode::append_decorations | boost::dll::load_mode::search_system_folders );

        LOG( logger, trace, "Loaded library for '{}'", uri->scheme( ) );

        if ( library.has( "getDriver" ) == true ) {
          Base *driver = library.get< Base *( ) >( "getDriver" )( );

          drivers[ uri->scheme( ) ] = DriverInfo{ std::move( library ), driver };

          LOG( logger, trace, "Cached driver for {}", uri->scheme( ) );

          return driver;
        }

        LOG( logger, trace, "Supporting driver for '{}' not found", uri->scheme( ) );
        throw std::runtime_error( "Supporting database type not found" );
      } else {
        LOG( logger, trace, "Database driver {} found in cache", uri->scheme( ) );
      }

      return entry->second.driver;
    }

    /**
     * @brief Create a database connection
     * @param uri connection creation uri
     * @return database connection
     */
    static std::shared_ptr< interface::Connection > createConnection( Uri *const uri ) {
      std::shared_ptr< interface::Connection > rawCxn;
      auto                                     driver = getDriver( uri );

      LOG( logger, trace, "Creating database connection for type '{}'", uri->scheme( ) );
      if ( !( rawCxn = driver->createConnection( uri ) ) ) {
        LOG( logger, trace, "Database connection creation failed for '{}'", uri->scheme( ) );
        throw std::runtime_error( "Connection create failed" );
      }

      if ( !rawCxn->connect( ) ) {
        LOG( logger, trace, "Database connection failed to connect '{}'", uri->scheme( ) );
        throw std::runtime_error( "Unable to connect" );
      }

      return rawCxn;
    }

    /**
     * @brief Connect to a database by uri string
     * @param uri resource identifier string
     * @return database connection
     */
    Connection connect( const std::string &uri ) {
      std::shared_ptr< Uri > _uri( Uri::parse( uri ) );

      return connect( _uri.get( ) );
    }

    /**
     * @brief Connect to a database by uri
     * @param uri resource identifier
     * @return database connection
     */
    Connection connect( Uri *uri ) { return Connection( createConnection( uri ) ); }
  } // namespace Driver

  /***************************************************************************************************
   * Special case for pools that require the above namespace methods
   ***********/

  /**
   * @brief Establish a pool connection
   * @return established connection
   */
  internal::Pool::connection_t internal::Pool::connect( ) {
    auto driver = Driver::getDriver( uri.get( ) );
    auto cxn    = driver->createConnection( uri.get( ) );

    if ( cxn == nullptr ) {
      throw std::runtime_error( "Connection create failed" );
    }

    cxn->setAutoCommit( autoCommit );

    if ( !cxn->connect( ) ) {
      throw std::runtime_error( "Unable to connect" );
    }

    return cxn;
  }
} // namespace dbcpp

ext: cc

max_stars_repo_name: tcsantanello/dbcpp

max_issues_repo_name: tcsantanello/dbcpp

max_forks_repo_name: tcsantanello/dbcpp


text:
#include <algorithm>
#include <iostream>
#include <string>
#include <vector>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

bool pieces_well_placed(const std::string& first_row, bool no_adjacent_bishops)
{
    const int size = first_row.size();

    int first_bishop = -1;
    int first_archbishop_or_queen = -1;
    int num_rooks = 0;

    for (auto i = 0; i < size; i++) {
        switch (first_row[i]) {
        case 'B':
            if (first_bishop == -1)
                first_bishop = i;
            else if ((i - first_bishop) % 2 == 0)
                return false;  // same color bishops
            else if (no_adjacent_bishops && i - first_bishop == 1)
                return false;  // adjacent bishops (with --no-adjacent-bishops)
            break;

        case 'A':
        case 'Q':
            if (first_archbishop_or_queen == -1)
                first_archbishop_or_queen = i;
            else if ((i - first_archbishop_or_queen) % 2 == 0)
                return false;  // same color archbishop/queen
            break;

        case 'R':
            num_rooks += 1;
            break;

        case 'K':
            if (num_rooks != 1)
                return false;  // king not between the rooks
            break;
        }
    }

    return true;
}

bool second_row_protected(const std::string& first_row)
{
    const int size = first_row.size();

    std::vector<bool> second_row(size);

    for (auto i = 0; i < size; i++) {
        // knight component
        if (first_row[i] == 'A' || first_row[i] == 'C' || first_row[i] == 'N') {
            if (i-2 >= 0)
                second_row[i-2] = true;
            if (i+2 < size)
                second_row[i+2] = true;
        }

        // ferz component
        if (first_row[i] == 'A' || first_row[i] == 'B' ||
                first_row[i] == 'K' || first_row[i] == 'Q') {
            if (i-1 >= 0)
                second_row[i-1] = true;
            if (i+1 < size)
                second_row[i+1] = true;
        }

        // wazir component
        if (first_row[i] == 'C' || first_row[i] == 'R' ||
                first_row[i] == 'K' || first_row[i] == 'Q') {
            second_row[i] = true;
        }
    }

    return std::all_of(second_row.begin(), second_row.end(),
                       [](bool i) { return i; });
}

int main(int argc, char **argv)
{
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("no-adjacent-bishops", "prevent adjacent bishops")
    ;

    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);
    } catch (po::unknown_option& e) {
        std::cerr << desc;
        return 1;
    }

    if (vm.count("help")) {
        std::cerr << desc;
        return 1;
    }

    // piece abbreviations come from
    //  https://en.wikipedia.org/wiki/Capablanca_Random_Chess
    std::string first_row("RNABQKBCNR");
    std::sort(first_row.begin(), first_row.end());

    const bool no_adjacent_bishops = vm.count("no-adjacent-bishops");
    do {
        if (pieces_well_placed(first_row, no_adjacent_bishops) &&
                second_row_protected(first_row))
            std::cout << first_row << '\n';
    } while (std::next_permutation(first_row.begin(), first_row.end()));

    return 0;
}

max_stars_repo_name: agt-the-walker/chess-utils

max_issues_repo_name: agt-the-walker/chess-utils

max_forks_repo_name: agt-the-walker/chess-utils


text:
/**
 * @file
 * @copyright defined in gxc/LICENSE
 */
#pragma once

#include <eosio/name.hpp>
#include <boost/utility/string_view.hpp>

namespace gxc {

using eosio::name;

bool starts_with(const name& input, const name& test) {
   uint64_t mask = 0xF800000000000000ull;
   auto maxlen = 12;
   auto v = input.value;
   auto c = test.value;

   for (auto i = 0; i < maxlen; ++i, v <<= 5, c<<= 5) {
      if ((v & mask) == (c & mask)) continue;

      if (c & mask) return false;
      else break;
   }

   return true;
}

bool starts_with(const name& input, const boost::string_view& test) {
   return boost::string_view(input.to_string()).starts_with(test);
}

bool has_dot(eosio::name input) {
   uint64_t mask = 0xF800000000000000ull;
   auto v = input.value;
   auto len = input.length();
   auto has_dot = false;

   for (auto i = 0; i < len; ++i, v <<= 5) {
      has_dot |= !(v & mask);
   }
   return has_dot;
}

}

ext: hpp


text:
#include <cmath>

#include <chrono>

#include <boost/program_options.hpp>

#include "flpmc_yche.h"
#include "simrank.h"
#include "../playground/pretty_print.h"

using namespace std;
using namespace boost::program_options;

#ifdef VARYING_RMAX
FLPMC::FLPMC(string g_name_, GraphYche &g_, double c_, double epsilon_, double delta_, size_t Q_, double r_max_) :
        g_name(g_name_), g(&g_), c(c_), epsilon(epsilon_), delta(delta_), Q(Q_) {
    // 1st: init r_max
    r_max = r_max_;

    // 2nd: compute or load index
    size_t n = static_cast<size_t>(g->n);
    cout << format("r_max: %s") % get_rmax() << endl;
    lp = new Full_LocalPush(*g, g_name, c, get_lp_epsilon(), n);
    if (!lp_file_exists(g_name, c, get_lp_epsilon(), n, true)) { // test wether the local push index exists
        cout << "local push offline index doesn't exists.. " << endl;
        auto start_time = std::chrono::high_resolution_clock::now();
        lp->local_push(*g);
        auto end_time = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> elapsed = end_time - start_time;
        cout << format("total indexing cost: %s s") % elapsed.count() << endl; // record the pre-processing time
        cout << format("building compete, saving to %s ") % lp->get_file_path_base() << endl;
        lp->save();
        cout << "saved." << endl;
    } else {
        cout << "offline index exists..loading " << endl;
        lp->load();
    }
}
#else

FLPMC::FLPMC(string g_name_, GraphYche &g_, double c_, double epsilon_, double delta_, size_t Q_) :
        g_name(g_name_), g(&g_), c(c_), epsilon(epsilon_), delta(delta_), Q(Q_) {
    // init local push
    size_t n = static_cast<size_t>(g->n);
    cout << format("r_max: %s") % get_rmax() << endl;
    lp = new Full_LocalPush(*g, g_name, c, get_lp_epsilon(), n);

//    {
//        // directly compute the index, instead of reading and parsing
//        auto clock_start = clock();
//
//        auto start_time = std::chrono::high_resolution_clock::now();
//
//        auto plp = Full_LocalPush(*g, g_name, c, get_lp_epsilon(), n);
//        plp.local_push(*g);
//        swap(lp->P, plp.P);
//        swap(lp->R, plp.R);
//        auto end_time = std::chrono::high_resolution_clock::now();
//        auto clock_end = clock();
//        cout << "total indexing cpu time:" << static_cast<double>(clock_end - clock_start) / CLOCKS_PER_SEC << "s"
//             << endl;
//        std::chrono::duration<double> elapsed = end_time - start_time;
//        cout << format("total indexing cost: %s s") % elapsed.count() << endl; // record the pre-processing time
//    }

    if (!lp_file_exists(g_name, c, get_lp_epsilon(), n, true)) { // test wether the local push index exists
        cout << "local push offline index doesn't exists.. " << endl;
        auto start_time = std::chrono::high_resolution_clock::now();
        lp->local_push(*g);
        auto end_time = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> elapsed = end_time - start_time;
        cout << format("total indexing cost: %s s") % elapsed.count() << endl; // record the pre-processing time
        cout << format("building compete, saving to %s ") % lp->get_file_path_base() << endl;
        lp->save();
        cout << "saved." << endl;
    } else {
        cout << "offline index exists..loading " << endl;
        lp->load();
    }
}

#endif

FLPMC::FLPMC(const FLPMC &other_obj) {
//    cout << "flpmc..." << endl;
    rand_gen = SFMTRand();

    g_name = other_obj.g_name;
    Q = other_obj.Q;
    epsilon = other_obj.epsilon; // the error bound required by query
    delta = other_obj.delta;
    c = other_obj.c;
    g = other_obj.g; // the pointer to the graph
    lp = other_obj.lp; // the pointer to the local push index

    // rmax
#ifdef VARYING_RMAX
    r_max = other_obj.r_max;
#endif
}

double FLPMC::get_rmax() {
//    double r = sqrt(epsilon);
#ifdef VARYING_RMAX
    return r_max;
#else
    double r = pow(epsilon, 1.0 / 1.5);
    return r;
#endif
//    double r = pow(pow(epsilon, 2) / (pow(c, 3) / log(2.0 / delta)), 1.0 / 3);
//    cout << format("r_max of local push: %s") % r << endl;
}

double FLPMC::get_lp_epsilon() {
//    double lp_epsilon = get_rmax() * (1 - c);
    double lp_epsilon = get_rmax() / (1 - c);
//    cout << format("local push error: %s") % lp_epsilon << endl;
    return lp_epsilon;
}

double FLPMC::query_one2one(NodePair np) {
//    cout << np << endl;
    if (np.first == np.second) {
        return 1;
    } else if (np.first > np.second) { // make sure np.first < np.second
        np = NodePair({np.second, np.first});
    }
    /* FLP result */
    double p_i = lp->query_P(np.first, np.second);
    double r_i = lp->query_R(np.first, np.second);
//    cout << format("pi: %s, ri:%s") % p_i % r_i << endl;

    // set up the random number generator
#ifndef SFMT
    std::random_device rd;  //Will be used to obtain a seed for the random number engine
    std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
    std::uniform_real_distribution<double> distribution(0.0, 1.0);
#endif

    /* MC sampling phase */
    int N = get_N();
    double E_residual = 0;
    for (int i = 0; i < N; i++) {
        int a = np.first;
        int b = np.second;
        int step = 0;
        bool is_dead_node = false;

#ifndef SFMT
        while(( distribution(gen) < c ) && (a != b)){
            a = sample_in_neighbor(a, *g);
            b = sample_in_neighbor(b, *g);
#else
        while ((step == 0 || rand_gen.double_rand() < c) && (a != b)) {
            a = sample_in_neighbor(a, *g, rand_gen);
            b = sample_in_neighbor(b, *g, rand_gen);
#endif
            step++;
            if (a == -1 || b == -1) {
                is_dead_node = true;
                break;
            }
        }
        if (is_dead_node) {
            E_residual += 0;
        } else {
            E_residual += lp->query_R(a, b);
        }
    }

//    cout << format("p_i: %s") % p_i << endl;
//    cout << format("number of samples: %s") % get_N() << endl;
    return p_i + r_i + (c / (1 - c)) * (E_residual / N);
}

double FLPMC::get_N() {
    // compute the number of needed samples
    double tmp = (pow(c, 2.) * pow(get_rmax(), 2.) * log(2. / delta)) / (2 * pow(epsilon, 2.) * pow((1 - c), 2.));
    int n = ceil(tmp);
    return n;
}

max_stars_repo_name: CheYulin/SimRankRelease

max_issues_repo_name: KeithYue/SimRankRelease

max_forks_repo_name: KeithYue/SimRankRelease


text:
#ifndef MPLLIBS_METAMONAD_V1_FWD_DO_HPP
#define MPLLIBS_METAMONAD_V1_FWD_DO_HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu)  2013.
// 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)

#include <mpllibs/metamonad/limit_do_size.hpp>
#include <mpllibs/metamonad/v1/syntax.hpp>

#include <boost/preprocessor/repetition/enum_params_with_a_default.hpp>

// for boost::mpl::na
#include <boost/mpl/vector.hpp>

namespace mpllibs
{
  namespace metamonad
  {
    namespace v1
    {
      template <
        class Monad,
        BOOST_PP_ENUM_PARAMS_WITH_A_DEFAULT(
          MPLLIBS_LIMIT_DO_SIZE,
          class E,
          syntax<boost::mpl::na>
        )
      >
      struct do_;
    }
  }
}

#endif


text:
//******************************************************************************
// RCF - Remote Call Framework
//
// Copyright (c) 2005 - 2013, Delta V Software. All rights reserved.
// http://www.deltavsoft.com
//
// RCF is distributed under dual licenses - closed source or GPL.
// Consult your particular license for conditions of use.
//
// If you have not purchased a commercial license, you are using RCF
// under GPL terms.
//
// Version: 2.0
// Contact: support <at> deltavsoft.com
//
//******************************************************************************

//
// detail/win_event.hpp
// ~~~~~~~~~~~~~~~~~~~~
//
// Copyright (c) 2003-2011 Christopher M. Kohlhoff (chris at kohlhoff dot com)
//
// 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)
//

#ifndef RCF_DETAIL_WIN_EVENT_HPP
#define RCF_DETAIL_WIN_EVENT_HPP

#if defined(_MSC_VER) && (_MSC_VER >= 1200)
# pragma once
#endif // defined(_MSC_VER) && (_MSC_VER >= 1200)

#if defined(BOOST_WINDOWS)

#include <boost/assert.hpp>

#include <RCF/thread/push_options.hpp>

#include <RCF/thread/mutex.hpp>

namespace RCF {
namespace detail {

class RCF_EXPORT win_event
  : private noncopyable
{
public:
  // Constructor.
  win_event();

  // Destructor.
  ~win_event()
  {
    ::CloseHandle(event_);
  }

  // Signal the event.
  template <typename Lock>
  void signal_all(Lock& lock)
  {
    BOOST_ASSERT(lock.locked());
    (void)lock;
    if (mWaitCount > 0)
    {
        ::SetEvent(event_);
    }
  }

  // Reset the event.
  template <typename Lock>
  void clear(Lock& lock)
  {
    BOOST_ASSERT(lock.locked());
    (void)lock;
    ::ResetEvent(event_);
  }

  // Wait for the event to become signalled.
  template <typename Lock>
  void wait(Lock& lock)
  {
    BOOST_ASSERT(lock.locked());
    lock.unlock();

    {
        mutex::scoped_lock waitLock(mWaitCountMutex);
        ++mWaitCount;
    }

    ::WaitForSingleObject(event_, INFINITE);

    {
        mutex::scoped_lock waitLock(mWaitCountMutex);
        --mWaitCount;
        if (mWaitCount == 0)
        {
            // Set signalled state back to false.
            ::ResetEvent(event_);
        }
    }

    lock.lock();

  }

  // Wait for the event to become signalled.
  template <typename Lock>
  bool timed_wait(Lock& lock, boost::uint32_t waitMs)
  {
      BOOST_ASSERT(lock.locked());

      lock.unlock();

      {
          mutex::scoped_lock waitLock(mWaitCountMutex);
          ++mWaitCount;
      }

      DWORD ret = ::WaitForSingleObject(event_, waitMs);
      assert(ret != WAIT_ABANDONED && ret != WAIT_FAILED);

      {
          mutex::scoped_lock waitLock(mWaitCountMutex);
          --mWaitCount;
          if (ret != WAIT_TIMEOUT && mWaitCount == 0)
          {
              // Set signalled state back to false.
              ::ResetEvent(event_);
          }
      }

      lock.lock();
      if (ret == WAIT_TIMEOUT)
      {
          return false;
      }

      return true;
  }

  // Signal the event.
  template <typename Lock>
  void notify_all(Lock& lock)
  {
      signal_all(lock);
  }

protected:

  // Newer versions of Windows have built in condition variables (InitializeConditionVariable() etc)
  // but they are not supported in XP, Server 2003, and older. So for now we use
  // events instead, with the mWatiCount workaround below for resetting the event.
  HANDLE event_;

  // Number of threads waiting on the event. When the threads are signalled,
  // the counter is decremented, and the thread that decrements the counter
  // to zero will reset the event. The event is a "manual reset" event, as
  // an "auto reset" event only releases 1 thread at a time.
  mutex mWaitCountMutex;
  int mWaitCount;
};

} // namespace detail
} // namespace RCF

#include <RCF/thread/pop_options.hpp>

#endif // defined(BOOST_WINDOWS)

#endif // RCF_DETAIL_WIN_EVENT_HPP

max_stars_repo_name: ASMlover/study

max_issues_repo_name: ASMlover/study

max_forks_repo_name: ASMlover/study


text:
/*    Lua2Bmp Kernel V3
 *
 *    Copyright 2014 wtof1996
 *    Licensed under the Apache License, Version 2.0 (the "License");
 *    you may not use this file except in compliance with the License.
 *    You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing, software
 *    distributed under the License is distributed on an "AS IS" BASIS,
 *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *    See the License for the specific language governing permissions and
 *    limitations under the License.
 *
 *     this software is based in part on the work of the Independent JPEG Group.
 *     this software is based in part on the libpng library.
 *     this software is based in part on the Boost library.
 */

#include "io.hpp"
#include "draw.hpp"

#include <boost/format.hpp>
#include <boost/progress.hpp>
#include <sstream>
#include <bitset>

#include <cctype>

using std::vector;
using std::string;
using std::bitset;
using std::make_pair;
using boost::iequals;
using boost::progress_display;
using namespace boost::gil;
namespace file = boost::filesystem;

unsigned int readByte(file::ifstream &fin)
{
    char read;
    unsigned char ret = 0;

    fin.get(read);
    if(read == '\\'){
        for(int i = 0; i < 3; ++i){
            fin.get(read);
            if(isdigit(read) == 0){
                fin.unget();
                break;
            }
            ret = ret * 10 + (read - '0');
        }
    }
    else if(read == '\"'){
        while(fin.get(read), read == '\"');
        fin.unget();
        return io::NIL;
    }
    else if(read == 0x0d || read == 0x0a) return io::NIL;
    else ret = read;
    return ret;
}

unsigned char io::getByte(file::ifstream &fin)
{
    unsigned int ret;
    do
        ret = readByte(fin);
    while(ret == io::NIL);

    return static_cast<unsigned char>(ret);
}

rgb8_pixel_t io::RGB555_to_RGB888(const io::MyByte &p)
{
    bitset<8> low(p.first);
    bitset<8> high(p.second);

    string byte_seq(low.to_string() + high.to_string());

    bitset<5> r(byte_seq.substr(1, 5));
    bitset<5> g(byte_seq.substr(6, 5));
    bitset<5> b(byte_seq.substr(11, 5));

    return rgb8_pixel_t(r.to_ulong() << 3, g.to_ulong() << 3, b.to_ulong() << 3);
}
void io::output(Image &img)
{
    string ext = file::extension(Settings::get().getOutPath());

    io::notice("Writing to:  " + Settings::get().getOutPath().string());

    if(iequals(ext, ".jpeg") || iequals(ext, ".jpg")){
        try{
            jpeg_write_view(Settings::get().getOutPath().string(), img.getView());
        }
        catch(std::ios_base::failure e){
            io::log("Failed to write the file,please check the filename and try again.");
            exit(EXIT_FAILURE);
        }

    }
    else if(iequals(ext, ".png")){
        try{
            png_write_view(Settings::get().getOutPath().string(), img.getView());
        }
        catch(std::ios_base::failure e){
            io::log("Failed to write the file,please check the filename and try again.");
            exit(EXIT_FAILURE);
        }
    }
    else{
        io::log("Unknown error, terminated.");
        exit(EXIT_FAILURE);
    }

    io::notice("Convert Complete.");
}

void io::Image::readLuaImage()
{
    auto get_char_bits = [](char &&s){return bitset<8>(s).to_string();};
    auto get_ulong = [](string &&s){return bitset<32>(s).to_ulong();};

    file::ifstream fin(Settings::get().getInPath());

    if(!fin){
        io::log("Cannot open input file.");
        exit(EXIT_FAILURE);
    }

    ///READHEADER

    io::notice("Reading header......", false);

    for(size_t i = 0; i < 20; ++i){
        header[i] = getByte(fin);

    }

    std::stringstream s;
    for(int i = 3; i >= 0; --i) s << get_char_bits(header[i]);
    size.first = get_ulong(s.str());
    s.str("");

    for(size_t i = 7; i >= 4; --i) s << get_char_bits(header[i]);
    size.second = get_ulong(s.str());

    io::notice("Complete.");
    io::notice("Image Width:", false);
    io::notice(size.first);
    io::notice("Image Height:", false);
    io::notice(size.second);

    ///READDATA

    io::notice("Reading data......", false);

    auto w = size.first, h = size.second;
    progress_display status(w * h);

    for(decltype(h) y = 0; y < h; ++y){
        for(decltype(w) x = 0; x < w; ++x){
            auto l = getByte(fin), h = getByte(fin);
            data.push_back(make_pair(h, l));
            ++status;
        }
    }

}

void io::Image::convert()
{
    rgb8_image_t tmp_img(size.first, size.second);
    rgb8_view_t tmp_view = view(tmp_img);
    auto w = size.first, h = size.second;
    auto i = data.begin();

    io::notice("Converting.........", false);

    progress_display status(w * h);
    for(decltype(h) y = 0; y < h; ++y){
        for(decltype(w) x = 0; x < w; ++x){
            tmp_view(x, y) = io::RGB555_to_RGB888(*i++);
            ++status;
        }
    }

    src = tmp_img;
    src_view = view(src);
}


text:
// Copyright 2020, 2021 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/system/error_code.hpp>
#include <boost/system/generic_category.hpp>
#include <boost/system/system_category.hpp>
#include <boost/core/lightweight_test.hpp>

namespace sys = boost::system;

int main()
{
    {
        sys::error_code ec;

        BOOST_TEST_EQ( ec.value(), 0 );
        BOOST_TEST( ec.category() == sys::system_category() );

        BOOST_TEST_EQ( ec.what(), ec.message() + " [system:0]" );
    }

    {
        sys::error_code ec( 5, sys::generic_category() );

        BOOST_TEST_EQ( ec.value(), 5 );
        BOOST_TEST( ec.category() == sys::generic_category() );

        BOOST_TEST_EQ( ec.what(), ec.message() + " [generic:5]" );
    }

    {
        sys::error_code ec( 5, sys::system_category() );

        BOOST_TEST_EQ( ec.value(), 5 );
        BOOST_TEST( ec.category() == sys::system_category() );

        BOOST_TEST_EQ( ec.what(), ec.message() + " [system:5]" );
    }

    {
        BOOST_STATIC_CONSTEXPR boost::source_location loc = BOOST_CURRENT_LOCATION;

        sys::error_code ec( 5, sys::generic_category(), &loc );

        BOOST_TEST_EQ( ec.value(), 5 );
        BOOST_TEST( ec.category() == sys::generic_category() );
        BOOST_TEST_EQ( &ec.location(), &loc );

        BOOST_TEST_EQ( ec.what(), ec.message() + " [generic:5 at " + loc.to_string() + "]" );
    }

    {
        BOOST_STATIC_CONSTEXPR boost::source_location loc = BOOST_CURRENT_LOCATION;

        sys::error_code ec( 5, sys::system_category(), &loc );

        BOOST_TEST_EQ( ec.value(), 5 );
        BOOST_TEST( ec.category() == sys::system_category() );
        BOOST_TEST_EQ( &ec.location(), &loc );

        BOOST_TEST_EQ( ec.what(), ec.message() + " [system:5 at " + loc.to_string() + "]" );
    }

#if defined(BOOST_SYSTEM_HAS_SYSTEM_ERROR)

    {
        std::error_code ec;
        sys::error_code ec2( ec );

        BOOST_TEST_EQ( ec2.what(), ec2.message() + " [std:system:0]" );
    }

    {
        std::error_code ec( 5, std::generic_category() );
        sys::error_code ec2( ec );

        BOOST_TEST_EQ( ec2.what(), ec2.message() + " [std:generic:5]" );
    }

    {
        std::error_code ec( 5, std::system_category() );
        sys::error_code ec2( ec );

        BOOST_TEST_EQ( ec2.what(), ec2.message() + " [std:system:5]" );
    }

#endif

    return boost::report_errors();
}


text:
#pragma once

#include <bacs/statement_provider/request.pb.h>

#include <bunsan/pm/cache.hpp>
#include <bunsan/web/mime_file.hpp>

#include <botan/pk_keys.h>

#include <booster/regex.h>
#include <cppcms/application.h>

#include <boost/filesystem/path.hpp>

#include <memory>

#include <ctime>

namespace bacs {
namespace statement_provider {
namespace web {

class server : public cppcms::application {
 public:
  server(cppcms::service &srv, const std::shared_ptr<bunsan::pm::cache> &cache);

 private:
  void get_index(std::string signed_request);
  void get(std::string signed_request, std::string path);

  bool get_index_and_root(const std::string &signed_request,
                          bunsan::pm::cache::entry *cache_entry = nullptr,
                          boost::filesystem::path *index = nullptr,
                          boost::filesystem::path *data_root = nullptr);

  bool verify_and_parse(const std::string &signed_request, Request &request);
  bool verify(const std::string &coded_request, const std::string &referrer,
              const std::string &signature);
  bool parse(const std::string &getter, const std::string &coded_request,
             Request &request);
  bool parse_binary(const std::string &coded_request, Request &request);
  bool parse_text(const std::string &coded_request, Request &request);

 private:
  const booster::regex m_request_regex;
  const std::shared_ptr<bunsan::pm::cache> m_cache;
  const bunsan::web::mime_file m_mime_file;
  std::unordered_map<std::string, std::unique_ptr<Botan::Public_Key>> m_keys;
};

}  // namespace web
}  // namespace statement_provider
}  // namespace bacs