<?php

namespace Machine;

use InvalidArgumentException;
use Program\Block;
use RuntimeException;

class InstructionSet
{
	private const SPEEDS = [0, 40, 50, 63, 80, 100, 125, 160, 200, 250, 315, 400, 500, 630, 800, 1000, 1250, 1600, 2000];

	private CNC6600 $CNC6600;


	public function __construct(CNC6600 $CNC6600)
	{
		$this->CNC6600 = $CNC6600;
	}


	public function apply(Block $block): void
	{
		system('clear');
		print("\n$block\n\n");
		foreach ($block->words as $word) {
			if (method_exists($this, "$word")) {
				// There's a method dedicated to handling the full word (eg G22)
				$this->{"$word"}($block);
				@$block->pop($word->register);
			}
			if (method_exists($this, $word->register)) {
				// There's a method dedicated to handling data for the given register (eg T, S)
				$this->{$word->register}($block->pop($word->register));
			}
		}
		foreach ($block->words as $word) {
			// The word value may be put into the machine registers directly
			if (property_exists($this->CNC6600->DR, $word->register)) {
				$this->CNC6600->DR->{$word->register} = $block->pop($word->register);
			} else {
				print("WARNING: No method or register for word '{$word}'\n");
			}
		}
		dump($this->CNC6600->DR);
		readline();
	}


	protected function E(int $value): void
	{
		print("WARNING: Line repetitions are not implemented yet");
	}


	protected function T(int $value): void
	{
		// Triggers an exception if not found, good enough for now.
		// Might implement compensations and more explicit tool registers later on
		$this->CNC6600->TM->read($value);
		$this->CNC6600->DR->T = $value;
	}


	protected function S(int $value): void
	{
		if (!in_array($value, self::SPEEDS)) {
			throw new InvalidArgumentException(sprintf(
				"Speed not supported: '$value'. Supported are: \n%s\n",
				implode(', ', self::SPEEDS)
			));
		}
		$this->CNC6600->DR->S = $value;
	}


	protected function G0(Block $block): void
	{
		$this->CNC6600->DR->G_TRAVERSE = $block->read('G');
	}


	protected function G1(Block $block): void
	{
		$this->CNC6600->DR->G_TRAVERSE = $block->read('G');
	}


	protected function G2(Block $block): void
	{
		$this->CNC6600->DR->G_TRAVERSE = $block->read('G');
	}


	protected function G3(Block $block): void
	{
		$this->CNC6600->DR->G_TRAVERSE = $block->read('G');
	}


	protected function G4(Block $block): void
	{
		print("Ignoring dwell setting: {$block->pop('X')}\n");
	}


	protected function G17(Block $block): void
	{
		$this->CNC6600->DR->G_PLANE_SELECTION = $block->read('G');
	}


	protected function G18(Block $block): void
	{
		$this->CNC6600->DR->G_PLANE_SELECTION = $block->read('G');
	}


	protected function G19(Block $block): void
	{
		$this->CNC6600->DR->G_PLANE_SELECTION = $block->read('G');
	}


	protected function G22(Block $block): void
	{
		$N = $block->pop('X');
		$this->CNC6600->SPMC->loadProgram($N);
		$this->CNC6600->_AMC = $this->CNC6600->SPMC;
	}


	protected function G40(Block $block): void
	{
		$this->CNC6600->DR->G_RADIUS_COMPENSATION = $block->read('G');
	}


	protected function G41(Block $block): void
	{
		$this->CNC6600->DR->G_RADIUS_COMPENSATION = $block->read('G');
	}


	protected function G42(Block $block): void
	{
		$this->CNC6600->DR->G_RADIUS_COMPENSATION = $block->read('G');
	}


	protected function G43(Block $block): void
	{
		$this->CNC6600->DR->G_RADIUS_COMPENSATION = $block->read('G');
	}


	protected function G44(Block $block): void
	{
		$this->CNC6600->DR->G_RADIUS_COMPENSATION = $block->read('G');
	}


	protected function G79(Block $block): void
	{
		if ($this->CNC6600->DR->B_WORKCYCLE === 0) {
			throw new RuntimeException('No workcycle prepared!');
		}
		// TODO verify active workcycle, execute it?
	}


	protected function G81(Block $block): void
	{
		$this->CNC6600->DR->G_WORKCYCLE = $block->read('G');
		$this->getWorkcycleParameters($block);
	}


