#include "gcodetovhf.h"

#include <QList>
#include <cassert>
#include <cmath>
#include <QVector2D>
#include <QVector3D>
#include <QDebug>

static constexpr double TOLLERANCE = 0.005;

QByteArray removeComments(const QByteArray& gcodeCommand)
{
	size_t comment = 0;
	QByteArray output;
	for(size_t i = 0; i < gcodeCommand.size(); ++i)
	{
		if(gcodeCommand[i] == '(')
			++comment;
		else if(gcodeCommand[i] == ')' && comment > 0)
			--comment;

		if(comment == 0)
			output.push_back(gcodeCommand[i]);
	}
	return output;
}

class Gmove
{
public:
	int gmode = 0;
	int coordmode = 54;
	bool relative = false;
	double xStart = 0;
	double yStart = 0;
	double zStart = 0;
	double aStart = 0;

	bool x = false;
	bool y = false;
	bool z = false;
	bool a = false;
	bool i = false;
	bool j = false;
	bool p = false;
	bool r = false;
	bool l = false;

	double xVal;
	double yVal;
	double zVal;
	double aVal;
	double iVal;
	double jVal;
	double pVal;
	double rVal;
	double lVal;

	void fill()
	{
		if(!x)
			xVal = relative ? 0 : xStart;
		if(!y)
			yVal = relative ? 0 : yStart;
		if(!z)
			zVal = relative ? 0 : zStart;
		if(!a)
			aVal = relative ? 0 : aStart;

		x = true;
		y = true;
		z = true;
	}
	void makeAbsolute()
	{
		if(!relative)
			return;
		if(x)
			xVal = xVal+xStart;
		if(y)
			yVal = yVal+yStart;
		if(z)
			zVal = zVal+zStart;
		if(a)
			aVal = aVal+aStart;
		relative = false;
	}
};

static QString checkMove(Gmove gmove)
{
	if(gmove.gmode == 0 || gmove.gmode == 1)
	{
		if(!gmove.x && !gmove.y && !gmove.z)
			return "At least one coordinate is required for linear move.";
		else if(gmove.i || gmove.j || gmove.p || gmove.r)
			return "Invalid parameter in linear move.";
	}
	else if(gmove.gmode == 2 || gmove.gmode == 3)
	{
		if(!gmove.x && !gmove.y && !gmove.z)
			return "At least one coordinate is required for arc move.";
		else if(!gmove.i && !gmove.j && !gmove.r)
			return "No arc center or radius defined.";
		else if((gmove.i || gmove.j) && gmove.r)
			return "More than one arc center defined.";

		gmove.fill();
		gmove.makeAbsolute();
		QVector2D xyVect(gmove.xVal, gmove.yVal);
		QVector2D startVect(gmove.xStart, gmove.yStart);

		if(gmove.r)
		{
			if(gmove.rVal < TOLLERANCE)
				return "Arc radius must be larger than zero.";
			if((xyVect-startVect).length()/2 > gmove.rVal+TOLLERANCE)
				return "Arc radius to small to fit to provided sart and end points.";
		}
		else
		{
			/*QVector2D ijVect(gmove.i ? gmove.iVal : 0, gmove.j ? gmove.jVal : 0);
			QVector2D center = startVect+ijVect;
			double r = (center-xyVect).length();
			double r2 = (center-startVect).length();

			if(abs(r-r2) > TOLLERANCE)
				return "Arc start and end point radii differ";*/
		}
	}

	return QString();
}

