#include "MBDynItem.h"
#include "MBDynSystem.h"
#include "SymbolicParser.h"
#include "BasicUserFunction.h"
#include "EulerAngles.h"
#include "Constant.h"
#include "MBDynReference.h"
#include "FullMatrix.h"
#include "ASMTItem.h"
#include "MBDynBody.h"


using namespace MbD;

MBDynSystem* MbD::MBDynItem::root()
{
	return nullptr;
}

void MbD::MBDynItem::initialize()
{
	assert(false);
}

void MbD::MBDynItem::noop()
{
	//No Operations
}

void MbD::MBDynItem::parseMBDyn(std::vector<std::string>& lines)
{
	assert(false);
}

std::vector<std::string>::iterator MbD::MBDynItem::findLineWith(std::vector<std::string>& lines, std::vector<std::string>& tokens)
{
	auto it = std::find_if(lines.begin(), lines.end(), [&](const std::string& line) {
		return lineHasTokens(line, tokens);
		});
	return it;
}

bool MbD::MBDynItem::lineHasTokens(const std::string& line, std::vector<std::string>& tokens)
{
	size_t index = 0;
	for (auto& token : tokens) {
		index = line.find(token, index);
		if (index == std::string::npos) return false;
		index++;
	}
	return true;
}

std::shared_ptr<std::vector<std::shared_ptr<MBDynNode>>> MbD::MBDynItem::mbdynNodes()
{
	return owner->mbdynNodes();
}

std::vector<std::string> MbD::MBDynItem::nodeNames()
{
	return owner->nodeNames();
}

std::shared_ptr<std::map<std::string, Symsptr>> MbD::MBDynItem::mbdynVariables()
{
	return owner->mbdynVariables();
}

std::shared_ptr<std::map<std::string, std::shared_ptr<MBDynReference>>> MbD::MBDynItem::mbdynReferences()
{
	return owner->mbdynReferences();
}

void MbD::MBDynItem::createASMT()
{
	assert(false);
}

std::shared_ptr<MBDynNode> MbD::MBDynItem::nodeAt(std::string nodeName)
{
	return owner->nodeAt(nodeName);
}

int MbD::MBDynItem::nodeidAt(std::string nodeName)
{
	return owner->nodeidAt(nodeName);
}

std::shared_ptr<MBDynBody> MbD::MBDynItem::bodyWithNode(std::string nodeName)
{
	return owner->bodyWithNode(nodeName);
}

std::shared_ptr<ASMTAssembly> MbD::MBDynItem::asmtAssembly()
{
	return owner->asmtAssembly();
}

FColDsptr MbD::MBDynItem::readVector3(std::vector<std::string>& args)
{
	auto parser = std::make_shared<SymbolicParser>();
	parser->variables = mbdynVariables();
	auto rFfF = std::make_shared<FullColumn<double>>(3);
	auto& str = args.at(0);
	if (str.find("null") != std::string::npos) {
		args.erase(args.begin());
	}
	else {
		for (int i = 0; i < 3; i++)
		{
			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
			parser->parseUserFunction(userFunc);
			auto sym = parser->stack->top();
			rFfF->at(i) = sym->getValue();
		}

	}
	return rFfF;
}

FColDsptr MbD::MBDynItem::readPosition(std::vector<std::string>& args)
{
	auto rOfO = std::make_shared<FullColumn<double>>(3);
	if (args.empty()) return rOfO;
	auto& str = args.at(0);
	if (str.find("orientation") != std::string::npos) {
		//Do nothing
	}
	else if (str.find("reference") != std::string::npos) {
		args.erase(args.begin());
		auto refName = readStringOffTop(args);
		auto ref = mbdynReferences()->at(refName);
		auto rFfF = readBasicPosition(args);
		auto rOFO = ref->rOfO;
		auto aAOF = ref->aAOf;
		rOfO = rOFO->plusFullColumn(aAOF->timesFullColumn(rFfF));
	}
	else if (str.find("offset") != std::string::npos) {
		args.erase(args.begin());
		rOfO = readBasicPosition(args);
	}
	else if (str.find("null") != std::string::npos) {
		args.erase(args.begin());
	}
	else {
		rOfO = readBasicPosition(args);
	}
	return rOfO;
}

FColDsptr MbD::MBDynItem::readBasicPosition(std::vector<std::string>& args)
{
	return readVector3(args);
}

FMatDsptr MbD::MBDynItem::readOrientation(std::vector<std::string>& args)
{
	auto aAOf = FullMatrix<double>::identitysptr(3);
	if (args.empty()) return aAOf;
	auto& str = args.at(0);
	if (str.find("reference") != std::string::npos) {
		args.erase(args.begin());
		auto refName = readStringOffTop(args);
		auto ref = mbdynReferences()->at(refName);
		auto aAFf = readBasicOrientation(args);
		auto aAOF = ref->aAOf;
		aAOf = aAOF->timesFullMatrix(aAFf);
	}
	else if (str.find("hinge") != std::string::npos) {
		args.erase(args.begin());
		aAOf = readOrientation(args);
	}
	else if (str.find("orientation") != std::string::npos) {
		args.erase(args.begin());
		aAOf = readOrientation(args);
	}
	else {
		aAOf = readBasicOrientation(args);
	}
	return aAOf;
}

