import type { InterpolationParameters } from "../gui/interpolate";
import type { Body } from "./constants";
import { addVector, getVectorMagnitude, invertTwoByTwoMatrix, matrixMultiply, multiplyMatrixWithScalar, normalizeVector, subtractVector, vectorCrossProduct, vectorDotProduct } from "./mathematics";

export interface Orbit {
	semiLatusRectum: number,
	eccentricity: number,
	coordinateAxes: [number[][], number[][], number[][]]
}

export interface LocalVectors {
	prograde: number[][],
	radial: number[][],
	normal: number[][]
}

export interface OrbitalCoordinates {
	orbit: Orbit,
	meanAnomaly: number,
	eccentricAnomaly: number,
	trueAnomaly: number,
	meanAngularMotion: number,
	position: number[][]
}

export interface Manoeuvre {
	time: number,
	progradeDeltaV: number,
	radialDeltaV: number,
	normalDeltaV: number,
	totalDeltaV: number
}

export interface Transfer {
	transferOrbit: Orbit,
	transferOrbitTrueAnomalyAtManoeuvreOne: number,
	transferOrbitTrueanomalyAtManoeuvreTwo: number,
	firstManoeuvre: Manoeuvre,
	secondManoeuvre: Manoeuvre,
	farthestPointDistance: number,
	closestPointDistance: number,
}

export interface Landing {
	transferOrbit: Orbit,
	transferManoeuvre: Manoeuvre,
	brakingDeltaV: number,
	brakingAltitude: number,
	brakingTimestamp: number,
	totalDeltaV: number
}

export interface ShipParameters {
	mass: number,
	thrust: number,
	specificImpulse: number
};

export interface LandingParameters {
	targetLatitude: number;
	targetLongitude: number;
	targetAltitude: number;
	minimumAngle: number;
};

export const DefaultShipParameters: ShipParameters = {
	mass: 1000,
	thrust: 100,
	specificImpulse: 100
};

export class LambertSolutions {
	// Pre-calculate some values to make finding an orbit based on the parameter gamma faster
	// Naming these in a good way is very hard. To see where they came from, look at
	// https://en.wikipedia.org/wiki/Lambert%27s_problem#Parametrization_of_the_transfer_trajectories
	normalVector: number[][];
	positionOne: number[][];
	positionTwo: number[][];
	positionOneMagnitude: number;
	positionTwoMagnitude: number;
	
	multiplierSemiLatusRectum: number;
	firstTermSemiLatusRectum: number;
	gammaMultiplierSemiLatusRectum: number;

	firstTermEccentricity: number[][];
	secondTermEccentricity: number[][];

	startingLocalVectors: LocalVectors;
	startingVelocity: number[][];
	goalLocalVectors: LocalVectors;
	goalVelocity: number[][];

	extremalGamma: number;
	parabolaGamma: number;

	body: Body;

	constructor(startingOrbit: Orbit, startingTrueAnomaly: number, goalOrbit: Orbit, goalTrueAnomaly: number, body: Body, backwards?: boolean) {
		this.body = body;

		let startingRadius = startingOrbit.semiLatusRectum / (1 + startingOrbit.eccentricity * Math.cos(startingTrueAnomaly));
		let startingLocalX = startingRadius * Math.cos(startingTrueAnomaly);
		let startingLocalY = startingRadius * Math.sin(startingTrueAnomaly);

		let goalRadius = goalOrbit.semiLatusRectum / (1 + goalOrbit.eccentricity * Math.cos(goalTrueAnomaly));
		let goalLocalX = goalRadius * Math.cos(goalTrueAnomaly);
		let goalLocalY = goalRadius * Math.sin(goalTrueAnomaly);

		this.positionOne = addVector(
			multiplyMatrixWithScalar(startingLocalX, startingOrbit.coordinateAxes[0]),
			multiplyMatrixWithScalar(startingLocalY, startingOrbit.coordinateAxes[1])
		);

		this.positionTwo = addVector(
			multiplyMatrixWithScalar(goalLocalX, goalOrbit.coordinateAxes[0]),
			multiplyMatrixWithScalar(goalLocalY, goalOrbit.coordinateAxes[1])
		);

		// First, find the normal vector
		let crossProduct = vectorCrossProduct(this.positionOne, this.positionTwo);
		this.normalVector = normalizeVector(crossProduct);
		if (backwards) {
			this.normalVector = multiplyMatrixWithScalar(-1, this.normalVector);
		}

		this.gammaMultiplierSemiLatusRectum = vectorDotProduct(this.normalVector, crossProduct);

		this.positionOneMagnitude = getVectorMagnitude(this.positionOne);
		this.positionTwoMagnitude = getVectorMagnitude(this.positionTwo);
		let vectorDifference = subtractVector(this.positionTwo, this.positionOne);

		let differenceMagnitudeSquared = getVectorMagnitude(vectorDifference)**2;
		this.multiplierSemiLatusRectum = (this.positionOneMagnitude + this.positionTwoMagnitude) / differenceMagnitudeSquared;
		this.firstTermSemiLatusRectum = this.positionOneMagnitude*this.positionTwoMagnitude - vectorDotProduct(this.positionOne, this.positionTwo);

		this.firstTermEccentricity = multiplyMatrixWithScalar((this.positionOneMagnitude - this.positionTwoMagnitude) / differenceMagnitudeSquared, vectorDifference);
		this.secondTermEccentricity = vectorCrossProduct(multiplyMatrixWithScalar((this.positionOneMagnitude + this.positionTwoMagnitude) / differenceMagnitudeSquared, this.normalVector), vectorDifference);

		// Calculate starting and goal velocities
		const startingSpeed = getSpeed(startingTrueAnomaly, startingOrbit, body);
		this.startingLocalVectors = getLocalVectors(startingTrueAnomaly, startingOrbit);
		this.startingVelocity = multiplyMatrixWithScalar(startingSpeed, this.startingLocalVectors.prograde);

		const goalSpeed = getSpeed(goalTrueAnomaly, goalOrbit, body);
		this.goalLocalVectors = getLocalVectors(goalTrueAnomaly, goalOrbit);
		this.goalVelocity = multiplyMatrixWithScalar(goalSpeed, this.goalLocalVectors.prograde);

		this.extremalGamma = -(this.positionOneMagnitude*this.positionTwoMagnitude - vectorDotProduct(this.positionOne, this.positionTwo)) / vectorDotProduct(this.normalVector, (crossProduct));
		this.parabolaGamma = Math.sqrt(2*(this.positionOneMagnitude*this.positionTwoMagnitude - vectorDotProduct(this.positionOne, this.positionTwo))) / getVectorMagnitude(addVector(this.positionOne, this.positionTwo));
	}

	getTransfer(gamma: number): Transfer {
		let semiLatusRectum = this.multiplierSemiLatusRectum * (this.firstTermSemiLatusRectum + gamma * this.gammaMultiplierSemiLatusRectum);
		let eccentricityVector = subtractVector(this.firstTermEccentricity, multiplyMatrixWithScalar(gamma, this.secondTermEccentricity));

		let eccentricity = getVectorMagnitude(eccentricityVector);

		// If the eccentrity is near zero, the orbit is near circular, and the choice of localX is pretty much irrelevant
		let localX;
		if (eccentricity < 0.0001) {
			localX = normalizeVector(this.positionOne);
		} else {
			localX = multiplyMatrixWithScalar(1 / eccentricity, eccentricityVector);
		}
		let localY = normalizeVector(vectorCrossProduct(this.normalVector, localX));

		let transferOrbit: Orbit = {
			semiLatusRectum: semiLatusRectum,
			eccentricity: eccentricity,
			coordinateAxes: [localX, localY, this.normalVector]
		};

		let transferStartTrueAnomaly = Math.atan2(vectorDotProduct(this.positionOne, localY), vectorDotProduct(this.positionOne, localX));
		let transferGoalTrueAnomaly = Math.atan2(vectorDotProduct(this.positionTwo, localY), vectorDotProduct(this.positionTwo, localX));

		while (transferGoalTrueAnomaly < transferStartTrueAnomaly) {
			transferGoalTrueAnomaly += 2 * Math.PI;
		}

		if (transferGoalTrueAnomaly > transferStartTrueAnomaly + 2 * Math.PI) {
			transferGoalTrueAnomaly -= 2 * Math.PI;
		};

		while (transferStartTrueAnomaly < -Math.PI) {
			transferStartTrueAnomaly += 2 * Math.PI;
			transferGoalTrueAnomaly += 2 * Math.PI;
		}

		while (transferStartTrueAnomaly >= Math.PI) {
			transferStartTrueAnomaly -= 2 * Math.PI;
			transferGoalTrueAnomaly -= 2 * Math.PI;
		}

		let closestPoint;
		if ((transferStartTrueAnomaly < 0 && transferGoalTrueAnomaly >= 0) || (transferStartTrueAnomaly < 2 * Math.PI && transferGoalTrueAnomaly >= 2 * Math.PI)) {
			closestPoint = semiLatusRectum / (1 + eccentricity);
		} else {
			closestPoint = Math.min(this.positionOneMagnitude, this.positionTwoMagnitude);
		}

		let farthestPoint;
		if (transferStartTrueAnomaly < Math.PI && transferGoalTrueAnomaly >= Math.PI) {
			if (eccentricity < 1) {
				farthestPoint = semiLatusRectum / (1 - eccentricity);
			} else {
				farthestPoint = 1e200;
			}
		} else {
			farthestPoint = Math.max(this.positionOneMagnitude, this.positionTwoMagnitude);
		}

		const transferStartSpeed = getSpeed(transferStartTrueAnomaly, transferOrbit, this.body);
		const transferStartVectors = getLocalVectors(transferStartTrueAnomaly, transferOrbit);
		const transferStartVelocity = multiplyMatrixWithScalar(transferStartSpeed, transferStartVectors.prograde);

		const transferStartVelocityChange = subtractVector(transferStartVelocity, this.startingVelocity);
		const transferStartTotalDeltaV = getVectorMagnitude(transferStartVelocityChange);

		const transferStartPrograde = vectorDotProduct(transferStartVelocityChange, this.startingLocalVectors.prograde);
		const transferStartNormal = vectorDotProduct(transferStartVelocityChange, this.startingLocalVectors.normal);
		const transferStartRadial = vectorDotProduct(transferStartVelocityChange, this.startingLocalVectors.radial);

		const startManoeuvre: Manoeuvre = {
			time: 0,
			progradeDeltaV: transferStartPrograde,
			radialDeltaV: transferStartRadial,
			normalDeltaV: transferStartNormal,
			totalDeltaV: transferStartTotalDeltaV
		};

		const transferGoalSpeed = getSpeed(transferGoalTrueAnomaly, transferOrbit, this.body);
		const transferGoalVectors = getLocalVectors(transferGoalTrueAnomaly, transferOrbit);
		const transferGoalVelocity = multiplyMatrixWithScalar(transferGoalSpeed, transferGoalVectors.prograde);

		const transferGoalVelocityChange = subtractVector(this.goalVelocity, transferGoalVelocity);
		const transferGoalTotalDeltaV = getVectorMagnitude(transferGoalVelocityChange);

		const transferGoalPrograde = vectorDotProduct(transferGoalVelocityChange, transferGoalVectors.prograde);
		const transferGoalRadial = vectorDotProduct(transferGoalVelocityChange, transferGoalVectors.radial);
		const transferGoalNormal = vectorDotProduct(transferGoalVelocityChange, transferGoalVectors.normal);

		const timeToTransfer = getTimeBetweenTrueAnomalies(transferStartTrueAnomaly, transferGoalTrueAnomaly, transferOrbit, this.body);
		const goalManoeuvre: Manoeuvre = {
			time: timeToTransfer,
			progradeDeltaV: transferGoalPrograde,
			radialDeltaV: transferGoalRadial,
			normalDeltaV: transferGoalNormal,
			totalDeltaV: transferGoalTotalDeltaV
		};

		return {
			transferOrbit: transferOrbit,
			transferOrbitTrueAnomalyAtManoeuvreOne: transferStartTrueAnomaly,
			transferOrbitTrueanomalyAtManoeuvreTwo: transferGoalTrueAnomaly,
			firstManoeuvre: startManoeuvre,
			secondManoeuvre: goalManoeuvre,
			closestPointDistance: closestPoint,
			farthestPointDistance: farthestPoint
		}
	}
}

