#!/usr/bin/env php7.4
<?php

/*
 * ▓▒░
 * »«º×■·
 * ╔ ╚ ╝ ╗ ║ ═
 */

use Machine\CNC6600;

require_once __DIR__ . '/../vendor/autoload.php';

const W = 164;
const H = 60;
const TWO_PI = 6.28318530717958647692528676655900576;

/** @var CNC6600 $nc */
$nc = require __DIR__ . '/bootstrap.php';
$nc->loadProgram(9002);

while ($nc->PPMC->ready()) {
	$beforeStep = clone $nc->DR;
	$nc->step();
	$afterStep = clone $nc->DR;
	$afterStep->F = 10000.0;

	$feedPerSecond = ($afterStep->F / 60.0) * 100.0;
	switch ($afterStep->G_TRAVERSE) {
		case 0:
		case 1:
			$posStart = new Vector2D($beforeStep->X, $beforeStep->Z);
			$posEnd = new Vector2D($afterStep->X, $afterStep->Z);
			$toolPath = $posEnd->diff($posStart);
			$segmentLength = $toolPath->length();
			if ($segmentLength < 2) {
				break;
			}
			$duration = $segmentLength / $feedPerSecond;

			$stepSize = 100.0; // 0.1 mm
			$nSteps = $segmentLength / $stepSize;
			$interval = 1_000_000 * $duration / $nSteps;

			$xDir = $posEnd->x < $posStart->x ? -1.0 : 1.0;
			$zDir = $posEnd->y < $posStart->y ? -1.0 : 1.0;
			for ($i = 0; $i < $nSteps; $i++) {
				$fraction = $i / $nSteps;
				$x = $posStart->x + $fraction * $toolPath->x * $xDir;
				$y = $posStart->y + $fraction * $toolPath->y * $zDir;
				$vector = new Vector2D($x, $y);
				p($vector);
				usleep($interval);
			}
			break;
		case 2:
		case 3:
			// Circular interpolation
			$posCenter = new Vector2D($afterStep->I, $afterStep->K);
			$posStart = new Vector2D($beforeStep->X, $beforeStep->Z);
			$posEnd = new Vector2D($afterStep->X, $afterStep->Z);

			// Calculate radius
			$radiusStart = $posStart->diff($posCenter)->length();
			$radiusEnd = $posEnd->diff($posCenter)->length();
			if (abs($radiusStart - $radiusEnd) > 2) {
				die("ERROR PERFORMING CIRCULAR INTERPOLATION: INCONSISTENT RADIUS\n");
			}
			$r = $radiusStart;

			// Calculate starting and ending angles [0 - 2pi]
			$angleStart = atan2($posStart->y - $posCenter->y, $posStart->x - $posCenter->x);
//			while ($angleStart < 0) {
//				$angleStart += TWO_PI;
//			}
			$angleEnd = atan2($posEnd->y - $posCenter->y, $posEnd->x - $posCenter->x);
//			while ($angleEnd < 0) {
//				$angleEnd += TWO_PI;
//			}

			// segment length = 2 * pi * r * alpha / (2*pi) = r * alpha
			$segmentLength = $r * abs($angleStart - $angleEnd);

			$duration = $segmentLength / $feedPerSecond;
			$nSteps = abs($angleStart - $angleEnd) / 0.001;
			if ($nSteps === 0) {
				break;
			}

			$interval = 1_000_000 * $duration / $nSteps;
			$delta = $afterStep->G_TRAVERSE === 2 ? 0.001 : -0.001; // CW[2] vs CCW[3]

			dump([
				"G{$afterStep->G_TRAVERSE}",
				'from'     => $posStart,
				'to'       => $posEnd,
				'center'   => $posCenter,
				'r'        => $r,
				'a_from'   => ($angleStart / pi()) . 'π',
				'a_end'    => ($angleEnd / pi()) . 'π',
				'length'   => $segmentLength,
				'steps'    => $nSteps,
				'duration' => $duration,
				'interval' => $interval,
				'delta'    => $delta,
			]);
			readline();
			$i = 0;
			for ($a = $angleStart; abs($a - $angleEnd) > 0.001; $a += $delta) {
				if ($a > TWO_PI) {
					$a -= TWO_PI;
				} else if ($a < -TWO_PI) {
					$a += TWO_PI;
				}
				$pos = new Vector2D($posCenter->x + $r * cos($a), $posCenter->y + $r * sin($a));
				p($pos);
				print("Alpha: $a\n");
				usleep($interval);
				$i++;
			}
			dump(['took_steps' => $i]);
			readline();
			break;
		default:
			break;
	}
	p(new Vector2D($afterStep->X, $afterStep->Z));
}

function p(Vector2D $v): void
{
	static $previous = null;
	$x = (int)map($v->x, 350 * W, -350 * W, 0, W);
	$z = (int)map($v->y, 700 * H, -750 * H, 0, H);

	$previous ??= new Vector2D($x - 1, $z);

	if ($previous->x === $x && $previous->y === $z) {
		return;
	}
	$previous->x = $x;
	$previous->y = $z;
	system('clear');
	print('╔' . str_repeat('═', W) . '╗' . PHP_EOL);    // Top
	print(str_repeat("║" . str_repeat(' ', W) . "║\n", $z - 1));
	print('║' . str_repeat(' ', $x - 1) . '×' . str_repeat(' ', W - $x) . "║\n");
	print(str_repeat("║" . str_repeat(' ', W) . "║\n", H - $z));
	print('╚' . str_repeat('═', W) . '╝' . PHP_EOL); // Bottom
}

function map(int $x, int $in_min, int $in_max, int $out_min, int $out_max): int
{
	return ($x - $in_min) * ($out_max - $out_min) / ($in_max - $in_min) + $out_min;
}

class Vector2D
{
	public float $x;
	public float $y;


	public function __construct(float $x, float $y)
	{
		$this->x = $x;
		$this->y = $y;
	}


	public function length(): float
	{
		return sqrt($this->x ** 2 + $this->y ** 2);
	}


	public function diff(self $other): self
	{
		return new self(
			abs($this->x - $other->x),
			abs($this->y - $other->y)
		);
	}
}

class Arc2D
{
	public Vector2D $start;
	public Vector2D $end;
	public Vector2D $centre;


	public function __construct(Vector2D $start, Vector2D $end, Vector2D $centre)
	{
		$this->start = $start;
		$this->end = $end;
		$this->centre = $centre;
	}


	public function radius(): float
	{
		return $this->start->diff($this->centre)->length();
	}
}