import type { Body } from "./constants";
import { addVector, getVectorMagnitude, 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,
	position: number[][]
}

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

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

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) {
			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,
			firstManoeuvre: startManoeuvre,
			secondManoeuvre: goalManoeuvre,
			closestPointDistance: closestPoint,
			farthestPointDistance: farthestPoint
		}
	}
}

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 getOrbitalCoordinates(timeToPeriapsis: number, orbit: Orbit, planet: Body) {
	const meanAnomaly = -Math.sqrt(planet.gravitationalParameter / Math.abs(orbit.semiLatusRectum / (orbit.eccentricity**2 - 1))**3) * timeToPeriapsis;
	var eccentricAnomaly;
	var trueAnomaly;

	if (Math.abs(orbit.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 (orbit.eccentricity < 1) {
			keplerEquation = (guess: number) => guess - orbit.eccentricity * Math.sin(guess) - meanAnomaly;
			keplerEquationDerivative = (guess: number) => 1 - orbit.eccentricity * Math.cos(guess);
		} else {
			keplerEquation = (guess: number) => orbit.eccentricity * Math.sinh(guess) - guess - meanAnomaly;
			keplerEquationDerivative = (guess: number) => orbit.eccentricity * Math.cosh(guess) - 1;
		}

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

		if (orbit.eccentricity < 1) {
			trueAnomaly = 2*Math.atan(Math.sqrt((1 + orbit.eccentricity) / (1 - orbit.eccentricity)) * Math.tan(eccentricAnomaly / 2));
		} else {
			trueAnomaly = 2*Math.atan(Math.sqrt((orbit.eccentricity + 1) / (orbit.eccentricity - 1)) * Math.tanh(eccentricAnomaly / 2));
		}
	}

	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,
		position: globalPosition,
	}
}

export function getTimeBetweenTrueAnomalies(startingTrueAnomaly: number, endingTrueAnomaly: number, orbit: Orbit, planet: Body): number {
	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;
			}
		} 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;
	}
}

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 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 function findCheapestTransfer(startingSituation: OrbitalCoordinates, targetOrbit: Orbit, body: Body, progressCallback?: (anomaliesChecked: number, totalAnomalies: number, currentBestDeltaV: number | null, currentBestTransfer: Transfer | null) => void): 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)));
		for (var i = 0; i < 100; i++) {
			let step = (finalAnomaly - startingSituation.trueAnomaly) / 100;
			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;
}