static QByteArray generateLinearCommand(const Gmove& gmove)
{
	assert(gmove.gmode == 0 || gmove.gmode == 1);

	QByteArray output;
	if(gmove.gmode == 0)
	{
		if(gmove.relative)
			output.append("GR");
		else if(gmove.coordmode != 53)
			output.append("GA");
		else
			output.append("GB");
	}
	else if(gmove.gmode == 1)
	{
		if(gmove.relative)
			output.append("PR");
		else if(gmove.coordmode != 53)
			output.append("PA");
		else
			output.append("PB");
	}
	else
	{
		return output;
	}

	if(gmove.x)
		output.append(QByteArray::number(static_cast<int>(gmove.xVal*1000)));
	if(gmove.y || gmove.z || gmove.a)
		output.append(',');
	if(gmove.y)
		output.append(QByteArray::number(static_cast<int>(gmove.yVal*1000)));
	if(gmove.z || gmove.a)
		output.append(',');
	if(gmove.z)
		output.append(QByteArray::number(static_cast<int>(gmove.zVal*-1000)));
	if(gmove.a)
	{
		output.append(',');
		output.append(QByteArray::number(static_cast<int>(gmove.aVal*1000)));
	}

	output.append(';');

	return output;
}

static bool findCircleCenter(QVector2D a, QVector2D b, double r, bool leftHanded, QVector2D& result)
{	float distance = (a-b).length();
	if(distance/2 > r+TOLLERANCE)
		return false;

	QVector2D baseVect = (b-a).normalized();
	float loftDist = sqrt(pow(r, 2)-pow(distance/2, 2));
	QVector2D loftVect;
	if(leftHanded)
	{
		loftVect.setX(baseVect.y()*-1);
		loftVect.setY(baseVect.x());
	}
	else
	{
		loftVect.setX(baseVect.y());
		loftVect.setY(baseVect.x()*-1);
	}
	loftVect = loftVect*loftDist;
	baseVect = baseVect*(distance/2);
	result = a+baseVect+loftVect;

	return true;
}

static bool withinTollerance(double a, double b)
{
	return a+TOLLERANCE > b && a-TOLLERANCE < b;
}

static QByteArray generateArcCommand(Gmove gmove, size_t resolution)
{
	assert(gmove.gmode == 2 || gmove.gmode == 3);
	gmove.fill();
	gmove.makeAbsolute();

	QVector2D startMove(gmove.xStart, gmove.yStart);
	QVector2D endMove(gmove.xVal, gmove.yVal);
	QVector2D center;
	double radius;
	if(gmove.i && gmove.j)
	{
		center.setX(gmove.xStart+gmove.iVal);
		center.setY(gmove.yStart+gmove.jVal);
		radius = (QVector2D(gmove.xStart, gmove.yStart)-center).length();
	}
	else if(gmove.r)
	{
		findCircleCenter(QVector2D(gmove.xStart, gmove.yStart), QVector2D(gmove.xVal, gmove.yVal), gmove.rVal, gmove.gmode == 3, center);
		radius = gmove.rVal;
	}
	else
	{
		qCritical()<<"Can't create arc move";
		return QByteArray();
	}

	QVector2D aVector = startMove-center;
	QVector2D bVector = endMove-center;
	double startAngle = acos(aVector.x()/(aVector.length()));
	double endAngle = acos(bVector.x()/(bVector.length()));

	{
		bool a = false;
		bool b = false;
		QVector2D computedEnpoint(cos(endAngle)*radius+center.x(), sin(endAngle)*radius+center.y());
		if((computedEnpoint-endMove).length() > TOLLERANCE)
			endAngle = 0 - endAngle;

		QVector2D computedSart(cos(startAngle)*radius+center.x(), sin(startAngle)*radius+center.y());
		if((computedSart-startMove).length() > TOLLERANCE)
			startAngle = 0 - startAngle;

		if((startAngle-endAngle) < 0)
			startAngle = startAngle+2*M_PI;
	}
	{
		/*QVector2D computedEnpoint(cos(endAngle)*radius+center.x(), sin(endAngle)*radius+center.y());
		assert((computedEnpoint-endMove).length() > TOLLERANCE);

		QVector2D computedSart(cos(startAngle)*radius+center.x(), sin(startAngle)*radius+center.y());
		assert((computedSart-startMove).length() > TOLLERANCE);*/
	}


	if(gmove.p && gmove.pVal > 1)
		endAngle = endAngle+std::copysign(1.0, endAngle-startAngle)*2*(gmove.pVal-1)*M_PI;

	size_t steps = (resolution*(fabs(endAngle-startAngle)))/(2*M_PI);

	double angleStep;
	if(gmove.gmode == 2)
		angleStep = (endAngle-startAngle)/steps;
	else
		angleStep = (2*M_PI-(startAngle-endAngle))/steps;

	QByteArray output;
	double deltaZ = (gmove.zVal - gmove.zStart)/steps;
	for(size_t i = 1; i < steps; ++i)
	{
		double workAngle = startAngle+angleStep*i;
		QVector2D point;
		point.setX(cos(workAngle)*radius);
		point.setY(sin(workAngle)*radius);
		point = point+center;
		QByteArray command("PA");
		command.append(QByteArray::number(static_cast<int>(point.x()*1000)) + "," +
					   QByteArray::number(static_cast<int>(point.y()*1000)) + "," +
					   QByteArray::number(static_cast<int>((gmove.zStart+deltaZ*i)*-1000)) + ";");
		output.append(command);
	}

	QByteArray command("PA");
	command.append(QByteArray::number(static_cast<int>(gmove.xVal*1000)) +"," +
				   QByteArray::number(static_cast<int>(gmove.yVal*1000)) + "," +
				   QByteArray::number(static_cast<int>(gmove.zVal*-1000)) + ";");
	output.append(command);

	return output;
}

