class Vector {
		constructor(x, y, z) {
			this.x = x;
			this.y = y;
			this.z = z;
		};

		distanceTo(vector) {
			return Math.sqrt(Math.pow(this.x - vector.x, 2) + Math.pow(this.y - vector.y, 2) + Math.pow(this.z - vector.z, 2));
		}

		diff(vector) {
			return new Vector(
				this.x - vector.x,
				this.y - vector.y,
				this.z - vector.z
			);
		}

		add(vector) {
			return new Vector(
				this.x + vector.x,
				this.y + vector.y,
				this.z + vector.z
			);
		}
	}

	class Joint {
		constructor(angle, position, length) {
			this.angle = angle;
			this.position = position;
			this.length = length;

			this.targetAngle = angle;
			this.previousAngle = angle;
			this.velocity = 0;
		};

		setTargetAngle(targetAngle) {
			this.targetAngle = targetAngle;
			this.velocity = this.targetAngle - this.normalizeAngle(this.angle);
		}

		normalizeAngle(angle) {
			while (angle <= -Math.PI) angle += Math.PI * 2;
			while (angle > Math.PI) angle -= Math.PI * 2;
			return angle;
		}

		getCurrentVelocity() {//per tick
			return this.normalizeAngle(this.angle - this.previousAngle);
		}

		tick() {
			this.previousAngle = this.angle;
			this.angle = this.angle + this.velocity;
		}
	}

	class Step {
		constructor(
			idlePosition,//vector relative to inner joint
			angle,//step direction
			length,//step length
			height,//step height
			phaseShift//
		) {
			this.idlePosition = idlePosition;
			this.angle = angle;//radians
			this.length = length;
			this.height = height;
			this.phaseShift = phaseShift;
		}
	}

	class Leg {
		constructor(
			vehicleCenter,
			innerJoint,
			midJoint,
			outerJoint,
			step,
			phaseStep
		) {
			this.vehicleCenter = vehicleCenter;
			this.innerJoint = innerJoint;
			this.midJoint = midJoint;
			this.outerJoint = outerJoint;
			this.step = step;
			this.phaseStep = phaseStep;

			this.innerJoint.length = innerJoint.position.distanceTo(midJoint.position);//calculate
			this.midJoint.length = midJoint.position.distanceTo(outerJoint.position);//calculate
			//this.outerJoint.length = 100;

			this.joints = [innerJoint, midJoint, outerJoint];

			this.preCalculateAngles();
		}

		preCalculateAngles() {
			this.angles = {};
			for (let phase = 0; phase < 360; phase += this.phaseStep) {
				this.angles[phase] = this.getLegAngles(this.getEsimatedLegPosition(phase, this.step.phaseShift))
			}
		}

		applyStepHeight(z) {
			const idleYawRad = Math.atan2(this.step.idlePosition.x, this.step.idlePosition.y);
			const diffHypot = Math.hypot(this.step.idlePosition.x, this.step.idlePosition.y);

			const minZ = Math.abs(this.midJoint.length - this.outerJoint.length);
			const maxZ = (this.midJoint.length + this.outerJoint.length) * 0.6;

			if (Math.hypot(z, 0) > maxZ) {
				z = z > 0 ? maxZ : -maxZ;
			}

			const safeY = (this.innerJoint.length + this.midJoint.length * 0.5 + this.outerJoint.length * 0.5) * Math.cos(idleYawRad);

			const vAngle = Math.asin(z / safeY);
			const y = safeY * Math.cos(vAngle) * Math.cos(idleYawRad);
			this.step.idlePosition.z = z;
			this.step.idlePosition.y = this.step.idlePosition.y > 0 ? y : -y;
			this.preCalculateAngles();
		}

		applyStepAngle(angle) {
			this.step.angle = angle;
			this.preCalculateAngles();
		}
		applyPhase(phase/*0-360*/) {
			const legAngles = this.angles[phase];

			this.innerJoint.setTargetAngle(legAngles.yaw);
			this.midJoint.setTargetAngle(legAngles.midPitch);
			this.outerJoint.setTargetAngle(legAngles.outerPitch);
		}

		getEsimatedLegPosition(phase, phaseShift) {
			phase = (phase + phaseShift) % 360;

			const stepX = ((phase < 180 ? phase : 180 - phase % 180) / 180 - 0.5) * this.step.length;//linear movement along step direction
			const stepZ = Math.max(Math.sin(phase * Math.PI / 180), -0.2) * this.step.height / 1.2;
			//const stepZ = Math.max((phase > 180 ? Math.cos(phase * Math.PI / 360) + 0.9 : Math.cos((phase - 120) * Math.PI / 360)) * .9 - .1, 0) * this.step.height;

			const x = this.step.idlePosition.x + stepX * Math.cos(this.step.angle);
			const y = this.step.idlePosition.y + stepX * Math.sin(this.step.angle);

			return new Vector(x, y, stepZ);
		}

		getLegAngles(esimatedLegPosition) {
			const yawRad = Math.atan2(esimatedLegPosition.x, esimatedLegPosition.y);

			const dx = Math.hypot(esimatedLegPosition.x, esimatedLegPosition.y) - this.innerJoint.length;
			const dz = this.step.idlePosition.z + esimatedLegPosition.z;

			const hyp = Math.hypot(dx, dz);
			if (hyp > this.midJoint.length + this.outerJoint.length) {//out of reach
				hyp = this.midJoint.length + this.outerJoint.length;
			}

			const innerAngleRad = Math.acos((this.outerJoint.length * this.outerJoint.length - this.midJoint.length * this.midJoint.length - hyp * hyp) / (-2 * this.midJoint.length * hyp)) + Math.atan2(dz, dx);
			const outerAngleRad = Math.acos((hyp * hyp - this.midJoint.length * this.midJoint.length - this.outerJoint.length * this.outerJoint.length) / (-2 * this.midJoint.length * this.outerJoint.length)) - Math.PI;

			if (isNaN(yawRad) || isNaN(innerAngleRad) || isNaN(outerAngleRad)) {
				console.log(yawRad, innerAngleRad, outerAngleRad);
				console.log(dx, dz);
				return;
			}

			return { yaw: yawRad, midPitch: innerAngleRad, outerPitch: outerAngleRad };
		}

		getMaxMinAngles() {
			const angles = [0, 90, 180, 270].map((phase) => {
				return this.getLegAngles(getEsimatedLegPosition(phase, 0));
			});
			return {
				yawMin: Math.min(angles.map((x) => { return x.yaw })),
				yawMax: Math.max(angles.map((x) => { return x.yaw })),
				midPitchMin: Math.min(angles.map((x) => { return x.midPitch })),
				midPitchMax: Math.max(angles.map((x) => { return x.midPitch })),
				outerPitchMin: Math.min(angles.map((x) => { return x.outerPitch })),
				outerPitchMax: Math.max(angles.map((x) => { return x.outerPitch })),
			}
		}

		tick() {
			this.joints.forEach(function (joint) { joint.tick(); });
		}

		getVectors() {
			const res = [];
			const sinYaw = Math.sin(this.innerJoint.angle);
			const cosYaw = Math.cos(this.innerJoint.angle);

			let currentVector = this.vehicleCenter;
			res.push(currentVector);

			currentVector = currentVector.add(this.innerJoint.position);
			res.push(currentVector);

			currentVector = currentVector.add(new Vector(
				this.innerJoint.length * sinYaw,
				this.innerJoint.length * cosYaw,
				0
			));
			res.push(currentVector);

			const dxMid = Math.cos(this.midJoint.angle) * this.midJoint.length;
			const dzMid = Math.sin(this.midJoint.angle) * this.midJoint.length;

			currentVector = currentVector.add(new Vector(
				dxMid * sinYaw,
				dxMid * cosYaw,
				dzMid
			));
			res.push(currentVector);

			const c = this.midJoint.angle + this.outerJoint.angle;
			const dxOuter = Math.cos(c) * this.outerJoint.length;
			const dzOuter = Math.sin(c) * this.outerJoint.length;
			currentVector = currentVector.add(new Vector(
				dxOuter * sinYaw,
				dxOuter * cosYaw,
				dzOuter
			));
			res.push(currentVector);

			return res;
		}
	}

	class Hexapod {
		constructor(phaseStep) {
			this.idleHeight = -70;
			this.stepAngle = 0;
			this.turnAngle = 0;
			this.stepLength = 70;
			this.stepHeight = 30;
			this.debugPoints = [];
			const vehicleCenter = new Vector(0, 0, 0);
			this.legs = [
				new Leg(
					vehicleCenter,
					new Joint(0, new Vector(-70, 10, 0), 50),
					new Joint(0, new Vector(-70, 60, 0), 50),
					new Joint(0, new Vector(-70, 110, 0), 70),
					new Step(new Vector(-30, 90, this.idleHeight), this.stepAngle, this.stepLength, this.stepHeight, 0),
					phaseStep
				),
				new Leg(
					vehicleCenter,
					new Joint(0, new Vector(-70, -10, 0), 50),
					new Joint(0, new Vector(-70, -60, 0), 50),
					new Joint(0, new Vector(-70, -110, 0), 70),
					new Step(new Vector(-30, -90, this.idleHeight), this.stepAngle, this.stepLength, this.stepHeight, 180),
					phaseStep
				),
				new Leg(
					vehicleCenter,
					new Joint(0, new Vector(0, 10, 0), 50),
					new Joint(0, new Vector(0, 60, 0), 50),
					new Joint(0, new Vector(0, 110, 0), 70),
					new Step(new Vector(0, 100, this.idleHeight), this.stepAngle, this.stepLength, this.stepHeight, 180),
					phaseStep
				),
				new Leg(
					vehicleCenter,
					new Joint(0, new Vector(0, -10, 0), 50),
					new Joint(0, new Vector(0, -60, 0), 50),
					new Joint(0, new Vector(0, -110, 0), 70),
					new Step(new Vector(0, -100, this.idleHeight), this.stepAngle, this.stepLength, this.stepHeight, 0),
					phaseStep
				),
				new Leg(
					vehicleCenter,
					new Joint(0, new Vector(70, 10, 0), 50),
					new Joint(0, new Vector(70, 60, 0), 50),
					new Joint(0, new Vector(70, 110, 0), 70),
					new Step(new Vector(30, 90, this.idleHeight), this.stepAngle, this.stepLength, this.stepHeight, 0),
					phaseStep
				),
				new Leg(
					vehicleCenter,
					new Joint(0, new Vector(70, -10, 0), 50),
					new Joint(0, new Vector(70, -60, 0), 50),
					new Joint(0, new Vector(70, -110, 0), 70),
					new Step(new Vector(30, -90, this.idleHeight), this.stepAngle, this.stepLength, this.stepHeight, 180),
					phaseStep
				),
			];
		}

		applyPhase(phase/*0-360*/) {
			this.legs.forEach(function (leg) {
				leg.applyPhase(phase);
			});
		}
		changeHeight(value) {
			this.legs.forEach(function (leg) {
				leg.applyStepHeight(this.idleHeight + value);
			}, this);
		}
		changeStepLength(value) {
			this.stepLength += value;
			this.legs.forEach(function (leg) {
				leg.step.length = this.stepLength;
				leg.preCalculateAngles();
			}, this);
		}
		applyTurn1(centerX, centerY) {
			const angleToAxis = Math.atan2(centerX, centerY);
			const distanceToAxis = Math.hypot(centerX, centerY);
			distanceToAxis = 1000/distanceToAxis;
			this.legs.forEach(leg => {
				const dx = leg.step.idlePosition.x + leg.innerJoint.position.x + Math.sin(angleToAxis)*distanceToAxis || 0;
				const dy = leg.step.idlePosition.y + leg.innerJoint.position.y + Math.cos(angleToAxis)*distanceToAxis || 0;
				
				const angle = Math.atan2(dy,dx);

				const hypIdle = Math.hypot(dx, dy);
				leg.applyStepAngle(angle+Math.PI/2);
				leg.step.length = this.stepLength *hypIdle/ ((distanceToAxis || 0) + 1000);
			});
		}
		applyTurn(centerX, centerY) {
			this.stepAngle = Math.atan2(centerX, centerY);
			if (this.stepAngle > Math.PI / 2) this.stepAngle -= Math.PI;
			if (this.stepAngle < -Math.PI / 2) this.stepAngle += Math.PI;
			const mults = this.legs.map(leg =>
				Math.hypot(leg.step.idlePosition.y + leg.innerJoint.position.y, leg.step.idlePosition.x + leg.innerJoint.position.x)
				/ Math.hypot(leg.step.idlePosition.y + leg.innerJoint.position.y + centerY*.3, leg.step.idlePosition.x + leg.innerJoint.position.x + centerX*.3));
			const minMult = Math.min(...mults);
			const maxMult = Math.max(...mults);
			const mult = minMult / maxMult;
			const d = Math.pow(Math.max(...this.legs.map(leg =>Math.hypot(leg.step.idlePosition.y + leg.innerJoint.position.y, leg.step.idlePosition.x + leg.innerJoint.position.x))),2)/Math.hypot(centerX,centerY);
			const a =  Math.atan2(centerX,centerY);
			this.legs.forEach(leg => {
				const dx = leg.step.idlePosition.x + leg.innerJoint.position.x;
				const dy = leg.step.idlePosition.y + leg.innerJoint.position.y;
				const idleAngle = Math.atan2(dx, dy) + this.stepAngle;
				const turnAngle = Math.atan2(dx + centerX, dy + centerY);

				const hypIdle = Math.hypot(dx, dy);
				const hyp = Math.hypot(dx + centerX, dy + centerY);
				leg.applyStepAngle(turnAngle - idleAngle);
				leg.step.length = this.stepLength * hyp / hypIdle * mult;
			});
			this.debugPoints = [new Vector(Math.sin(a)*-d,Math.cos(a)*-d,0)];
		}
		tick() {
			this.legs.forEach(function (leg) {
				leg.tick();
			});
		}
		getVectors() {
			return this.legs.map(function (leg) { return leg.getVectors() });
		}
	}

	class Canvas {
		constructor(id, label, axisSelectorX, axisSelectorY) {
			const self = this;
			this.id = id;
			this.label = label;
			this.canvas = document.getElementById(id);
			this.ctx = this.canvas.getContext('2d');
			this.axisSelectorX = axisSelectorX;
			this.axisSelectorY = axisSelectorY;

			this.canvasHeight = this.canvas.offsetHeight;
			this.canvasWidth = this.canvas.offsetWidth;

			this.initialY = this.canvasHeight / 2;
			this.initialX = this.canvasWidth / 2;

			this.traceCounter = 0;
			this.maxTraces = 50;
			this.traces = {};

			const axisSize = 150;
			this.axisVectors = [
				[
					new Vector(-axisSize, -axisSize, -axisSize),
					new Vector(-axisSize, -axisSize, axisSize)
				],
				[
					new Vector(-axisSize, -axisSize, -axisSize),
					new Vector(-axisSize, axisSize, -axisSize)
				],
				[
					new Vector(-axisSize, -axisSize, -axisSize),
					new Vector(axisSize, -axisSize, -axisSize)
				],
			]

			this.mouseOver = false;
			this.mousePos = { x: 0, y: 0 };//relative to center
			this.clickPos = { x: 0, y: 0 };//relative to center
			this.canvas.addEventListener("mouseenter", function (event) {
				self.mouseOver = true;
			}, false);
			this.canvas.addEventListener("mouseleave", function (event) {
				self.mouseOver = false;
			}, false);
			this.canvas.addEventListener("mousemove", function (event) {
				if (self.mouseOver) {
					self.mousePos = { x: event.offsetX - self.initialX, y: event.offsetY - self.initialY };
				}
			}, false);
			this.canvas.addEventListener("mouseup", function (event) {
				if (self.mouseOver) {
					self.clickPos = { x: event.offsetX - self.initialX, y: event.offsetY - self.initialY };
				}
			}, false);
		};
		clear(drawAxis) {
			this.ctx.clearRect(0, 0, this.canvasWidth, this.canvasHeight);
			this.ctx.strokeStyle = "#000000";
			this.ctx.strokeText(this.label, 10, 10);

			if (drawAxis) {
				this.axisVectors.forEach(function (vectors, i) {
					this.ctx.moveTo(this.initialX, this.initialY);
					this.ctx.beginPath();

					vectors.forEach(function (vector) {
						this.ctx.lineTo(this.initialX + this.axisSelectorX(vector), this.initialY - this.axisSelectorY(vector));
					}, this);
					this.ctx.stroke();

					const lastVector = vectors[vectors.length - 1];
					this.traces[[this.traceCounter, i]] = lastVector

				}, this);
			}
		}
		drawVectors(vectors) {/*2d array*/
			vectors.forEach(function (vectors, i) {
				this.ctx.moveTo(this.initialX, this.initialY);
				this.ctx.beginPath();

				vectors.forEach(function (vector) {
					this.ctx.lineTo(this.initialX + this.axisSelectorX(vector), this.initialY - this.axisSelectorY(vector));
				}, this);
				this.ctx.stroke();

				const lastVector = vectors[vectors.length - 1];
				this.traces[[this.traceCounter, i]] = lastVector

			}, this);

			for (const key in this.traces) {
				const vector = this.traces[key];
				this.ctx.fillStyle = "#FF0000";//red
				this.ctx.fillRect(this.initialX + this.axisSelectorX(vector), this.initialY - this.axisSelectorY(vector), 1, 1);
			}

			this.ctx.strokeStyle = "#000000";
			this.ctx.beginPath();
			this.ctx.arc(this.clickPos.x + this.initialX, this.clickPos.y + this.initialY, 5, 0, 2 * Math.PI);
			this.ctx.stroke();
			if (this.mouseOver) {
				this.ctx.strokeStyle = "#00FF00";
				this.ctx.beginPath();
				this.ctx.arc(this.mousePos.x + this.initialX, this.mousePos.y + this.initialY, 10, 0, 2 * Math.PI);
				this.ctx.stroke();
			}

			this.traceCounter = (this.traceCounter + 1) % this.maxTraces;
		}
		drawPoints(points) {		
			this.ctx.fillStyle = "#00ff00";//green
			points.forEach(function (point) {
				this.ctx.fillRect(this.initialX + this.axisSelectorX(point), this.initialY - this.axisSelectorY(point), 3, 3);
			}, this);
		}
	}

	class Worker {
		constructor(tickTime) {
			const self = this;
			this.phaseStep = 5;
			this.tickTime = tickTime;
			const tan30 = Math.tan(Math.PI / 6);
			const scale = 0.7;
			this.canvases = [
				new Canvas('canvasForward', 'yz Forward', function (v) { return v.y }, function (v) { return v.z }),
				new Canvas('canvasSide', 'xz Side', function (v) { return v.x }, function (v) { return v.z }),
				new Canvas('canvasTop', 'xy Top', function (v) { return v.x }, function (v) { return -v.y }),
				new Canvas('canvasIso', 'xyz Iso', function (v) { return v.x * scale + v.y * scale }, function (v) { return v.z * scale + v.x * tan30 * scale - v.y * tan30 * scale }),
			];

			this.bot = new Hexapod(this.phaseStep);

			this.phase = 0;
			this.focus = true;

			window.addEventListener('focus', function () {
				console.log('focus');
				self.focus = true;
			});

			window.addEventListener('blur', function () {
				console.log('blur');
				self.focus = false;
			});
			this.start();
		}

		tick(argument) {
			const canvasForward = this.canvases[0];
			const bot = this.bot;
			if (canvasForward.mouseOver) {
				bot.changeHeight(-canvasForward.mousePos.y);
			} else {
				bot.changeHeight(0);
			}
			const canvasTop = this.canvases[2];
			if (canvasTop.mouseOver) {
				bot.applyTurn(-canvasTop.mousePos.x, -canvasTop.mousePos.y);
			} else {
				bot.applyTurn(0, 0);
			}



			this.phase = (this.phase + this.phaseStep) % 360;
			bot.applyPhase(this.phase);
			bot.tick();

			const vectors = bot.getVectors();

			this.canvases.forEach(function (c) {
				c.clear(false);
				c.drawVectors(vectors);
				c.drawPoints(bot.debugPoints);
			});
		}
		start() {
			this.stop();
			this.interval = setInterval((function (self) {
				return function () {
					if (self.focus) {
						self.tick();
					}
				}
			})(this), this.tickTime);
		}
		stop() {
			clearInterval(this.interval);
		}
	}