export const ZeroManoeuvre: Manoeuvre = {
	time: 0,
	progradeDeltaV: 0,
	radialDeltaV: 0,
	normalDeltaV: 0,
	totalDeltaV: 0
}

export interface SimplePlaneChange {
	firstManoeuvre: Manoeuvre,
	secondManoeuvre: Manoeuvre
}

export function getCoordinateAxes(inclination: number, longitudeOfAscendingNode: number, argumentOfPeriapsis: number): [number[][], number[][], number[][]] {
	const xAxis = [
		[Math.cos(longitudeOfAscendingNode)*Math.cos(argumentOfPeriapsis) - Math.sin(longitudeOfAscendingNode)*Math.cos(inclination)*Math.sin(argumentOfPeriapsis)],
		[Math.sin(longitudeOfAscendingNode)*Math.cos(argumentOfPeriapsis) + Math.cos(longitudeOfAscendingNode)*Math.cos(inclination)*Math.sin(argumentOfPeriapsis)],
		[Math.sin(inclination)*Math.sin(argumentOfPeriapsis)]
	];

	const yAxis = [
		[-Math.cos(longitudeOfAscendingNode)*Math.sin(argumentOfPeriapsis) - Math.sin(longitudeOfAscendingNode)*Math.cos(inclination)*Math.cos(argumentOfPeriapsis)],
		[-Math.sin(longitudeOfAscendingNode)*Math.sin(argumentOfPeriapsis) + Math.cos(longitudeOfAscendingNode)*Math.cos(inclination)*Math.cos(argumentOfPeriapsis)],
		[Math.sin(inclination)*Math.cos(argumentOfPeriapsis)]
	];

	const zAxis = [
		[Math.sin(longitudeOfAscendingNode)*Math.sin(inclination)],
		[-Math.cos(longitudeOfAscendingNode)*Math.sin(inclination)],
		[Math.cos(inclination)]
	];

	return [xAxis, yAxis, zAxis];
}


export function getOrbit(periapsis: number, apoapsis: number, inclination: number, longitudeOfAscendingNode: number, argumentOfPeriapsis: number): Orbit {
	const semiMajor = (periapsis + apoapsis) / 2;
	const linearEccentricity = semiMajor - periapsis;
	const eccentricity = linearEccentricity / semiMajor;

	return {
		semiLatusRectum: semiMajor * (1 - eccentricity**2),
		eccentricity: eccentricity,
		coordinateAxes: getCoordinateAxes(inclination, longitudeOfAscendingNode, argumentOfPeriapsis)
	};
};

export function getOrbitFromEccentricity(periapsis: number, eccentricity: number, inclination: number, longitudeOfAscendingNode: number, argumentOfPeriapsis: number): Orbit {
	return {
		semiLatusRectum: periapsis * (eccentricity + 1),
		eccentricity: eccentricity,
		coordinateAxes: getCoordinateAxes(inclination, longitudeOfAscendingNode, argumentOfPeriapsis)
	}
};

export function getEccentricAndTrueAnomalyFromMeanAnomaly(meanAnomaly: number, eccentricity: number): [number, number] {
	var eccentricAnomaly;
	var trueAnomaly;

	if (Math.abs(eccentricity - 1) < 0.0001) {
		// Parabolic trajectory, Barker's equation
		const A = 3 * meanAnomaly / Math.sqrt(8);
		const B = Math.pow(A + Math.sqrt(A**2 + 1), 1/3);
		trueAnomaly = 2 * Math.atan(B - 1 / B);
		eccentricAnomaly = trueAnomaly;
	} else {
		// Elliptical or hyperbolic orbit, use Newton's method to find eccentric anomaly
		var keplerEquation;
		var keplerEquationDerivative;
		eccentricAnomaly = meanAnomaly;

		if (eccentricity < 1) {
			keplerEquation = (guess: number) => guess - eccentricity * Math.sin(guess) - meanAnomaly;
			keplerEquationDerivative = (guess: number) => 1 - eccentricity * Math.cos(guess);
		} else {
			keplerEquation = (guess: number) => eccentricity * Math.sinh(guess) - guess - meanAnomaly;
			keplerEquationDerivative = (guess: number) => eccentricity * Math.cosh(guess) - 1;
		}

		while (Math.abs(keplerEquation(eccentricAnomaly)) > 0.0000000001) {
			eccentricAnomaly = eccentricAnomaly - keplerEquation(eccentricAnomaly) / keplerEquationDerivative(eccentricAnomaly);
		}

		if (eccentricity < 1) {
			let beta = eccentricity / (1 + Math.sqrt(1 - eccentricity**2));
			trueAnomaly = eccentricAnomaly + 2 * Math.atan(beta * Math.sin(eccentricAnomaly) / (1 - beta * Math.cos(eccentricAnomaly)));
		} else {
			trueAnomaly = Math.atan2(Math.sqrt(eccentricity**2 - 1) * Math.sinh(eccentricAnomaly), (eccentricity - Math.cosh(eccentricAnomaly)));
		}
	}

	return [eccentricAnomaly, trueAnomaly];
}

export function getOrbitalCoordinates(timeToPeriapsis: number, orbit: Orbit, planet: Body): OrbitalCoordinates {
	let meanAngularMotion;

	if (Math.abs(orbit.eccentricity - 1) < 0.0001) {
		meanAngularMotion = Math.sqrt(8 * planet.gravitationalParameter / orbit.semiLatusRectum**3);
	} else {
		meanAngularMotion = Math.sqrt(planet.gravitationalParameter * Math.abs(1 - orbit.eccentricity**2)**3 / orbit.semiLatusRectum**3);
	}

	const meanAnomaly = meanAngularMotion * -timeToPeriapsis;
	

	const [eccentricAnomaly, trueAnomaly] = getEccentricAndTrueAnomalyFromMeanAnomaly(meanAnomaly, orbit.eccentricity);
	const radius = orbit.semiLatusRectum / (1 + orbit.eccentricity * Math.cos(trueAnomaly));
	const localX = radius * Math.cos(trueAnomaly);
	const localY = radius * Math.sin(trueAnomaly);
	const globalPosition = addVector(multiplyMatrixWithScalar(localX, orbit.coordinateAxes[0]), multiplyMatrixWithScalar(localY, orbit.coordinateAxes[1]));

	return {
		orbit: orbit,
		meanAnomaly: meanAnomaly,
		eccentricAnomaly: eccentricAnomaly,
		trueAnomaly: trueAnomaly,
		meanAngularMotion: meanAngularMotion,
		position: globalPosition,
	}
}

export function getOrbitalCoordinatesFromAltitude(altitude: number, headingInwards: boolean, orbit: Orbit, planet: Body): OrbitalCoordinates {
	// If the eccentricity is zero, this will not work, and we may as well just return assume we're at the periapsis
	if (orbit.eccentricity < 0.0001) {
		return getOrbitalCoordinates(0, orbit, planet);
	}

	let cosineOfTrueAnomaly = (orbit.semiLatusRectum - altitude) / (orbit.eccentricity * altitude);
	if (cosineOfTrueAnomaly < -1 || cosineOfTrueAnomaly > 1) {
		// We're outside the range of this function. Return NAN
		return {
			orbit: orbit,
			meanAnomaly: NaN,
			eccentricAnomaly: NaN,
			trueAnomaly: NaN,
			meanAngularMotion: NaN,
			position: [[NaN], [NaN], [NaN]]
		};
	}

	let trueAnomaly = Math.acos(cosineOfTrueAnomaly) * (headingInwards ? -1 : 1);
	let localX = altitude * Math.cos(trueAnomaly);
	let localY = altitude * Math.sin(trueAnomaly);

	let globalPosition = addVector(
		multiplyMatrixWithScalar(localX, orbit.coordinateAxes[0]),
		multiplyMatrixWithScalar(localY, orbit.coordinateAxes[1])
	);

	let eccentricAnomaly;
	let meanAnomaly;
	let meanAngularMotion: number | null = null;
	if (Math.abs(orbit.eccentricity - 1) < 0.0001) {
		eccentricAnomaly = trueAnomaly;
		meanAnomaly = Math.sqrt(2) * (Math.tan(trueAnomaly / 2) + Math.tan(trueAnomaly / 2)**3 / 3);
		meanAngularMotion = Math.sqrt(8 * planet.gravitationalParameter / orbit.semiLatusRectum**3);
	} else if (orbit.eccentricity < 1) {
		eccentricAnomaly = Math.atan2(Math.sqrt(1 - orbit.eccentricity**2)*Math.sin(trueAnomaly), orbit.eccentricity + Math.cos(trueAnomaly));
		meanAnomaly = eccentricAnomaly - orbit.eccentricity * Math.sin(eccentricAnomaly);
	} else {
		eccentricAnomaly = 2 * Math.atanh(Math.sqrt((orbit.eccentricity - 1) / (orbit.eccentricity + 1)) * Math.tan(trueAnomaly / 2));
		meanAnomaly = orbit.eccentricity * Math.sinh(eccentricAnomaly) - eccentricAnomaly;
	}

	if (meanAngularMotion == null) {
		meanAngularMotion = Math.sqrt(planet.gravitationalParameter * Math.abs(1 - orbit.eccentricity**2)**3 / orbit.semiLatusRectum**3);
	}

	return {
		orbit: orbit,
		meanAnomaly: meanAnomaly,
		eccentricAnomaly: eccentricAnomaly,
		trueAnomaly: trueAnomaly,
		meanAngularMotion: meanAngularMotion,
		position: globalPosition
	};
}

