#include "vhfmill.h"
#include <QDebug>

VhfMill::VhfMill(QIODevice* serial, uint8_t nAxis, QObject *parent)
	: serial_(serial), nAxis_(nAxis), QObject{parent}
{
	QObject::connect(serial_, &QIODevice::readyRead, this, &VhfMill::serialReadyRead);

	qDebug()<<__func__<<" object created";

	lastKnownPos_.assign(5, 0);
	touchoffPos_.assign(5, 0);

	initTimer.callOnTimeout(this, &VhfMill::initTimerFn);
	initTimer.setInterval(3000);

	stateTimer.callOnTimeout(this, &VhfMill::stateTimerFn);
	stateTimer.setInterval(1000);

	positionTimer.callOnTimeout(this, &VhfMill::reqPosition);
	positionTimer.setInterval(100);

	limits_ = {190000, 92000, 85000, 100000};
}

void VhfMill::serialReadyRead()
{
	char charBuf;
	while(serial_->getChar(&charBuf))
	{
		lineBuffer_.push_back(charBuf);
		if(lineBuffer_.endsWith(';') )
		{
			lineBuffer_.chop(1);
			processIncomeingLine();
			gotLine(lineBuffer_);
			lineBuffer_.clear();
		}
	}
}

void VhfMill::init()
{
	initTimerFn();
	initTimer.start();
}

void VhfMill::setSpindleSpeed(int speed)
{
	serial_->write("RVS"+QByteArray::number(speed)+";\n");
	serial_->write("?RV;");
}

void VhfMill::setjogStep(int step)
{
	jogStep_ = step;
	jogStepChanged(jogStep_);
}

VhfMill::Axis VhfMill::homed() const
{
	return axisHomed_;
}

void VhfMill::jog(VhfMill::Axis axis, int jogDirection)
{
	if(homed() & axis)
	{
		if(jogStep_ > 0 && jogDirection != 0 && jogAxis_ == AXIS_NONE)
		{
			QByteArray command("GR");
			if(axis == AXIS_X)
				command.append(QByteArray::number(jogStep_*jogDirection*-1));
			else
				command.push_back('0');
			command.push_back(',');
			if(axis == AXIS_Y)
				command.append(QByteArray::number(jogStep_*jogDirection));
			else
				command.push_back('0');
			command.push_back(',');
			if(axis == AXIS_Z)
				command.append(QByteArray::number(jogStep_*jogDirection*-1));
			else
				command.push_back('0');
			command.push_back(',');
			if(axis == AXIS_A)
				command.append(QByteArray::number(jogStep_*jogDirection*-1));
			else
				command.push_back('0');
			command.push_back(';');
			send(command);
		}
		else
		{
			if(jogAxis_ == axis && jogDirection == 0)
			{
				serial_->write("!M;");
				jogDirection_ = 0;
				jogAxis_ = AXIS_NONE;
			}
			else if(jogAxis_ == AXIS_NONE && jogDirection != 0)
			{
				jogAxis_ = axis;
				jogDirection_ = jogDirection;
				jogDirection = jogDirection == -1 ? 1 : 0;
				QByteArray command("GB");
				if(axis == AXIS_X)
					command.append(QByteArray::number(getLimits()[0]*jogDirection));
				else
					command.push_back(QByteArray::number(getLastKnownPosition()[0]));
				command.push_back(',');
				if(axis == AXIS_Y)
					command.append(QByteArray::number(getLimits()[1]*jogDirection));
				else
					command.push_back(QByteArray::number(getLastKnownPosition()[1]));
				command.push_back(',');
				if(axis == AXIS_Z)
					command.append(QByteArray::number(getLimits()[2]*jogDirection));
				else
					command.push_back(QByteArray::number(getLastKnownPosition()[2]));
				command.push_back(',');
				if(axis == AXIS_A)
					command.append(QByteArray::number(getLimits()[3]*jogDirection));
				else
					command.push_back(QByteArray::number(getLastKnownPosition()[3]));
				command.push_back(';');
				send(command);
			}
		}
	}
}