static QByteArray generateDrillingCycle(Gmove gmove)
{
	if(!gmove.r)
		return QByteArray();
	if(!gmove.z)
		return QByteArray();
	gmove.gmode = 0;
	if(gmove.relative)
		gmove.rVal = gmove.rVal+gmove.zVal;
	gmove.makeAbsolute();

	QByteArray output("GA,,"+QByteArray::number(static_cast<int>(gmove.rVal*-1000))+"; ");
	if(gmove.x || gmove.y)
	{
		gmove.z = false;
		output.append(generateLinearCommand(gmove));
	}

	size_t count = gmove.l ? gmove.lVal : 1;

	for(size_t i = 0; i < count; ++i)
	{
		output.append("PA,,"+QByteArray::number(static_cast<int>(gmove.zVal*-1000))+"; ");
		output.append("GA,,"+QByteArray::number(static_cast<int>(gmove.rVal*-1000))+"; ");
	}
	return output;
}

static QByteArray generateMoveCommand(const Gmove& gmove)
{
	if(gmove.gmode == 0 || gmove.gmode == 1)
		return generateLinearCommand(gmove);
	else if(gmove.gmode == 2 || gmove.gmode == 3)
		return generateArcCommand(gmove, 100);
	else if(gmove.gmode == 81)
		return generateDrillingCycle(gmove);
	return QByteArray();
}

static Gmove doMove(Gmove gmove)
{
	gmove.fill();
	gmove.makeAbsolute();

	Gmove out;
	out.xStart = gmove.xVal;
	out.yStart = gmove.yVal;
	out.zStart = gmove.zVal;
	out.aStart = gmove.aVal;

	return out;
}