export function getTimeBetweenTrueAnomalies(startingTrueAnomaly: number, endingTrueAnomaly: number, orbit: Orbit, planet: Body): number {
	let extraTime = 0;
	if (Math.abs(orbit.eccentricity - 1) < 0.00001) {
		// Parabola. Solve using Barker's equation
		const startingD = Math.tan(startingTrueAnomaly / 2);
		const endingD = Math.tan(endingTrueAnomaly / 2);

		const startingTime = Math.sqrt(orbit.semiLatusRectum**3 / planet.gravitationalParameter) * (startingD + startingD**3/3) / 2;
		const endingTime = Math.sqrt(orbit.semiLatusRectum**3 / planet.gravitationalParameter) * (endingD + endingD**3/3) / 2;

		return endingTime - startingTime;
	} else {
		var startingMeanAnomaly;
		var endingMeanAnomaly;
		if (orbit.eccentricity < 1) {
			// Ellipse
			const startingEccentricAnomaly = Math.atan2(
				Math.sqrt(1 - orbit.eccentricity**2) * Math.sin(startingTrueAnomaly),
				orbit.eccentricity + Math.cos(startingTrueAnomaly)
			);

			const endingEccentricAnomaly = Math.atan2(
				Math.sqrt(1 - orbit.eccentricity**2) * Math.sin(endingTrueAnomaly),
				orbit.eccentricity + Math.cos(endingTrueAnomaly)
			);

			startingMeanAnomaly = startingEccentricAnomaly - orbit.eccentricity * Math.sin(startingEccentricAnomaly);
			endingMeanAnomaly = endingEccentricAnomaly - orbit.eccentricity * Math.sin(endingEccentricAnomaly);

			while (endingMeanAnomaly < startingMeanAnomaly) {
				endingMeanAnomaly += 2*Math.PI;
			}

			// Add extra orbits if necessary
			if (endingTrueAnomaly > startingTrueAnomaly + 2 * Math.PI) {
				let orbitalPeriod = 2 * Math.PI * Math.sqrt(orbit.semiLatusRectum**3 / (planet.gravitationalParameter * (1 - orbit.eccentricity**2)**3));
				let extraOrbits = Math.floor((endingTrueAnomaly - startingTrueAnomaly) / (2 * Math.PI));
				extraTime = extraOrbits * orbitalPeriod;
			}
		} else {
			const startingEccentricAnomaly = 2*Math.atanh(Math.sqrt((orbit.eccentricity - 1)/(orbit.eccentricity + 1)) * Math.tan(startingTrueAnomaly / 2));
			const endingEccentricAnomaly = 2*Math.atanh(Math.sqrt((orbit.eccentricity - 1)/(orbit.eccentricity + 1)) * Math.tan(endingTrueAnomaly / 2));

			startingMeanAnomaly = orbit.eccentricity * Math.sinh(startingEccentricAnomaly) - startingEccentricAnomaly;
			endingMeanAnomaly = orbit.eccentricity * Math.sinh(endingEccentricAnomaly) - endingEccentricAnomaly;
		}

		const startingTime = Math.sqrt(orbit.semiLatusRectum**3 / (planet.gravitationalParameter * Math.abs(1 - orbit.eccentricity**2)**3)) * startingMeanAnomaly;
		const endingTime = Math.sqrt(orbit.semiLatusRectum**3 / (planet.gravitationalParameter * Math.abs(1 - orbit.eccentricity**2)**3)) * endingMeanAnomaly;

		return endingTime - startingTime + extraTime;
	}
}

export function extrapolateTrajectory(addedTime: number, orbitalCoordinates: OrbitalCoordinates, body: Body): OrbitalCoordinates | null {
	const newMeanAnomaly = orbitalCoordinates.meanAnomaly + orbitalCoordinates.meanAngularMotion * addedTime;
	const [newEccentricAnomaly, newTrueAnomaly] = getEccentricAndTrueAnomalyFromMeanAnomaly(newMeanAnomaly, orbitalCoordinates.orbit.eccentricity);

	// Calculate new position
	const newRadius = orbitalCoordinates.orbit.semiLatusRectum / (1 + orbitalCoordinates.orbit.eccentricity * Math.cos(newTrueAnomaly));

	// If we are outside our planet's sphere of influence, return nothing
	if (newRadius >= body.sphereOfInfluence) {
		return null;
	}

	const localX = newRadius * Math.cos(newTrueAnomaly);
	const localY = newRadius * Math.sin(newTrueAnomaly);
	const newPosition = addVector(
		multiplyMatrixWithScalar(localX, orbitalCoordinates.orbit.coordinateAxes[0]),
		multiplyMatrixWithScalar(localY, orbitalCoordinates.orbit.coordinateAxes[1])
	);

	return {
		orbit: orbitalCoordinates.orbit,
		meanAnomaly: newMeanAnomaly,
		eccentricAnomaly: newEccentricAnomaly,
		trueAnomaly: newTrueAnomaly,
		meanAngularMotion: orbitalCoordinates.meanAngularMotion,
		position: newPosition
	};
}

export function getLocalVectors(trueAnomaly: number, orbit: Orbit): LocalVectors {
	const changeInX = -orbit.semiLatusRectum * Math.sin(trueAnomaly) / (1 + orbit.eccentricity * Math.cos(trueAnomaly))**2;
	const changeInY = orbit.semiLatusRectum * (orbit.eccentricity + Math.cos(trueAnomaly)) / (1 + orbit.eccentricity * Math.cos(trueAnomaly))**2;
	const localHeading = Math.atan2(changeInY, changeInX);
	const localPrograde = [
		[Math.cos(localHeading)],
		[Math.sin(localHeading)],
		[0]
	];

	const localRadial = [
		[Math.sin(localHeading)],
		[-Math.cos(localHeading)],
		[0]
	];

	const globalPrograde = addVector(
		multiplyMatrixWithScalar(localPrograde[0][0], orbit.coordinateAxes[0]),
		multiplyMatrixWithScalar(localPrograde[1][0], orbit.coordinateAxes[1])
	)

	const globalRadial = addVector(
		multiplyMatrixWithScalar(localRadial[0][0], orbit.coordinateAxes[0]),
		multiplyMatrixWithScalar(localRadial[1][0], orbit.coordinateAxes[1])
	);

	return {
		prograde: globalPrograde,
		radial: globalRadial,
		normal: orbit.coordinateAxes[2]
	}
}

export function getSpeed(trueAnomaly: number, orbit: Orbit, planet: Body): number {
	return Math.sqrt(planet.gravitationalParameter * (1 + 2 * orbit.eccentricity * Math.cos(trueAnomaly) + orbit.eccentricity**2) / orbit.semiLatusRectum);
}

export function getOrbitalPeriod(semiMajor: number, body: Body) {
	return Math.sqrt(semiMajor**3 / body.gravitationalParameter);
}

export function perform2dGradientDescent(functionToMinimize: ((variableOne: number, variableTwo: number) => number | null), initialGuessVariableOne: number, initialGuessVaribaleTwo: number, maxDifference?: number, maxTries?: number, variableOneBounds?: [number, number], variableTwoBounds?: [number, number]): [number, number] | null {
	let variableOne = initialGuessVariableOne;
	let variableTwo = initialGuessVaribaleTwo;
	let bestValue = functionToMinimize(variableOne, variableTwo);

	if (bestValue == null) {
		return null;
	}

	maxDifference = maxDifference ?? 0.5;
	maxTries = maxTries ?? 1000;

	// Do some gradient descent on the best values found so far
	let tries = 0;
	while (bestValue != null && bestValue > maxDifference && tries < maxTries) {
		tries++;

		let dfx = functionToMinimize(variableOne+0.0001, variableTwo);
		let dfy = functionToMinimize(variableOne, variableTwo+0.0001);

		let estimateDfDx = null;
		let estimateDfDy = null;
		
		if (dfx) {
			estimateDfDx = (dfx - bestValue) / 0.0001;
		}

		if (dfy) {
			estimateDfDy = (functionToMinimize(variableOne, variableTwo+0.0001) ?? bestValue - bestValue) / 0.0001;
		}

		let direction: number[][] | null = null;

		if (estimateDfDx && estimateDfDy) {
			let slopeNormal = vectorCrossProduct(
				[[1], [0], [estimateDfDx]],
				[[0], [1], [estimateDfDy]]
			);	

			direction = normalizeVector([[slopeNormal[0][0]], [slopeNormal[1][0]], [0]]);
		} else if (estimateDfDx) {
			direction = [[1], [0], [0]];
		} else if (estimateDfDy) {
			direction = [[0], [1], [0]];
		}

		if (!direction) {
			break;
		}

		let df = functionToMinimize(variableOne + direction[0][0]*0.0001, variableTwo + direction[1][0]*0.0001);
		if (!df) {
			break;
		}

		let estimateDfDv = (df - bestValue) / 0.0001;

		let bestValueImprovement: number = bestValue;
		let variableOneUpdate = variableOne;
		let variableTwoUpdate = variableTwo;
		let divisor = 0;
		let deadEnd = false;

		while (bestValueImprovement >= bestValue) {
			variableOneUpdate = variableOne - bestValue * direction[0][0] / (estimateDfDv * 2**divisor);
			variableTwoUpdate = variableTwo - bestValue * direction[1][0] / (estimateDfDv * 2**divisor);
			bestValueImprovement = functionToMinimize(variableOneUpdate, variableTwoUpdate) ?? bestValue;
			divisor += 1;

			if (divisor >= 32) {
				deadEnd = true;
				break;
			}
		}

		if (deadEnd) {
			break;
		}

		if (variableOneBounds) {
			if (variableOneUpdate < variableOneBounds[0] || variableOneUpdate > variableOneBounds[1]) {
				break;
			}
		}

		if (variableTwoBounds) {
			if (variableTwoUpdate < variableTwoBounds[0] || variableTwoUpdate > variableTwoBounds[1]) {
				break;
			}
		}

		variableOne = variableOneUpdate;
		variableTwo = variableTwoUpdate;
		bestValue = bestValueImprovement;
	}

	return [variableOne, variableTwo];
}