const std::vector<int>& VhfMill::getLimits() const
{
	return limits_;
}

QByteArray VhfMill::generateCmdForOffset(QByteArray command)
{
	QByteArray output = command.first(2);
	command.remove(0, 2);
	QList<QByteArray> values = command.split(',');

	for(size_t i = 0; i < values.size(); ++i)
	{
		if(values[i].size() != 0)
		{
			bool ok;
			int value = values[i].toInt(&ok);
			if(!ok)
			{
				raiseError(10);
				return QByteArray();
			}
			output.append(QByteArray::number(value+touchoffPos_[i]));
		}
		output.push_back(',');
	}

	output.back() == ',';
	output.chop(1);
	return output;
}

void VhfMill::send(const QByteArray& cmd)
{
	QList<QByteArray> commands = cmd.split(';');

	for(const QByteArray& command : commands)
	{
		QByteArray trimmedCmd = command.trimmed().toUpper();
		if(trimmedCmd.size() < 2)
			continue;
		if(trimmedCmd.first(2) == "PA" || trimmedCmd.first(2) == "GA")
			trimmedCmd = generateCmdForOffset(trimmedCmd);
		else if(trimmedCmd.first(2) == "PB")
			trimmedCmd[1] = 'A';
		else if(trimmedCmd.first(2) == "GB")
			trimmedCmd[1] = 'A';
		qDebug()<<"Writeing"<<(trimmedCmd + ";\n");
		serial_->write(trimmedCmd + ";\n");
	}

}

void VhfMill::stop()
{
	send("!B;");
	send("CONT;");
}

void VhfMill::home(VhfMill::Axis axis)
{
	if(mode_ != MODE_NORMAL || homeAxis_ != AXIS_NONE)
	{
		raiseError(3);
		return;
	}
	if(axis == AXIS_ALL)
		serial_->write("RF;\n");
	else if(axis == AXIS_NONE)
	{
		return;
	}
	else
	{
		int axisNum = axisToMashineNumber(axis);
		qDebug()<<__func__<<axisNum;
		if(axisNum == 0)
		{
			raiseError(20);
			return;
		}
		serial_->write("RF" + QByteArray::number(axisNum) + ";\n");
	}
	homeAxis_ = axis;
	mode_ = MODE_HOME;
	axisHomed_ &= ~axis;
	isHomed(axisHomed_);
}

void VhfMill::stateTimerFn()
{
	reqOutputs();
	reqSensors();
	reqSpindleSpeed();
}

void VhfMill::setOutput(int output, bool state)
{
	serial_->write("OA" + QByteArray::number(static_cast<int>(output)) + "," + QByteArray::number(static_cast<int>(state)) + ";\n");
	reqOutputs();
}

void VhfMill::setTool(int tool)
{
	if(mode_ != MODE_NORMAL)
	{
		VhfMill::raiseError(3);
		return;
	}
	mode_ = MODE_TOOLCHANGE;
	serial_->write("T" + QByteArray::number(tool) + ";\n");
	reqTool();
}

void VhfMill::reqTool()
{
	serial_->write("?T;\n");
}

void VhfMill::reqOutputs()
{
	serial_->write("?OA;\n");
}

void VhfMill::reqPosition()
{
	serial_->write("?PA;\n");
}

void VhfMill::reqSpindleSpeed()
{
	serial_->write("?RV;\n");
}

void VhfMill::reqSensors()
{
	serial_->write("?IA;\n");
}

void VhfMill::enablePosUpdates(bool enable)
{
	if(enable && !positionTimer.isActive())
		positionTimer.start();
	else
		positionTimer.stop();
}

void VhfMill::enableStateUpdates(bool enable)
{
	if(enable && !stateTimer.isActive())
		stateTimer.start();
	else
		stateTimer.stop();
}

void VhfMill::reinit()
{
	mode_ = MODE_PREINIT;
	enablePosUpdates(false);
	enableStateUpdates(false);
	serial_->write("!N;\n");
}