	protected function G84(Block $block): void
	{
		$this->CNC6600->DR->G_WORKCYCLE = $block->read('G');
		$this->getWorkcycleParameters($block);
	}


	protected function G85(Block $block): void
	{
		$this->CNC6600->DR->G_WORKCYCLE = $block->read('G');
		$this->getWorkcycleParameters($block);
	}


	protected function G86(Block $block): void
	{
		$this->CNC6600->DR->G_WORKCYCLE = $block->read('G');
		$this->getWorkcycleParameters($block);
	}


	private function getWorkcycleParameters(Block $block): void
	{
		$this->CNC6600->DR->X_WORKCYCLE = $block->has('X') ? $block->pop('X') : 0;
		$this->CNC6600->DR->Y_WORKCYCLE = $block->has('Y') ? $block->pop('Y') : 0;
		$this->CNC6600->DR->Z_WORKCYCLE = $block->has('Z') ? $block->pop('Z') : 0;
		$this->CNC6600->DR->B_WORKCYCLE = $block->has('B') ? $block->pop('B') : 0;
	}


	protected function G90(Block $block): void
	{
		$this->CNC6600->DR->G_DIMENSION_PROGRAMMING = $block->read('G');
	}


	protected function G91(Block $block): void
	{
		$this->CNC6600->DR->G_DIMENSION_PROGRAMMING = $block->read('G');
	}


	protected function G92(Block $block): void
	{
		// Incremental
		foreach (['X', 'Y', 'Z'] as $R) {
			if ($block->has($R)) {
				$this->CNC6600->DR->{$R} = -$block->pop($R);
			}
		}
	}


	protected function G93(Block $block): void
	{
		// Absolute zero datum shift
		foreach (['X', 'Y', 'Z'] as $R) {
			if ($block->has($R)) {
				$this->CNC6600->DR->{$R} -= $block->pop($R);
			}
		}
	}


	protected function G98(Block $block): void
	{
		// Automatic positioning to reference datum point
		print("Ignoring automatic positioning to zero datum point\n");
	}


	protected function M0(Block $block): void
	{
		$this->CNC6600->DR->M_PROGRAM = $block->read('M');
		$this->CNC6600->DR->M_SPINDLE = 5;
		$this->CNC6600->DR->M_COOLANT = 9;
	}


	protected function M2(Block $block): void
	{
		$this->CNC6600->DR->M_PROGRAM = $block->read('M');
		$this->CNC6600->DR->M_SPINDLE = 5;
		$this->CNC6600->DR->M_COOLANT = 9;
	}


	protected function M3(Block $block): void
	{
		$this->CNC6600->DR->M_SPINDLE = $block->read('M');
	}


	protected function M4(Block $block): void
	{
		$this->CNC6600->DR->M_SPINDLE = $block->read('M');
	}


	protected function M5(Block $block): void
	{
		$this->CNC6600->DR->M_SPINDLE = $block->read('M');
	}


	protected function M6(Block $block): void
	{
		$this->CNC6600->DR->M_SPINDLE = 5;
		$this->CNC6600->DR->M_COOLANT = 9;
	}


	protected function M8(Block $block): void
	{
		$this->CNC6600->DR->M_COOLANT = $block->read('M');
	}


	protected function M9(Block $block): void
	{
		$this->CNC6600->DR->M_COOLANT = $block->read('M');
	}


	protected function M10(Block $block): void
	{
		$this->CNC6600->DR->M_TABLE_CLAMPING = $block->read('M');
	}


	protected function M11(Block $block): void
	{
		$this->CNC6600->DR->M_TABLE_CLAMPING = $block->read('M');
	}


	protected function M30(Block $block): void
	{
		$this->CNC6600->DR->M_SPINDLE = 5;
		$this->CNC6600->DR->M_COOLANT = 9;
		$N = $this->CNC6600->PPMC->N;
		$this->CNC6600->PPMC->reset();
		$this->CNC6600->SPMC->reset();
		$this->CNC6600->PPMC->loadProgram($N);
		$this->CNC6600->_AMC = $this->CNC6600->PPMC;
	}


	protected function M67(Block $block): void
	{
		throw new RuntimeException('[Enter new tool information without machine stop] is not implemented');
	}
}