export function calculateSimplePlaneChange(coordinates: OrbitalCoordinates, planet: Body, targetInclination: number, targetLongitudeOfAscendingNode: number, circularizeOrbit: boolean): SimplePlaneChange {
	const otherPlaneNormal = [
		[Math.sin(targetLongitudeOfAscendingNode)*Math.sin(targetInclination)],
		[-Math.cos(targetLongitudeOfAscendingNode)*Math.sin(targetInclination)],
		[Math.cos(targetInclination)]
	];

	var planesIntersection = vectorCrossProduct(otherPlaneNormal, coordinates.orbit.coordinateAxes[2]);
	if (getVectorMagnitude(planesIntersection) < 0.0001) {
		return {
			firstManoeuvre: ZeroManoeuvre,
			secondManoeuvre: ZeroManoeuvre
		}
	};

	planesIntersection = normalizeVector(planesIntersection);

	// Find true anomalies of crossings
	const intersectionTrueAnomaly = Math.atan2(vectorDotProduct(planesIntersection, coordinates.orbit.coordinateAxes[1]), vectorDotProduct(planesIntersection, coordinates.orbit.coordinateAxes[0]));
	var firstManoeuvre: Manoeuvre = ZeroManoeuvre;
	var secondManoeuvre: Manoeuvre = ZeroManoeuvre;

	var intersections = [intersectionTrueAnomaly, intersectionTrueAnomaly + Math.PI];
	intersections = intersections.map(anomaly => (anomaly - coordinates.trueAnomaly + 10 * Math.PI) % (2 * Math.PI) + coordinates.trueAnomaly).sort();

	intersections.forEach((trueAnomaly, index) => {
		const speed = getSpeed(trueAnomaly, coordinates.orbit, planet);
		const localVectors = getLocalVectors(trueAnomaly, coordinates.orbit);

		const velocity = multiplyMatrixWithScalar(speed, localVectors.prograde);
		var excess: number[][];
		if (circularizeOrbit) {
			const radius = coordinates.orbit.semiLatusRectum / (1 + coordinates.orbit.eccentricity * Math.cos(trueAnomaly));
			const targetSpeed = Math.sqrt(planet.gravitationalParameter / radius);

			const radiusVector = addVector(
				multiplyMatrixWithScalar(Math.cos(trueAnomaly), coordinates.orbit.coordinateAxes[0]),
				multiplyMatrixWithScalar(Math.sin(trueAnomaly), coordinates.orbit.coordinateAxes[1])
			);

			const velocityDirection = normalizeVector(vectorCrossProduct(otherPlaneNormal, radiusVector));
			const targetVelocity = multiplyMatrixWithScalar(targetSpeed, velocityDirection);

			excess = subtractVector(velocity, targetVelocity);
		} else {
			excess = multiplyMatrixWithScalar(vectorDotProduct(velocity, otherPlaneNormal), otherPlaneNormal);
		}

		const totalChange = getVectorMagnitude(excess);
		const progradeChange = -vectorDotProduct(excess, localVectors.prograde);
		const radialChange = -vectorDotProduct(excess, localVectors.radial);
		const normalChange = -vectorDotProduct(excess, localVectors.normal);

		const timeUntil = getTimeBetweenTrueAnomalies(coordinates.trueAnomaly, trueAnomaly, coordinates.orbit, planet);

		const manoeuvre: Manoeuvre = {
			time: timeUntil,
			progradeDeltaV: progradeChange,
			radialDeltaV: radialChange,
			normalDeltaV: normalChange,
			totalDeltaV: totalChange
		}

		if (index == 0) {
			firstManoeuvre = manoeuvre;
		} else {
			secondManoeuvre = manoeuvre;
		}
	});

	return {
		firstManoeuvre: firstManoeuvre,
		secondManoeuvre: secondManoeuvre
	}
}

export function findCheapestLambertSolution(lambertSolutions: LambertSolutions): Transfer | null {
	// Test a bunch of gamma values
	let stepLength = (lambertSolutions.parabolaGamma - lambertSolutions.extremalGamma) / 100;
	let bestTransfer = null;
	let bestDeltaV = null;

	for (var i = 1; i < 200; i++) {
		let gamma = lambertSolutions.extremalGamma + i * stepLength;
		let transfer = lambertSolutions.getTransfer(gamma);

		if (transfer.closestPointDistance < lambertSolutions.body.closestSafeDistance) {
			continue;
		}

		if (transfer.farthestPointDistance > lambertSolutions.body.sphereOfInfluence) {
			continue;
		}

		let totalDeltaV = transfer.firstManoeuvre.totalDeltaV + transfer.secondManoeuvre.totalDeltaV;
		if (isNaN(totalDeltaV)) {
			continue;
		}

		if (bestDeltaV == null || totalDeltaV < bestDeltaV) {
			bestDeltaV = totalDeltaV;
			bestTransfer = transfer;
		}
	}

	return bestTransfer;
}

export type ProgressCallbackFunction = (transfersChecked: number, totalNumberOfTransfers: number, currentBestDeltaV: number | null, currentBestTransfer: Transfer | null) => void;
export type LandingProgressCallbackFunction = (pathsChecked: number, totalNumberOfPaths: number, currentBestDeltaV: number | null, currentBestLanding: Landing | null) => void;

export function findCheapestTransfer(startingSituation: OrbitalCoordinates, targetOrbit: Orbit, body: Body, progressCallback?: ProgressCallbackFunction): Transfer | null {
	// First, create a set of starting true anomalies
	let startingTrueAnomalies = [];

	let stableOrbit = false;
	if (startingSituation.orbit.eccentricity < 1) {
		// We might still not be in a stable orbit, if the apoapsis is beyond the body's sphere of influence
		let apoapsis = startingSituation.orbit.semiLatusRectum / (1 - startingSituation.orbit.eccentricity);
		if (apoapsis < body.sphereOfInfluence) {
			stableOrbit = true;
			// If the orbit is stable and all that, just sample the true anomalies equally
		}
	}

	if (stableOrbit) {
		for (var i = 0; i < 100; i++) {
			startingTrueAnomalies.push(i * 2 * Math.PI / 100);
		}
	} else {
		let finalAnomaly = Math.abs(Math.acos((startingSituation.orbit.semiLatusRectum - body.sphereOfInfluence) / (body.sphereOfInfluence * startingSituation.orbit.eccentricity)));
		let step = (finalAnomaly - startingSituation.trueAnomaly) / 100;
		for (var i = 0; i < 101; i++) {
			startingTrueAnomalies.push(startingSituation.trueAnomaly + i * step);
		}
	}

	// Next, find a set of true anomalies to aim for in the target orbit
	let endingTrueAnomalies = [];
	let targetIsStable = false;
	if (targetOrbit.eccentricity < 1) {
		let targetApoapsis = targetOrbit.semiLatusRectum / (1 - targetOrbit.eccentricity);
		if (targetApoapsis < body.sphereOfInfluence) {
			targetIsStable = true;
		}
	}

	if (targetIsStable) {
		for (var i = 0; i < 100; i++) {
			endingTrueAnomalies.push(i * 2 * Math.PI / 100);
		}
	} else {
		let finalAnomaly = Math.abs(Math.acos((targetOrbit.semiLatusRectum - body.sphereOfInfluence) / (body.sphereOfInfluence * targetOrbit.eccentricity)));
		let step = 2 * finalAnomaly / 100;
		for (var i = 0; i < 100; i++) {
			endingTrueAnomalies.push(-finalAnomaly + i * step);
		}
	}

	let bestTransfer: Transfer | null = null;
	let bestDeltaV: number | null = null;

	let totalAnomalies = startingTrueAnomalies.length * endingTrueAnomalies.length * 2;
	let numberChecked = 0;

	startingTrueAnomalies.forEach(startingAnomaly => {
		while (startingAnomaly < startingSituation.trueAnomaly) {
			startingAnomaly += 2*Math.PI;
		}

		let timeToAnomaly = getTimeBetweenTrueAnomalies(startingSituation.trueAnomaly, startingAnomaly, startingSituation.orbit, body);

		endingTrueAnomalies.forEach(endingAnomaly => {
			[true, false].forEach(goBackwards => {
				let lambertSolutions = new LambertSolutions(startingSituation.orbit, startingAnomaly, targetOrbit, endingAnomaly, body, goBackwards);
				let currentBestTransfer = findCheapestLambertSolution(lambertSolutions);
				if (currentBestTransfer) {
					let totalDeltaV = currentBestTransfer.firstManoeuvre.totalDeltaV + currentBestTransfer.secondManoeuvre.totalDeltaV;
					if (bestDeltaV == null || totalDeltaV < bestDeltaV) {
						bestDeltaV = totalDeltaV;
						bestTransfer = currentBestTransfer;
						bestTransfer.firstManoeuvre.time += timeToAnomaly;
						bestTransfer.secondManoeuvre.time += timeToAnomaly;
					}
				}

				if (progressCallback) {
					numberChecked += 1;
					progressCallback(numberChecked, totalAnomalies, bestDeltaV, bestTransfer);
				}
			});
		})
	});

	return bestTransfer;
}

export function findLambertSolutionsWithCorrectTime(lambertSolutions: LambertSolutions, expectedTime: number): Transfer | null {	
	// Do a secant method search. Start near the parabola
	let scale = Math.abs(lambertSolutions.parabolaGamma - lambertSolutions.extremalGamma);
	let x0 = lambertSolutions.parabolaGamma;
	let x1 = lambertSolutions.parabolaGamma + scale / 100;

	let t0 = lambertSolutions.getTransfer(x0);
	let t1 = lambertSolutions.getTransfer(x1);

	let f0 = t0.secondManoeuvre.time - expectedTime;
	let f1 = t1.secondManoeuvre.time - expectedTime;

	let f2 = 100;
	let t2 = null;

	let counter = 0;

	while (Math.abs(f2) > 0.5 && counter < 100) {
		counter++;
		let divisor = 1;
		let step = f1 * (x1 - x0) / (f1 - f0);

		let manoeuvreTime = -1;
		let x2 = 0;
		let innerCounter = 0;
		while ((isNaN(manoeuvreTime) || manoeuvreTime < 0) && innerCounter < 10) {
			innerCounter++;
			x2 = x1 - step / (2**divisor);
			t2 = lambertSolutions.getTransfer(x2);
			manoeuvreTime = t2.secondManoeuvre.time;
			divisor += 1;
		}

		if (innerCounter == 10) {
			break;
		}

		f2 = manoeuvreTime - expectedTime;

		x0 = x1;
		x1 = x2;
		f0 = f1;
		f1 = f2;
	}

	if (Math.abs(f2) <= 0.5) {
		return t2;
	} else {
		return null;
	}
}