void VhfMill::initTimerFn()
{
	if(mode_ != MODE_PREINIT)
	{
		initTimer.stop();
		enablePosUpdates(true);
		enableStateUpdates(true);
		return;
	}

	reinit();
}

void VhfMill::processIncomeingLine()
{
	if(lineBuffer_.isEmpty())
	{
		if(mode_ == MODE_PREINIT)
		{
			qDebug()<<"init done signal";
			mode_ = MODE_NORMAL;
			initDone();
		}
		else if(mode_ == MODE_TOOLCHANGE)
		{
			qDebug()<<"toolchange done signal";
			mode_ = MODE_NORMAL;
			toolChangeDone();
		}
		else if(mode_ == MODE_HOME)
		{
			qDebug()<<"homeing done";
			axisHomed_ = axisHomed_ | homeAxis_;
			mode_ = MODE_NORMAL;
			homeAxis_ = AXIS_NONE;
			isHomed(axisHomed_);
		}
		else
		{
			return;
		}
	}

	if(lineBuffer_.size() == 0)
		return;

	if(lineBuffer_[0] == 'E')
	{
		lineBuffer_.remove(0,1);
		bool ok;
		int errorNumber = lineBuffer_.toInt(&ok);
		if(!ok)
			errorNumber = 90;
		VhfMill::raiseError(errorNumber);
	}
	else if(lineBuffer_.startsWith("PA="))
	{
		lineBuffer_.remove(0,3);
		QList<QByteArray> list = lineBuffer_.split(',');
		if(list.size() != nAxis_)
		{
			VhfMill::raiseError(22);
			return;
		}

		std::vector<int> postion(5, 0);
		for(int i = 0; i < list.size(); ++i)
		{
			bool ok;
			postion[i] = list[i].toInt(&ok);
			if(!ok)
			{
				VhfMill::raiseError(90);
				return;
			}
			lastKnownPos_ = postion;
			positionUpdate(lastKnownPos_);
		}
	}
	else if(lineBuffer_.startsWith("T="))
	{
		lineBuffer_.remove(0,2);
		bool ok;
		int toolNumber = lineBuffer_.toInt(&ok);
		if(!ok)
		{
			VhfMill::raiseError(90);
			return;
		}
		gotToolNum(toolNumber);
	}
	else if(lineBuffer_.startsWith("O="))
	{
		lineBuffer_.remove(0,2);
		bool ok;
		int outputs = lineBuffer_.toInt(&ok);
		if(!ok)
		{
			VhfMill::raiseError(90);
			return;
		}
		if(outputs_ != outputs)
		{
			outputs_ = outputs;
			gotOutputs(outputs_);
		}
	}
	else if(lineBuffer_.startsWith("RV="))
	{
		lineBuffer_.remove(0,3);
		bool ok;
		int speed = lineBuffer_.toInt(&ok);
		if(!ok)
		{
			VhfMill::raiseError(90);
			return;
		}
		if(spindleSpeed_ != speed)
		{
			spindleSpeed_ = speed;
			gotSindleSpeed(spindleSpeed_);
		}
	}
	else if(lineBuffer_.startsWith("I="))
	{
		lineBuffer_.remove(0,2);
		bool ok;
		unsigned int sensorBits = lineBuffer_.toUInt(&ok);
		if(!ok)
		{
			VhfMill::raiseError(90);
			return;
		}
		if(sensors_ != sensorBits)
		{
			sensors_ = sensorBits;
			gotProbeState(sensors_ & 1U);
			gotPressureState(sensors_ & 1U<<1);
		}
	}
}

bool VhfMill::presureReady() const
{
	return !(sensors_ & 1<<1);
}

bool VhfMill::isInitDone() const
{
	return mode_ != MODE_PREINIT;
}

int VhfMill::getLastSpindleSpeed() const
{
	return spindleSpeed_;
}