QByteArray gcodeToVhf(const QByteArray& gcode, bool* ok, QList<QString>* errors)
{
	int gmode = -1;
	int coordmode = 54;
	bool relmode = false;
	double spinspeed = 1000;
	double feedrate = std::numeric_limits<double>::min();
	int tool = 0;
	Gmove move;
	bool clearMove;

	QByteArray output;

	if(gcode.size()< 2)
		return output;

	QList<QByteArray> gcodeCommands = gcode.toUpper().split('\n');

	if(ok)
		*ok = true;

	size_t line = 0;
	for(QByteArray& command : gcodeCommands)
	{
		++line;
		move.gmode = gmode;
		move.coordmode = coordmode;
		move.relative = relmode;
		bool startSpindle = false;
		bool toolchange = false;
		command = removeComments(command);

		QList<QByteArray> subcommands = command.split(' ');
		for(QByteArray& subcommand : subcommands)
		{
			subcommand = subcommand.trimmed();
			if(subcommand.size() < 1)
				continue;
			if(subcommand[0] == 'G')
			{
				subcommand.remove(0, 1);
				int gcodeCode = subcommand.toInt();
				if(gcodeCode >= 0 && gcodeCode <= 3 || gcodeCode == 81)
					gmode = gcodeCode;
				else if(gcodeCode == 80 && gmode == 81)
					gmode = 0;
				else if(gcodeCode == 91)
					relmode = true;
				else if(gcodeCode == 90)
					relmode = false;
				else if(gcodeCode == 53)
					move.coordmode = 53;
				else if(gcodeCode > 54 && gcodeCode < 59)
					move.coordmode = gcodeCode;
				else if(gcodeCode == 28)
					output.append("PB0,0,0,0; ");
				else if(gcodeCode == 64)
					clearMove = true;

				move.gmode = gmode;
				if(move.coordmode != 53)
					move.coordmode = coordmode;
				move.relative = relmode;
			}
			else if(subcommand[0] == 'M')
			{
				subcommand.remove(0, 1);
				int mCode = subcommand.toDouble();
				switch(mCode)
				{
					case 0:
					case 1:
						output.append("!S; ");
						break;
					case 3:
						startSpindle = true;
						break;
					case 2:
					case 5:
						output.append("RVS0; ");
						startSpindle = false;
						break;
					case 6:
						toolchange = true;
					default:
						break;
				}
			}
			else if(subcommand[0] == 'F')
			{
				subcommand.remove(0, 1);
				double newFeedrate = subcommand.toDouble();
				if(abs(newFeedrate - feedrate) > TOLLERANCE)
				{
					feedrate = newFeedrate;
					output.append("VS" + QByteArray::number(static_cast<int>(feedrate*(1000.0/60.0))) + "; ");
				}
			}
			else if(subcommand[0] == 'T')
			{
				subcommand.remove(0, 1);
				tool = subcommand.toInt();
			}
			else if(subcommand[0] == 'S')
			{
				subcommand.remove(0, 1);
				spinspeed = subcommand.toDouble();
			}
			else if(subcommand[0] == 'X')
			{
				subcommand.remove(0, 1);
				move.x = true;
				move.xVal = subcommand.toDouble();
			}
			else if(subcommand[0] == 'Y')
			{
				subcommand.remove(0, 1);
				move.y = true;
				move.yVal = subcommand.toDouble();
			}
			else if(subcommand[0] == 'Z')
			{
				subcommand.remove(0, 1);
				move.z = true;
				move.zVal = subcommand.toDouble();
			}
			else if(subcommand[0] == 'P')
			{
				subcommand.remove(0, 1);
				move.p = true;
				move.pVal = subcommand.toDouble();
			}
			else if(subcommand[0] == 'A')
			{
				subcommand.remove(0, 1);
				move.a = true;
				move.aVal = subcommand.toDouble();
			}
			else if(subcommand[0] == 'I')
			{
				subcommand.remove(0, 1);
				move.i = true;
				move.iVal = subcommand.toDouble();
			}
			else if(subcommand[0] == 'J')
			{
				subcommand.remove(0, 1);
				move.j = true;
				move.jVal = subcommand.toDouble();
			}
			else if(subcommand[0] == 'R')
			{
				subcommand.remove(0, 1);
				move.r = true;
				move.rVal = subcommand.toDouble();
			}
		}

		if(toolchange)
			output.append("T" + QByteArray::number(static_cast<int>(tool)) + "; ");
		if(startSpindle)
			output.append("RVS" + QByteArray::number(static_cast<int>(spinspeed)) + "; ");
		if(move.x || move.y || move.z || move.a)
		{
			QString error = checkMove(move);
			if(error.isEmpty())
			{
				output.append(generateMoveCommand(move));
			}
			else
			{
				*ok = false;
				if(errors)
					errors->push_back("Error on line " + QString::number(line) + ": " + error);
				qWarning()<<"Error on line"<<line<<error;
			}
			move = doMove(move);
		}
		else if(clearMove)
			move = doMove(move);
		if(output.size() > 0 && output.back() != '\n')
			output.push_back('\n');
	}

	return output;
}