export function findCheapestIntercept(startingSituation: OrbitalCoordinates, targetSituation: OrbitalCoordinates, body: Body, extraTrueAnomaly: number, progressCallback?: ProgressCallbackFunction): Transfer | null {
	// Find acceptable intercept times
	// First number is intercept time, second number is true anomaly
	let startTimes: number[] = [];
	let endTimes: number[] = [];

	// Check if the starting orbit is stable0
	let startingOrbitStable = false;
	if (startingSituation.orbit.eccentricity < 1) {
		let startingApoapsis = startingSituation.orbit.semiLatusRectum / (1 - startingSituation.orbit.eccentricity);
		if (startingApoapsis < body.sphereOfInfluence) {
			startingOrbitStable = true;
		}
	}

	// Next, check if intercept orbit is stable
	let interceptOrbitStable = false;
	if (targetSituation.orbit.eccentricity < 1) {
		let targetApoapsis = targetSituation.orbit.semiLatusRectum / (1 - targetSituation.orbit.eccentricity);
		if (targetApoapsis < body.sphereOfInfluence) {
			interceptOrbitStable = true;
		}
	}

	let maxEndTime: number | null = null;
	let maxStartTime: number | null = null;

	if (!startingOrbitStable) {
		let maxStartTrueAnomaly = Math.abs(Math.acos((startingSituation.orbit.semiLatusRectum - body.sphereOfInfluence) / (body.sphereOfInfluence * startingSituation.orbit.eccentricity)));
		maxStartTime = getTimeBetweenTrueAnomalies(startingSituation.trueAnomaly, maxStartTrueAnomaly, startingSituation.orbit, body);
		// If we're leaving the galaxy, set the max end time to the time it would take to orbit at the edge of the sphere of influence
		maxEndTime = getOrbitalPeriod(body.sphereOfInfluence, body);
	}

	if (!interceptOrbitStable) {
		let maxEndTrueAnomaly = Math.abs(Math.acos((targetSituation.orbit.semiLatusRectum - body.sphereOfInfluence) / (body.sphereOfInfluence * targetSituation.orbit.eccentricity)));
		maxEndTime = Math.min(maxEndTime ?? 1e99, getTimeBetweenTrueAnomalies(targetSituation.trueAnomaly, maxEndTrueAnomaly, targetSituation.orbit, body));
	}

	if (startingOrbitStable && interceptOrbitStable) {
		// If both orbits are stable, try for four of the longest orbit
		let startingSemiMajor = startingSituation.orbit.semiLatusRectum / (1 - startingSituation.orbit.eccentricity**2);
		let targetSemiMajor = targetSituation.orbit.semiLatusRectum / (1 - targetSituation.orbit.eccentricity**2);

		let startingOrbitalPeriod = getOrbitalPeriod(startingSemiMajor, body);
		let targetOrbitalPeriod = getOrbitalPeriod(targetSemiMajor, body);

		maxStartTime = 4 * Math.max(startingOrbitalPeriod, targetOrbitalPeriod);
		maxEndTime = 5 * Math.max(startingOrbitalPeriod, targetOrbitalPeriod);
	}

	// Max end time should have been set by now, but Typescript doesn't seem to know that
	maxEndTime = maxEndTime ?? 0;

	// No point in having start times after the end times
	if (!maxStartTime || maxStartTime > maxEndTime) {
		maxStartTime = maxEndTime;
	}

	// We'll try starting at 100 different start and end times
	let startTimeStep = maxStartTime / 100;
	let endTimeStep = maxEndTime / 100;

	for (var i = 0; i < 100; i++) {
		startTimes.push(i * startTimeStep);
		endTimes.push(i * endTimeStep);
	}

	// Create pairs to test
	let pairs: [number, number][] = [];
	startTimes.forEach(startTime => {
		endTimes.forEach(endTime => {
			if (endTime > startTime) {
				pairs.push([startTime, endTime]);
			}
		})
	});

	const getTransfer = (startTime: number, endTime: number, backwards: boolean): Transfer | null => {
		let startOrbitMeanAnomaly = startingSituation.meanAnomaly + startTime * startingSituation.meanAngularMotion;
		let endOrbitMeanAnomaly = targetSituation.meanAnomaly + endTime * targetSituation.meanAngularMotion;

		if (Math.abs(startingSituation.orbit.eccentricity - 1) < 0.0001) {
			
		}

		let startTrueAnomaly: number = 0;
		let endTrueAnomaly: number = 0;

		["start", "end"].forEach(orbit => {
			let meanAnomaly = orbit == "start" ? startOrbitMeanAnomaly : endOrbitMeanAnomaly;
			let eccentricity = orbit == "start" ? startingSituation.orbit.eccentricity : targetSituation.orbit.eccentricity;
			let extraOrbits = 0;
			if (eccentricity < 1) {
				extraOrbits = Math.floor(meanAnomaly / (2 * Math.PI));
				meanAnomaly = meanAnomaly % (2 * Math.PI);
			}
			let [_, trueAnomaly] = getEccentricAndTrueAnomalyFromMeanAnomaly(meanAnomaly, eccentricity);
			trueAnomaly += extraOrbits * 2 * Math.PI;

			if (orbit == "start") {
				startTrueAnomaly = trueAnomaly;
			} else if (orbit == "end") {
				endTrueAnomaly = trueAnomaly;
			}
		});

		let targetTransferTime = endTime - startTime;
		let lambertSolutions = new LambertSolutions(startingSituation.orbit, startTrueAnomaly, targetSituation.orbit, endTrueAnomaly + extraTrueAnomaly, body, backwards);

		let transfer = findLambertSolutionsWithCorrectTime(lambertSolutions, targetTransferTime);
		if (transfer) {
			transfer.firstManoeuvre.time += startTime;
			transfer.secondManoeuvre.time += startTime;
		}

		return transfer;
	}

	// Create the function we want to gradient descend
	const getInterceptFunction = (backwards: boolean) => {
		return function(startTime: number, endTime: number): number | null {
			let transfer = getTransfer(startTime, endTime, backwards);
			if (!transfer) {
				return null;
			}

			if (transfer.farthestPointDistance >= body.sphereOfInfluence || transfer.closestPointDistance <= body.closestSafeDistance) {
				return null;
			}

			// Sometimes, we get eccentric orbits that are impossible to do
			if (transfer.transferOrbitTrueAnomalyAtManoeuvreOne < Math.PI && transfer.transferOrbitTrueanomalyAtManoeuvreTwo > Math.PI && transfer.transferOrbit.eccentricity - 1 >= 0) {
				return null;
			}

			return transfer.firstManoeuvre.totalDeltaV + transfer.secondManoeuvre.totalDeltaV;
		}
	};

	// Start at each pair, and do a little gradient descending
	let bestDeltaV: number | null = null;;
	let bestTransfer: Transfer | null = null;

	pairs.forEach(([startTime, endTime], index) => {
		// Try both forwards and backwards
		[true, false].forEach(backwards => {
			let interceptFunction = getInterceptFunction(backwards);

			let foundStartTime = startTime;
			let foundEndTime = endTime;

			let foundWorkingTransfer = false;
			let trialCounter = 0;
			while (!foundWorkingTransfer && trialCounter < 20) {
				trialCounter++;

				let testTransfer = interceptFunction(foundStartTime, foundEndTime);
				if (testTransfer) {
					foundWorkingTransfer = true;
				} else {
					foundStartTime = Math.max(0,startTime + Math.random() * 0.5 * startTimeStep);
					foundEndTime = Math.max(0,endTime + Math.random() * 0.5 * endTimeStep);

					if (foundEndTime < foundStartTime) {
						foundEndTime += startTimeStep;
					}
				}
			}

			if (!foundWorkingTransfer) {
				return;
			}

			let result = perform2dGradientDescent(interceptFunction, foundStartTime, foundEndTime, 0, 30, [0, 1e99]);
			if (result) {
				let foundTransfer = getTransfer(result[0], result[1], backwards);
				if (foundTransfer) {
					let transferDeltaV = foundTransfer.firstManoeuvre.totalDeltaV + foundTransfer.secondManoeuvre.totalDeltaV;
					if (bestDeltaV == null || transferDeltaV < bestDeltaV) {
						bestDeltaV = transferDeltaV;
						bestTransfer = foundTransfer;
					}
				}
			}
		});

		if (progressCallback) {
			progressCallback(index+1, pairs.length, bestDeltaV, bestTransfer);
		}
	});

	return bestTransfer;
}