bool VhfMill::allHomed() const
{
	bool allHomed = !(~axisHomed_ & (AXIS_X | AXIS_Y | AXIS_Z | AXIS_A | (nAxis_ > 4 ? AXIS_B : 0)));
	qDebug()<<__func__<<allHomed;
	return allHomed;
}

uint8_t VhfMill::axisCount() const
{
	return nAxis_;
}

void VhfMill::touchOff(VhfMill::Axis axis, int offset)
{
	if(axis == AXIS_ALL)
	{
		for(size_t i = 0; i < nAxis_; ++i)
			touchoffPos_[i] = lastKnownPos_[i] + offset;
	}
	else
	{
		touchoffPos_[axisToIndex(axis)] = lastKnownPos_[axisToIndex(axis)] + offset;
	}
	touchoffChanged(touchoffPos_);
}

void VhfMill::touchOffAbsolute(std::vector<int> touchoffPos)
{
	for(size_t i = 0; i < touchoffPos.size() && i < touchoffPos_.size(); ++i)
		touchoffPos_[i] = touchoffPos[i];
	touchoffChanged(touchoffPos_);
}

size_t VhfMill::axisToIndex(Axis axis)
{
	return axisToMashineNumber(axis)-1;
}

std::vector<int> VhfMill::getLastTouchoffPosition() const
{
	return touchoffPos_;
}

std::vector<int> VhfMill::getLastKnownPosition() const
{
	return lastKnownPos_;
}

int VhfMill::axisToMashineNumber(Axis axis)
{
	switch(axis)
	{
		case AXIS_X:
			return 1;
		case AXIS_Y:
			return 2;
		case AXIS_Z:
			return 3;
		case AXIS_A:
			return 4;
		case AXIS_B:
			return 5;
		default:
			return 0;
	}
}

const QString VhfMill::textForErrno(int errorNumber)
{
	switch(errorNumber)
	{
		case 0:
			return "No error";
		case 1:
			return "unsupported command";
		case 2:
			return "invalid command";
		case 3:
			return "command not allowed in this state";
		case 4:
			return "syntax error";
		case 5:
			return "invalid feature";
		case 6:
			return "stopped by user";
		case 7:
			return "unknown error";
		case 10:
			return "invalid parameter";
		case 12:
			return "missing parameter";
		case 13:
			return "parameter is out of range";
		case 14:
			return "no customer profile";
		case 20:
			return "axis not defined";
		case 21:
			return "axis not combinable";
		case 22:
			return "axis not referenced";
		case 23:
			return "no referenzpoint found";
		case 24:
			return "no measurepoint found";
		case 25:
			return "axis range ended";
		case 26:
			return "tool too long";
		case 27:
			return "tool too short";
		case 30:
			return "no spindle defined";
		case 31:
			return "no spindle response";
		case 32:
			return "spindle input active";
		case 40:
			return "can not record macro";
		case 41:
			return "macro too long";
		case 42:
			return "error in last macro";
		case 43:
			return "macro not found";
		case 50:
			return "initial stop";
		case 51:
			return "external stop";
		case 52:
			return "power driver stop";
		case 53:
			return "external spindle stop";
		case 54:
			return "internal spindle stop";
		case 55:
			return "hbox stop";
		case 56:
			return "powerfail stop";
		case 57:
			return "fpga confdone stop";
		case 58:
			return "refswitch stop";
		case 59:
			return "fpga error stop";
		case 60:
			return "overcurrent spindle stop";
		case 61:
			return "overload spindle stop";
		case 62:
			return "wait for input stop";
		case 63:
			return "unexpected input stop";
		case 70:
			return "leveloffset too high";
		case 71:
			return "internal error";
		case 72:
			return "error opening/reading file";
		case 73:
			return "no answer from device";
		case 74:
			return "error while loading fpga";
		case 75:
			return "update not feasible";
		case 76:
			return "update failed";
		case 77:
			return "wait for input failed";
		case 90:
			return "return parse error";
		default:
			return "errno not valid";
	}
}