FMatDsptr MbD::MBDynItem::readBasicOrientation(std::vector<std::string>& args)
{
	auto parser = std::make_shared<SymbolicParser>();
	parser->variables = mbdynVariables();
	auto& str = args.at(0);
	if (str.find("euler") != std::string::npos) {
		args.erase(args.begin());
		auto euler = std::make_shared<EulerAngles<Symsptr>>();
		euler->rotOrder = std::make_shared<FullColumn<int>>(std::initializer_list<int>{ 1, 2, 3 });
		for (int i = 0; i < 3; i++)
		{
			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
			parser->parseUserFunction(userFunc);
			auto sym = parser->stack->top();
			euler->at(i) = sym;
		}
		euler->calc();
		auto aAFf = euler->aA;
		return aAFf;
	}
	if (str.find("eye") != std::string::npos) {
		args.erase(args.begin());
		auto aAFf = FullMatrix<double>::identitysptr(3);
		return aAFf;
	}
	auto iss = std::istringstream(str);
	int integer;
	iss >> integer;
	if (integer == 1) {
		args.erase(args.begin());
		FColDsptr vecX, vecY, vecZ, vec;
		vecX = readPosition(args);
		vecX->normalizeSelf();
		auto axis = stoi(popOffTop(args));
		str = args.at(0);
		if (str.find("guess") != std::string::npos) {
			args.erase(args.begin());
			double min = std::numeric_limits<double>::max();
			double max = -1.0;
			int imin, imax;
			for (int i = 0; i < 3; i++)
			{
				auto mag = std::abs(vecX->at(i));
				if (mag > max) {
					imax = i;
					max = mag;
				}
				if (mag < min) {
					imin = i;
					min = mag;
				}
			}
			vec = std::make_shared<FullColumn<double>>(3);
			vec->at(imin) = 1.0;
			vec->at(imax) = -vecX->at(imin) / vecX->at(imax);
		}
		else {
			vec = readPosition(args);
		}
		vec->normalizeSelf();
		if (axis == 2) {
			vecZ = vecX->cross(vec);
			vecY = vecZ->cross(vecX);
		}
		else if (axis == 3) {
			vecY = vec->cross(vecX);
			vecZ = vecX->cross(vecY);
		}
		else {
			assert(false);
		}
		auto aAFf = FullMatrix<double>::identitysptr(3);
		aAFf->atijputFullColumn(0, 0, vecX);
		aAFf->atijputFullColumn(0, 1, vecY);
		aAFf->atijputFullColumn(0, 2, vecZ);
		return aAFf;
	}
	if (integer == 3) {
		args.erase(args.begin());
		FColDsptr vecX, vecY, vecZ, vec;
		vecZ = readPosition(args);
		vecZ->normalizeSelf();
		auto axis = stoi(popOffTop(args));
		str = args.at(0);
		if (str.find("guess") != std::string::npos) {
			args.erase(args.begin());
			double min = std::numeric_limits<double>::max();
			double max = -1.0;
			int imin, imax;
			for (int i = 0; i < 3; i++)
			{
				auto mag = std::abs(vecZ->at(i));
				if (mag > max) {
					imax = i;
					max = mag;
				}
				if (mag < min) {
					imin = i;
					min = mag;
				}
			}
			vec = std::make_shared<FullColumn<double>>(3);
			vec->at(imin) = 1.0;
			vec->at(imax) = -vecZ->at(imin) / vecZ->at(imax);
		}
		else {
			vec = readPosition(args);
		}
		vec->normalizeSelf();
		if (axis == 2) {
			vecX = vec->cross(vecZ);
			vecY = vecZ->cross(vecX);
		}
		else if (axis == 1) {
			vecY = vecZ->cross(vec);
			vecX = vecY->cross(vecZ);
		}
		else {
			assert(false);
		}
		auto aAFf = FullMatrix<double>::identitysptr(3);
		aAFf->atijputFullColumn(0, 0, vecX);
		aAFf->atijputFullColumn(0, 1, vecY);
		aAFf->atijputFullColumn(0, 2, vecZ);
		return aAFf;
	}
	auto aAFf = FullMatrix<double>::identitysptr(3);
	for (int i = 0; i < 3; i++)
	{
		auto rowi = aAFf->at(i);
		for (int j = 0; j < 3; j++)
		{
			auto userFunc = std::make_shared<BasicUserFunction>(popOffTop(args), 1.0);
			parser->parseUserFunction(userFunc);
			auto sym = parser->stack->top();
			rowi->at(j) = sym->getValue();
		}
	}
	return aAFf;
}

std::string MbD::MBDynItem::popOffTop(std::vector<std::string>& args)
{
	auto str = args.at(0);
	args.erase(args.begin());
	return str;
}

std::string MbD::MBDynItem::readStringOffTop(std::vector<std::string>& args)
{
	auto iss = std::istringstream(args.at(0));
	args.erase(args.begin());
	std::string str;
	iss >> str;
	return str;
}