export function findCheapestLanding(startingCoordinates: OrbitalCoordinates, startTime: number, ship: ShipParameters, landingParameters: LandingParameters, body: Body, progressCallback?: LandingProgressCallbackFunction): Landing | null {
	// Starting points are a list of true anomalies with corresponding times
	let startingPoints: [number, number][] = [];

	let minimumStartingTrueAnomaly = startingCoordinates.trueAnomaly;
	let maximumStartingTrueAnomaly = startingCoordinates.trueAnomaly + 2 * Math.PI;

	let boundedAbove = false;

	// Check if we're on a collision course with the planet
	let minimumAltitude = startingCoordinates.orbit.semiLatusRectum / (1 + startingCoordinates.orbit.eccentricity);

	// If we're colliding, we need to do our manoeuvre quickly
	if (minimumAltitude < body.closestSafeDistance) {
		let criticalAnomaly = Math.acos((startingCoordinates.orbit.semiLatusRectum - body.closestSafeDistance) / (body.closestSafeDistance * startingCoordinates.orbit.eccentricity));

		// We might be in a weird orbit where we're on the way away from the planet, in which case we shouldn't bound our starting
		if (minimumStartingTrueAnomaly < -criticalAnomaly) {
			boundedAbove = true;
			maximumStartingTrueAnomaly = -criticalAnomaly;
		}
	}

	// If we're on a parabolic or hyperbolic orbit, or one that will leave the planet's sphere of influence, set an upper bound for the anomaly
	if (!boundedAbove) {
		let goesOutsideSphereOfIncluence = false;
		if (startingCoordinates.orbit.eccentricity < 1) {
			let maximumAltitude = startingCoordinates.orbit.semiLatusRectum / (1 - startingCoordinates.orbit.eccentricity);
			if (maximumAltitude > body.sphereOfInfluence) {
				goesOutsideSphereOfIncluence = true;
			}
		} else {
			goesOutsideSphereOfIncluence = true;
		}

		if (goesOutsideSphereOfIncluence) {
			maximumStartingTrueAnomaly = Math.acos((startingCoordinates.orbit.semiLatusRectum - body.sphereOfInfluence) / (body.sphereOfInfluence * startingCoordinates.orbit.eccentricity));
		}
	}

	// Our set of starting points is decided
	let stepLength = (maximumStartingTrueAnomaly - minimumStartingTrueAnomaly) /  100.0;
	for (let i = 0; i < 100; i++) {
		let testAnomaly = minimumStartingTrueAnomaly + i * stepLength;
		let timeUntilAnomaly = getTimeBetweenTrueAnomalies(minimumStartingTrueAnomaly, testAnomaly, startingCoordinates.orbit, body);
		startingPoints.push([testAnomaly, timeUntilAnomaly + startTime]);
	};

	let bestLanding: Landing | null = null;
	let bestLandingDeltaV: number | null = null;

	// We need to simulate the ship braking, in order to refine the manoeuvre
	const simulateBraking = (transferTrueAnomaly: number, transferOrbit: Orbit, body: Body, ship: ShipParameters) => {
		let localVectors = getLocalVectors(transferTrueAnomaly, transferOrbit);
		let speed = getSpeed(transferTrueAnomaly, transferOrbit, body);
		let velocity = multiplyMatrixWithScalar(speed, localVectors.prograde);

		let radius = transferOrbit.semiLatusRectum / (1 + transferOrbit.eccentricity * Math.cos(transferTrueAnomaly));
		let localX = radius * Math.cos(transferTrueAnomaly);
		let localY = radius * Math.sin(transferTrueAnomaly);

		let startingPosition = addVector(
			multiplyMatrixWithScalar(localX, transferOrbit.coordinateAxes[0]),
			multiplyMatrixWithScalar(localY, transferOrbit.coordinateAxes[1])
		);

		let position = startingPosition;

		let mass = ship.mass;
		let massUse = ship.thrust / (ship.specificImpulse * 9.81);

		let timeSteps = 0;
		let lengthOfStep = 0.01;

		let totalDeltaV = 0;

		while (true) {
			let radius = getVectorMagnitude(position);
			let downwardsVector = multiplyMatrixWithScalar(-1 / radius, position);
			let gravityAcceleration = body.gravitationalParameter / radius**2;
			let gravityVector = multiplyMatrixWithScalar(gravityAcceleration, downwardsVector);

			let latitude = Math.atan2(position[2][0], Math.sqrt(position[0][0]**2 + position[1][0]**2));
			let longitude = Math.atan2(position[1][0], position[0][0]);

			let surfaceSpeed = body.radius * Math.cos(latitude) * 2 * Math.PI / body.rotationPeriod;
			let surfaceVelocity = [
				[-surfaceSpeed * Math.sin(longitude)],
				[surfaceSpeed * Math.cos(longitude)],
				[0]
			];

			let excessVelocity = subtractVector(velocity, surfaceVelocity);
			let excessSpeed = getVectorMagnitude(excessVelocity);

			let engineAcceleration = ship.thrust / mass;
			if (excessSpeed < lengthOfStep * engineAcceleration) {
				break;
			}

			let engineAccelerationVector = multiplyMatrixWithScalar(-engineAcceleration, normalizeVector(excessVelocity));
			let totalAcceleration = addVector(engineAccelerationVector, gravityVector);

			position = addVector(position, addVector(multiplyMatrixWithScalar(lengthOfStep, velocity), multiplyMatrixWithScalar(lengthOfStep**2 / 2, totalAcceleration)));
			velocity = addVector(velocity, multiplyMatrixWithScalar(lengthOfStep, totalAcceleration));
			mass -= massUse * lengthOfStep;
			totalDeltaV += engineAcceleration * lengthOfStep;

			if (mass < 0) {
				return null;
			}

			timeSteps += 1;
		}

		return {
			timeSpent: timeSteps * lengthOfStep,
			finalPosition: position,
			deltaVSpent: totalDeltaV
		};
	};

	const createFakeOrbit = (position: number[][]): Orbit => {
		let radius = getVectorMagnitude(position);
		let longitude = Math.atan2(position[1][0], position[0][0]);
		let latitude = Math.atan2(position[2][0], Math.sqrt(position[0][0]**2 + position[1][0]**2));

		if (latitude > 0) {
			return getOrbit(radius, radius, latitude, longitude - Math.PI / 2, Math.PI / 2);
		} else {
			return getOrbit(radius, radius, -latitude, longitude + Math.PI / 2, -Math.PI / 2);
		}
	}

	let freefallMultiplier = Math.PI / (2 * Math.sqrt(2 * body.gravitationalParameter));
	startingPoints.forEach(([trueAnomaly, startTime], startingIndex) => {
		// Test 100 different landing times at each true anomaly. We'll set the upper bound for the transfer time to be one and a
		// half the time it takes to free fall from the current position
		let currentRadius = startingCoordinates.orbit.semiLatusRectum / (1 + startingCoordinates.orbit.eccentricity * Math.cos(trueAnomaly));
		let freeFallTime = freefallMultiplier * currentRadius ** (3 / 2);

		for (let i = 1; i < 101; i++) {
			let targetTime = 1.5 * freeFallTime * i / 100;
			let rotatedTargetLongitude = landingParameters.targetLongitude + body.initialMeridianLongitude + (targetTime + startTime) * 2 * Math.PI / body.rotationPeriod;

			let targetX = (landingParameters.targetAltitude + body.radius) * Math.cos(landingParameters.targetLatitude) * Math.cos(rotatedTargetLongitude);
			let targetY = (landingParameters.targetAltitude + body.radius) * Math.cos(landingParameters.targetLatitude) * Math.sin(rotatedTargetLongitude);
			let targetZ = (landingParameters.targetAltitude + body.radius) * Math.sin(landingParameters.targetLatitude);

			let target = [[targetX], [targetY], [targetZ]];

			// Make up an orbit that goes through this point, so we can use it with the Lambert solver I made earlier that always
			// assumes you want to go between two orbits
			let fakeTargetOrbit = createFakeOrbit(target);

			[true, false].forEach(backwards => {
				let lambertSolutions = new LambertSolutions(startingCoordinates.orbit, trueAnomaly, fakeTargetOrbit, 0, body, backwards);
				let possibleTransfer = findLambertSolutionsWithCorrectTime(lambertSolutions, targetTime);

				if (possibleTransfer) {
					// Throw out any transfers that go below the surface and come up on our rendezvous
					if (possibleTransfer.closestPointDistance < landingParameters.targetAltitude + body.radius - 1) {
						return;
					}

					// Check if the transfer comes in steep enough
					let localVectors = getLocalVectors(possibleTransfer.transferOrbitTrueanomalyAtManoeuvreTwo, possibleTransfer.transferOrbit);
					let targetDownwardsVector = multiplyMatrixWithScalar(-1, normalizeVector(target));

					let maximumAngle = Math.PI / 2 - landingParameters.minimumAngle;
					let angle = Math.acos(vectorDotProduct(localVectors.prograde, targetDownwardsVector));
					if (angle > maximumAngle) {
						return;
					}

					// Do some iterative searching to find a transfer that lands in the middle
					let foundGoodLanding = false;
					let previousTargetPosition = target;
					let previousTargetTime = targetTime;

					let landingCounter = 0;

					while (!foundGoodLanding && landingCounter < 100) {
						landingCounter++;
						let massAfterTransfer = ship.mass / (Math.exp(possibleTransfer.firstManoeuvre.totalDeltaV / (9.81 * ship.specificImpulse)));
						let testShip: ShipParameters = {
							thrust: ship.thrust,
							mass: massAfterTransfer,
							specificImpulse: ship.specificImpulse
						};

						let simulationResult = simulateBraking(possibleTransfer.transferOrbitTrueanomalyAtManoeuvreTwo, possibleTransfer.transferOrbit, body, testShip);
						if (simulationResult == null) {
							return;
						}
						let difference = subtractVector(simulationResult.finalPosition, target);
						let timeDifference = possibleTransfer.secondManoeuvre.time + simulationResult.timeSpent - targetTime;
						let differenceMagnitude = getVectorMagnitude(difference);

						if (timeDifference > 100 || differenceMagnitude > 10) {
							// We're too far away, try another maneuver
							let newTarget = subtractVector(previousTargetPosition, difference);
							let newTime = previousTargetTime - timeDifference;
							let newTargetOrbit = createFakeOrbit(newTarget);

							previousTargetPosition = newTarget;
							previousTargetTime = newTime;

							lambertSolutions = new LambertSolutions(startingCoordinates.orbit, trueAnomaly, newTargetOrbit, 0, body, backwards);
							possibleTransfer = findLambertSolutionsWithCorrectTime(lambertSolutions, newTime);

							if (!possibleTransfer) {
								return;
							}
						} else {
							foundGoodLanding = true;

							possibleTransfer.firstManoeuvre.time += startTime;
							possibleTransfer.secondManoeuvre.time += startTime;

							let totalDeltaV = possibleTransfer.firstManoeuvre.totalDeltaV + simulationResult.deltaVSpent;
							if (bestLandingDeltaV == null || totalDeltaV < bestLandingDeltaV) {
								let secondManoeuvreAltitude = possibleTransfer.transferOrbit.semiLatusRectum / (1 + possibleTransfer.transferOrbit.eccentricity * Math.cos(possibleTransfer.transferOrbitTrueanomalyAtManoeuvreTwo)) - body.radius;

								let landing: Landing = {
									transferOrbit: possibleTransfer.transferOrbit,
									transferManoeuvre: possibleTransfer.firstManoeuvre,
									brakingDeltaV: simulationResult.deltaVSpent,
									brakingAltitude: secondManoeuvreAltitude,
									brakingTimestamp: possibleTransfer.secondManoeuvre.time,
									totalDeltaV: totalDeltaV
								};

								bestLanding = landing;
								bestLandingDeltaV = totalDeltaV;
							}
						}
					};
				}
			});

			if (progressCallback) {
				progressCallback(startingIndex * 100 + i, 100*100, bestLandingDeltaV, bestLanding);
			}
		}
	});

	return bestLanding;
}

export function findOrbitThroughInterpolation(ownCoordinates: OrbitalCoordinates, interpolationParameters: InterpolationParameters, body: Body): [OrbitalCoordinates, number][] {
	let targetPeriapsis = interpolationParameters.targetPeriapsis;

	let targetEccentricity;
	let targetSemiLatusRectum;
	if (interpolationParameters.orbitChoice == "apoapsis") {
		let targetApoapsis = interpolationParameters.targetApoapsis;
		const semiMajor = (targetPeriapsis + targetApoapsis) / 2;
		const linearEccentricity = semiMajor - targetPeriapsis;
		targetEccentricity = linearEccentricity / semiMajor;
		targetSemiLatusRectum = semiMajor * (1 - targetEccentricity**2);
	} else {
		let semiMajorInverse = 2 / interpolationParameters.targetAltitude - interpolationParameters.targetSpeed**2 / body.gravitationalParameter;
		
		// If this is zero, we have a parabolic trajectory
		if (Math.abs(semiMajorInverse) < 0.0001) {
			targetEccentricity = 1;
			targetSemiLatusRectum = 2 * targetPeriapsis;
		} else {
			let semiMajor = 1 / semiMajorInverse;
			// If this is positive, we have an elliptical orbit
			if (semiMajor > 0) {
				const linearEccentricity = semiMajor - targetPeriapsis;
				targetEccentricity = linearEccentricity / semiMajor;
				targetSemiLatusRectum = semiMajor * (1 - targetEccentricity**2);
			} else {
				const linearEccentricity = semiMajor + targetPeriapsis;
				targetEccentricity = linearEccentricity / semiMajor;
				targetSemiLatusRectum = semiMajor * (targetEccentricity**2 - 1);
			}
		}
	}

	// Now, we have a pretty good description of the orbit, we only need to find the plane it lies in
	// Given the first measurement of distance to planet, distance to target, and target to planet, there exists a circle
	// that the target could be in
	let ownShipHeadedInwards = interpolationParameters.secondOwnAltitude < interpolationParameters.firstOwnAltitude;
	let targetHeadedInwards = interpolationParameters.secondTargetAltitude < interpolationParameters.firstTargetAltitude;

	const getTrueAnomalyFromAltitude = (altitude: number, semiLatusRectum: number, eccentricity: number, headingInwards: boolean) => {
		let trueAnomaly = Math.acos((semiLatusRectum - altitude) / (altitude * eccentricity));
		if (headingInwards) {
			trueAnomaly = -trueAnomaly;
		}
		return trueAnomaly;
	}
	
	const ownFirstTrueAnomaly = getTrueAnomalyFromAltitude(interpolationParameters.firstOwnAltitude, ownCoordinates.orbit.semiLatusRectum, ownCoordinates.orbit.eccentricity, ownShipHeadedInwards);
	const ownSecondTrueAnomaly = getTrueAnomalyFromAltitude(interpolationParameters.secondOwnAltitude, ownCoordinates.orbit.semiLatusRectum, ownCoordinates.orbit.eccentricity, ownShipHeadedInwards);

	const targetFirstTrueAnomaly = getTrueAnomalyFromAltitude(interpolationParameters.firstTargetAltitude, targetSemiLatusRectum, targetEccentricity, targetHeadedInwards);
	const targetSecondTrueAnomaly = getTrueAnomalyFromAltitude(interpolationParameters.secondTargetAltitude, targetSemiLatusRectum, targetEccentricity, targetHeadedInwards);

	// We can use the true anomalies and such to calculate the distance between the two target points
	let angleDifference = targetSecondTrueAnomaly - targetFirstTrueAnomaly;
	let expectedDistance = Math.sqrt(
		interpolationParameters.firstTargetAltitude**2 
		+ interpolationParameters.secondTargetAltitude**2 
		- 2 * interpolationParameters.firstTargetAltitude * interpolationParameters.secondTargetAltitude * Math.cos(angleDifference)
	);

	const getVectors = (trueAnomaly: number, orbit: Orbit) => {
		const radius = orbit.semiLatusRectum / (1 + orbit.eccentricity * Math.cos(trueAnomaly));
		const localX = radius * Math.cos(trueAnomaly);
		const localY = radius * Math.sin(trueAnomaly);

		const vectorPointingTowardsShip = normalizeVector(
			addVector(
				multiplyMatrixWithScalar(localX, orbit.coordinateAxes[0]),
				multiplyMatrixWithScalar(localY, orbit.coordinateAxes[1])
			)
		);

		const perpendicularVectorInPlane = normalizeVector(
			vectorCrossProduct(orbit.coordinateAxes[2], vectorPointingTowardsShip)
		);

		const perpendicularVectorOutOfPlane = orbit.coordinateAxes[2];

		return [vectorPointingTowardsShip, perpendicularVectorInPlane, perpendicularVectorOutOfPlane];
	}

	const firstVectors = getVectors(ownFirstTrueAnomaly, ownCoordinates.orbit);
	const secondVectors = getVectors(ownSecondTrueAnomaly, ownCoordinates.orbit);

	const getDistances = (ownAltitude: number, targetAltitude: number, targetDistance: number) => {
		const angleBetweenPlanetAndTarget = Math.acos((ownAltitude**2 + targetAltitude**2 - targetDistance**2) / (2*ownAltitude*targetAltitude));
		const distanceAlongDirection = Math.cos(angleBetweenPlanetAndTarget) * targetAltitude;
		const distanceAlongPerpendicular = Math.sin(angleBetweenPlanetAndTarget) * targetAltitude;

		return [distanceAlongDirection, distanceAlongPerpendicular];
	}

	const [firstDistanceAlongDirection, firstDistancePerpendicularToDirection] = getDistances(interpolationParameters.firstOwnAltitude, interpolationParameters.firstTargetAltitude, interpolationParameters.firstDistance);
	const [secondDistanceAlongDirection, secondDistancePerpendicularToDirection] = getDistances(interpolationParameters.secondOwnAltitude, interpolationParameters.secondTargetAltitude, interpolationParameters.secondDistance);

	let cosOfAngleOne = firstDistanceAlongDirection * Math.tan(interpolationParameters.firstPhaseAngle) / firstDistancePerpendicularToDirection;;
	let cosOfAngleTwo = secondDistanceAlongDirection * Math.tan(interpolationParameters.secondPhaseAngle) / secondDistancePerpendicularToDirection;;

	// If either of these angles are outside the allowable values, clip them down
	if (Math.abs(cosOfAngleOne) > 1) {
		cosOfAngleOne /= Math.abs(cosOfAngleOne);
	}

	if (Math.abs(cosOfAngleTwo) > 1) {
		cosOfAngleTwo /= Math.abs(cosOfAngleTwo);
	}

	let results: [OrbitalCoordinates, number][] = [];
	let epochTrueAnomaly = ownCoordinates.trueAnomaly;
	while (epochTrueAnomaly + 2 * Math.PI < ownFirstTrueAnomaly) {
		epochTrueAnomaly += 2 * Math.PI;
	}
	const timeElapsed = getTimeBetweenTrueAnomalies(epochTrueAnomaly, ownSecondTrueAnomaly, ownCoordinates.orbit, body);

	const anglesToDistanceFunction = (angleOne: number, angleTwo: number): number => {
		let positionOne = addVector(multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]),
			addVector(
				multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]),
				multiplyMatrixWithScalar(Math.sin(angleOne) * firstDistancePerpendicularToDirection, firstVectors[2])
			)
		);

		let positionTwo = addVector(multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]),
			addVector(
				multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]),
				multiplyMatrixWithScalar(Math.sin(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[2])
			)
		);

		let distance = getVectorMagnitude(addVector(positionTwo, multiplyMatrixWithScalar(-1, positionOne)));
		return distance - expectedDistance;
	};

	const anglesToDistancePartialDerivativeWrtAngleOne = (angleOne: number, angleTwo: number): number => {
		let positionOne = addVector(multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]),
			addVector(
				multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]),
				multiplyMatrixWithScalar(Math.sin(angleOne) * firstDistancePerpendicularToDirection, firstVectors[2])
			)
		);

		let positionTwo = addVector(multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]),
			addVector(
				multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]),
				multiplyMatrixWithScalar(Math.sin(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[2])
			)
		);

		let difference = addVector(positionTwo, multiplyMatrixWithScalar(-1, positionOne));
		let distance = Math.abs(getVectorMagnitude(difference));

		let positionOneDerivative = addVector(
			multiplyMatrixWithScalar(Math.sin(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]),
			multiplyMatrixWithScalar(-Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[2])
		);

		return vectorDotProduct(difference, positionOneDerivative) / distance;
	}

	const anglesToDistancePartialDerivativeWrtAngleTwo = (angleOne: number, angleTwo: number): number => {
		let positionOne = addVector(multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]),
			addVector(
				multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]),
				multiplyMatrixWithScalar(Math.sin(angleOne) * firstDistancePerpendicularToDirection, firstVectors[2])
			)
		);

		let positionTwo = addVector(multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]),
			addVector(
				multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]),
				multiplyMatrixWithScalar(Math.sin(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[2])
			)
		);

		let difference = addVector(positionTwo, multiplyMatrixWithScalar(-1, positionOne));
		let distance = Math.abs(getVectorMagnitude(difference));

		let positionTwoDerivative = addVector(
			multiplyMatrixWithScalar(-Math.sin(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]),
			multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[2])
		);

		return vectorDotProduct(difference, positionTwoDerivative) / distance;
	};

	let firstExpectedPlaneAngle = (ownFirstTrueAnomaly + interpolationParameters.firstPhaseAngle);
	let secondExpectedPlaneAngle = (ownSecondTrueAnomaly + interpolationParameters.secondPhaseAngle);
	let planeAngleDifference = secondExpectedPlaneAngle - firstExpectedPlaneAngle;
	while (planeAngleDifference <= -Math.PI) {
		planeAngleDifference += 2 * Math.PI;
	}
	while (planeAngleDifference > Math.PI) {
		planeAngleDifference -= 2 * Math.PI;
	}

	const planeAngleFunction = (angleOne: number, angleTwo: number) => {
		const horizontalOne = addVector(
			multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]),
			multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1])
		);

		const horizontalTwo = addVector(
			multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]),
			multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1])
		);

		return vectorDotProduct(horizontalOne, horizontalTwo) / (getVectorMagnitude(horizontalOne) * getVectorMagnitude(horizontalTwo)) - Math.cos(planeAngleDifference);
	}

	const planeAnglePartialDerivativeAngleOne = (angleOne: number, angleTwo: number) => {
		let directionOne = multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]);
		let perpendicularOne = multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]);

		let directionTwo = multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[1]);
		let perpendicularTwo = multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]);

		const horizontalOne = addVector(
			directionOne,
			perpendicularOne
		);

		const horizontalTwo = addVector(
			directionTwo,
			perpendicularTwo
		);

		return (
			-Math.sin(angleOne) * vectorDotProduct(perpendicularOne, directionTwo)
			-Math.sin(angleOne) * Math.cos(angleTwo) * vectorDotProduct(perpendicularOne, perpendicularTwo)
			-Math.cos(angleOne) * Math.sin(angleOne) * vectorDotProduct(perpendicularOne, perpendicularOne) * vectorDotProduct(horizontalOne, horizontalTwo) / getVectorMagnitude(horizontalOne)
		) / (getVectorMagnitude(horizontalOne) * getVectorMagnitude(horizontalTwo));
	}

	const planeAnglePartialDerivativeAngleTwo = (angleOne: number, angleTwo: number) => {
		let directionOne = multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]);
		let perpendicularOne = multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]);

		let directionTwo = multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[1]);
		let perpendicularTwo = multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]);

		const horizontalOne = addVector(
			directionOne,
			perpendicularOne
		);

		const horizontalTwo = addVector(
			directionTwo,
			perpendicularTwo
		);

		return (
			-Math.sin(angleTwo) * vectorDotProduct(perpendicularTwo, directionOne)
			-Math.sin(angleTwo) * Math.cos(angleOne) * vectorDotProduct(perpendicularTwo, perpendicularOne)
			-Math.cos(angleTwo) * Math.sin(angleTwo) * vectorDotProduct(perpendicularTwo, perpendicularOne) * vectorDotProduct(horizontalOne, horizontalTwo) / getVectorMagnitude(horizontalTwo)
		) / (getVectorMagnitude(horizontalOne) * getVectorMagnitude(horizontalTwo))
	}




	let previouslyFoundAngles: [number, number][] = [];

	// Try all four possible arrangements of angles
	[-1, 1].forEach(angleOneMultiplier => {
		[-1, 1].forEach(angleTwoMultiplier => {
			let angleOne = angleOneMultiplier * Math.acos(cosOfAngleOne);
			let angleTwo = angleTwoMultiplier * Math.acos(cosOfAngleTwo);

			// Do some gradient descent to further optimize the angles
			for (var i = 0; i < 10000; i++) {
				// We'll descend one dimension at a time
				let currentDistance = anglesToDistanceFunction(angleOne, angleTwo);
				let dfD1 = anglesToDistancePartialDerivativeWrtAngleOne(angleOne, angleTwo);

				if (!dfD1 || !currentDistance) {
					break;
				}

				// If the distance function is below something like 10 metres, we are close enough
				if (Math.abs(currentDistance) < 10) {
					break;
				}

				let deadEndAngleOne = false;
				let counter = 0;
				let update = -currentDistance / dfD1;
				let learningRate = 2;
				let candidateDistance;
				do {
					counter++;
					if (counter == 32) {
						deadEndAngleOne = true;
						break;
					}

					learningRate *= 0.5;
					candidateDistance = anglesToDistanceFunction(angleOne + learningRate * update, angleTwo);
					if (!candidateDistance) {
						deadEndAngleOne = true;
						break;
					}
				} while (Math.abs(candidateDistance) > Math.abs(currentDistance));
				
				if (!deadEndAngleOne) {
					angleOne = (angleOne + learningRate * update) % (2 * Math.PI);
				}

				currentDistance = anglesToDistanceFunction(angleOne, angleTwo);
				let dfD2 = anglesToDistancePartialDerivativeWrtAngleTwo(angleOne, angleTwo);

				if (!currentDistance || !dfD2) {
					break;
				}

				let deadEndAngleTwo = false;
				counter = 0;
				update = -currentDistance / dfD2;
				learningRate = 2;
				do {
					counter++;
					if (counter == 32) {
						deadEndAngleTwo = true;
						break;
					}

					learningRate *= 0.5;
					candidateDistance = anglesToDistanceFunction(angleOne, angleTwo + learningRate * update);
					if (!candidateDistance) {
						deadEndAngleTwo = true;
						break;
					}
				} while (Math.abs(candidateDistance) > Math.abs(currentDistance));

				if (!deadEndAngleTwo) {
					angleTwo = (angleTwo + learningRate * update) % (2 * Math.PI);
					while (angleTwo < 0) {
						angleTwo += 2 * Math.PI;
					}
				}

				if (deadEndAngleOne && deadEndAngleTwo) {
					break;
				}
			}

			let distanceAway = anglesToDistanceFunction(angleOne, angleTwo);
			if (!distanceAway || Math.abs(distanceAway) > 1000) {
				return;
			}

			// Now that we've found an optimal expected distance between the two positions, try to optimize for the correct phase angle
			// Newton's method in two dimensions, baby!
				const getFunctionVector = (anglesVector: number[][]) => {
					return [
						[anglesToDistanceFunction(anglesVector[0][0], anglesVector[1][0])],
						[planeAngleFunction(anglesVector[0][0], anglesVector[1][0])]
					]
				};

				const getJacobianMatrix = (anglesVector: number[][]) => {
					let angleOne = anglesVector[0][0];
					let angleTwo = anglesVector[1][0];
					return[
						[anglesToDistancePartialDerivativeWrtAngleOne(angleOne, angleTwo), anglesToDistancePartialDerivativeWrtAngleTwo(angleOne, angleTwo)],
						[planeAnglePartialDerivativeAngleOne(angleOne, angleTwo), planeAnglePartialDerivativeAngleTwo(angleOne, angleTwo)]
					]
				};


			const performNewtonMethod = (initialGuess: number[][], iterations: number) => {
				let anglesVector = [
					[initialGuess[0][0]],
					[initialGuess[1][0]]
				];

				for (var i = 0; i < iterations; i++) {
					let functionVector = getFunctionVector(anglesVector);
					let jacobianMatrix = getJacobianMatrix(anglesVector);

					let update = matrixMultiply(invertTwoByTwoMatrix(jacobianMatrix), functionVector);

					// Don't make updates too big
					while (getVectorMagnitude(update) > Math.PI / 100) {
						update = multiplyMatrixWithScalar(0.5, update);
					}
					anglesVector = addVector(anglesVector, multiplyMatrixWithScalar(-1, update));
				};
				return anglesVector;
			}

			[[angleOne], [angleTwo]] = performNewtonMethod([[angleOne + Math.random()*0.00001], [angleTwo + Math.random()*0.00001]], 1000);

			// Check if we have found these angles before
			let foundAnglesBefore = false;
			previouslyFoundAngles.forEach(([otherAngleOne, otherAngleTwo]) => {
				if (Math.abs(otherAngleOne - angleOne) < 0.0001 && Math.abs(otherAngleTwo - angleTwo) < 0.0001) {
					foundAnglesBefore = true;
				}
			});

			if (foundAnglesBefore) {
				return;
			}

			previouslyFoundAngles.push([angleOne, angleTwo]);


			let targetPositionOne = addVector(multiplyMatrixWithScalar(firstDistanceAlongDirection, firstVectors[0]),
				addVector(
					multiplyMatrixWithScalar(Math.cos(angleOne) * firstDistancePerpendicularToDirection, firstVectors[1]),
					multiplyMatrixWithScalar(Math.sin(angleOne) * firstDistancePerpendicularToDirection, firstVectors[2])
				)
			);

			let targetPositionTwo = addVector(multiplyMatrixWithScalar(secondDistanceAlongDirection, secondVectors[0]),
				addVector(
					multiplyMatrixWithScalar(Math.cos(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[1]),
					multiplyMatrixWithScalar(Math.sin(angleTwo) * secondDistancePerpendicularToDirection, secondVectors[2])
				)
			);

			let firstMeasuredHorizontalAnomaly = Math.atan2(vectorDotProduct(targetPositionOne, ownCoordinates.orbit.coordinateAxes[1]), vectorDotProduct(targetPositionOne, ownCoordinates.orbit.coordinateAxes[0]));
			let secondMeasuredHorizontalAnomaly = Math.atan2(vectorDotProduct(targetPositionTwo, ownCoordinates.orbit.coordinateAxes[1]), vectorDotProduct(targetPositionTwo, ownCoordinates.orbit.coordinateAxes[0]));
			let measuredHorizontalAnomalyChange = secondMeasuredHorizontalAnomaly - firstMeasuredHorizontalAnomaly;
			while (measuredHorizontalAnomalyChange <= -Math.PI) {
				measuredHorizontalAnomalyChange += 2 * Math.PI;
			}
			while (measuredHorizontalAnomalyChange > Math.PI) {
				measuredHorizontalAnomalyChange -= 2 * Math.PI;
			}

			// These need to change in the same way, or the orbit we've found is going in the wrong direction
			if (Math.sign(planeAngleDifference) != Math.sign(measuredHorizontalAnomalyChange)) {
				return;
			}

			let normalVector = normalizeVector(vectorCrossProduct(targetPositionOne, targetPositionTwo));
			
			// Rotate the position vector about this normal vector to get the direction of the periapsis
			let ux = normalVector[0][0];
			let uy = normalVector[1][0];
			let uz = normalVector[2][0];

			let rotationMatrix = [
				[
					ux*ux*(1 - Math.cos(-targetFirstTrueAnomaly)) + Math.cos(-targetFirstTrueAnomaly),
					ux*uy*(1 - Math.cos(-targetFirstTrueAnomaly)) - uz * Math.sin(-targetFirstTrueAnomaly),
					ux*uz*(1 - Math.cos(-targetFirstTrueAnomaly)) + uy * Math.sin(-targetFirstTrueAnomaly)
				],
				[
					ux*ux*(1 - Math.cos(-targetFirstTrueAnomaly)) + uz * Math.sin(-targetFirstTrueAnomaly),
					uy*uy*(1 - Math.cos(-targetFirstTrueAnomaly)) + Math.cos(-targetFirstTrueAnomaly),
					uy*uz*(1 - Math.cos(-targetFirstTrueAnomaly)) - ux * Math.sin(-targetFirstTrueAnomaly)
				],
				[
					ux*uz*(1 - Math.cos(-targetFirstTrueAnomaly)) - uy * Math.sin(-targetFirstTrueAnomaly),
					uy*uz*(1 - Math.cos(-targetFirstTrueAnomaly)) + ux * Math.sin(-targetFirstTrueAnomaly),
					uz*uz*(1 - Math.cos(-targetFirstTrueAnomaly)) + Math.cos(-targetFirstTrueAnomaly)
				]
			];

			let periapsisVector = normalizeVector(matrixMultiply(rotationMatrix, targetPositionOne));
			let targetOrbit: Orbit = {
				semiLatusRectum: targetSemiLatusRectum,
				eccentricity: targetEccentricity,
				coordinateAxes: [
					periapsisVector,
					normalizeVector(vectorCrossProduct(normalVector, periapsisVector)),
					normalVector
				]
			};

			let targetCoordinates: OrbitalCoordinates = getOrbitalCoordinatesFromAltitude(
				interpolationParameters.secondTargetAltitude,
				targetHeadedInwards,
				targetOrbit,
				body
			);

			results.push([targetCoordinates, timeElapsed]);
		});
	});

	return results;
}