/*
 *      Copyright (c) 2024 Scott Duensing, scott@kangaroopunch.com
 *
 *      Permission is hereby granted, free of charge, to any person obtaining a copy
 *      of this software and associated documentation files (the "Software"), to deal
 *      in the Software without restriction, including without limitation the rights
 *      to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *      copies of the Software, and to permit persons to whom the Software is
 *      furnished to do so, subject to the following conditions:
 *
 *      The above copyright notice and this permission notice shall be included in
 *      all copies or substantial portions of the Software.
 *
 *      THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *      IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *      FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *      AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *      LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *      OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 *      SOFTWARE.
 */


#include "fflight.h"
#include "a23d2.h"


fAirplaneT _fPlane;


#define SEGMENT_MATH


#define PI  3.1415

#define Rads(d)  (((d) < 0 ? (d) + 360 : (d)) * (PI / 180))
#define Degs(r)  ((r) * (180 / PI))

// https://www.reddit.com/r/programming/comments/3e7ghi/discrete_arctan_in_6502
#define ATAN_SPLINE_C0 (double)(-0.14380550980765507115)  // 3pi/4 - 5/2
#define ATAN_SPLINE_C1 (double)(-0.07079632679489661923)  // 3/2 - pi/2
double ourAtan(double x){
	if (x >= 0) {
		if ( x<= 1) {
			return x + (ATAN_SPLINE_C0 + ATAN_SPLINE_C1 * x) * x * x;
		} else {
			x = 1 / x;
			return PI / 2 - (x + (ATAN_SPLINE_C0 + ATAN_SPLINE_C1 * x) * x * x);
		}
	} else {
		if (x >= -1) {
			return x - (ATAN_SPLINE_C0 - ATAN_SPLINE_C1 * x) * x * x;
		} else {
			x = -1 / x;
			return (x + (ATAN_SPLINE_C0 + ATAN_SPLINE_C1 * x) * x * x) - PI / 2;
		}
	}
}


float cosD(float d) {
	return cos(Rads(d));
	/*
	// Map 0-359 into 0-255.
	_tdata = (int)d % 359;
	a23d2Cos();
	return _trig;
	*/
}


float sinD(float d) {
	return sin(Rads(d));
	/*
	// Map 0-359 into 0-255.
	_tdata = (int)d % 359;
	a23d2Sin();
	return _trig;
	*/
}


#define SEGMENT_FLIGHT_MODEL_1


// This flight model is basically the one from "Build Your Own Flight Sim
// in C++" by Michael Radtke and Chris Lampton.
void fLightPlane(void) {
	// Flight model.  Power Dynamics.
	if (_fPlane.ignition) {
		// Start engine.
		if (!_fPlane.engine) _fPlane.engine = true;
		// Adjust RPM.
		if (_fPlane.rpm < (375 + (_fPlane.throttle * 117))) _fPlane.rpm += _fPlane.loopTime * 0.5;
		if (_fPlane.rpm > (375 + (_fPlane.throttle * 117))) _fPlane.rpm -= _fPlane.loopTime * 0.5;
	} else {
		// Stop engine.
		if (_fPlane.engine) _fPlane.engine = false;
		// Run down engine.
		if (_fPlane.rpm > 0) _fPlane.rpm -= (int16_t)(_fPlane.loopTime / 2);
		if (_fPlane.rpm < 0) _fPlane.rpm = 0;
	}

	// Flight model.  Flight Dynamics.
	// Calculate speed from RPM.
	_fPlane.iSpeed = _fPlane.rpm / 17.5;
	// Modify speed by pitch.
	_fPlane.iSpeed += (_fPlane.pitch * 1.5);
	// Horizontal acceleration - thrust.
	_fPlane.hAccel = ((_fPlane.rpm * (_fPlane.iSpeed - _fPlane.hSpeed)) / 10000);
	_fPlane.hAccel /= 1000;
	_fPlane.hAccel *= _fPlane.loopTime;
	if (_fPlane.brake && !_fPlane.airborne) {
		// Handle brakes.
		if (_fPlane.hSpeed > 0) {
			_fPlane.hSpeed -= 1;
		} else {
			_fPlane.hSpeed = 0;
		}
	} else {
		// Accelerate normally.
		_fPlane.hSpeed += _fPlane.hAccel;
	}
	// Force speed to range -1..1.
	_fPlane.lSpeed = (_fPlane.hSpeed / 65) - 1;
	if (_fPlane.lSpeed > 1) _fPlane.lSpeed = 1;
	// Lift curve.
	_fPlane.lVeloc = Degs(ourAtan(_fPlane.lSpeed));
	// Force lift to range 0..90.
	_fPlane.lVeloc += 45;
	// Shift range to 0..-17.
	_fPlane.lVeloc /= 5.29;
	// Multiply by pitch modifier.
	_fPlane.lVeloc *= (-(_fPlane.pitch * 0.157) + 1);
	// Time slice.
	_fPlane.lVeloc /= 1000;
	_fPlane.lVeloc *= _fPlane.loopTime;
	// Gravity.
	_fPlane.gVeloc = _fPlane.loopTime * (-16.0 / 1000);  // -16.0 is ft/sec for gravity.
	// Sum vertical velocity.
	_fPlane.vSpeed = _fPlane.gVeloc + _fPlane.lVeloc;
	// No vertical speed if we're on the ground.
	if ((!_fPlane.airborne) && (_fPlane.vSpeed < 0)) _fPlane.vSpeed = 0;
	// Save climb rate in ft/min.
	_fPlane.climbRate = _fPlane.vSpeed / _fPlane.loopTime;
	_fPlane.climbRate *= 60000L;
	// Expand to ft/hr.
	_fPlane.deltaZ = _fPlane.hSpeed * 5280;
	// Get ft/ms.
	_fPlane.deltaZ /= 3600000L;
	_fPlane.deltaZ *= _fPlane.loopTime;
	// Find effective angle of flight.
	if (_fPlane.deltaZ) {
		_fPlane.efAOF = -(ourAtan(_fPlane.vSpeed / _fPlane.deltaZ));
	} else {
		_fPlane.efAOF = -(ourAtan(_fPlane.vSpeed));
	}
	// Convert to degrees.
	_fPlane.AOA = Degs(_fPlane.efAOF);
	// Handle stalling.
	if (((_fPlane.pitch < _fPlane.AOA) && (_fPlane.AOA < 0)) && (_fPlane.hSpeed < 40)) {
		if ((_fPlane.pitch - _fPlane.AOA) < -20) _fPlane.stall = true;
	}
	if (_fPlane.stall) {
		if (_fPlane.pitch > 30) {
			_fPlane.stall = false;
		} else {
			_fPlane.pitch++;
		}
	}
}


#define SEGMENT_FLIGHT_MODEL_2


void fLightPlane2(void) {
	// Flight model.  Inertial Damping.
	if (_fPlane.dPitch) {
		_fPlane.dPitch -= _fPlane.dPitch / 10;
		if (((_fPlane.dPitch > 0) && (_fPlane.dPitch < 0.01)) || ((_fPlane.dPitch < 0) && (_fPlane.dPitch > -0.01))) _fPlane.dPitch = 0;
	}
	if (_fPlane.dYaw) {
		_fPlane.dYaw -= _fPlane.dYaw / 10;
		if (((_fPlane.dYaw > 0) && (_fPlane.dYaw < 0.01)) || ((_fPlane.dYaw < 0) && (_fPlane.dYaw > -0.01))) _fPlane.dYaw = 0;
	}
	if (_fPlane.dRoll) {
		_fPlane.dRoll -= _fPlane.dRoll / 10;
		if (((_fPlane.dRoll > 0) && (_fPlane.dRoll < 0.01)) || ((_fPlane.dRoll < 0) && (_fPlane.dRoll > -0.01))) _fPlane.dRoll = 0;
	}

	// Flight model.  Rate of Change.
	if (_fPlane.airborne) {
		if (_fPlane.aileron != 0) {
			_fPlane.torque = ((_fPlane.hSpeed * _fPlane.aileron) / 10000);
			if (_fPlane.dRoll != (_fPlane.torque * _fPlane.loopTime)) _fPlane.dRoll += _fPlane.torque * 6;
		}
	}
	if (_fPlane.elevator != 0) {
		_fPlane.torque = ((_fPlane.hSpeed * _fPlane.elevator) / 10000);
		if ((!_fPlane.airborne) && (_fPlane.torque > 0)) _fPlane.torque = 0;  //***FIX*** This is dumb.
		if (_fPlane.dPitch != (_fPlane.torque * _fPlane.loopTime)) _fPlane.dPitch += _fPlane.torque * 1.5;
	}
	if (_fPlane.hSpeed) {
		_fPlane.torque = 0.0;
		if (_fPlane.rudder != 0) _fPlane.torque = -((_fPlane.hSpeed * _fPlane.rudder) / 10000);
		_fPlane.torque2 = 0.0;
		if ((_fPlane.roll > 0) && (_fPlane.roll <= 90)) {  //***FIX*** This is dumb.
			_fPlane.torque2 = _fPlane.roll * 0.00050;
		} else {
			if ((_fPlane.roll < 0) && (_fPlane.roll >= -90)) {
				_fPlane.torque2 = _fPlane.roll * 0.00050;
			}
		}
		_fPlane.torque += _fPlane.torque2;
		if (_fPlane.dYaw != (_fPlane.torque * _fPlane.loopTime)) _fPlane.dYaw += _fPlane.torque * 1.5;
	}

	// Flight model.  Apply Rotations.
	// Transform pitch into components of yaw and pitch based on roll.
	_fPlane.roll  += _fPlane.dRoll;
	_fPlane.yaw   += _fPlane.dYaw;
	_fPlane.pitch += (_fPlane.dPitch * cosD(_fPlane.roll));
	_fPlane.yaw   += -(_fPlane.dPitch * sinD(_fPlane.roll));
	if (_fPlane.roll > 180) {
		_fPlane.roll = -180 + (_fPlane.roll - 180);
	} else {
		if (_fPlane.roll < -180) _fPlane.roll = 180 + (_fPlane.roll + 180);
	}
	if (_fPlane.yaw > 180) {
		_fPlane.yaw = -180 + (_fPlane.yaw - 180);
	} else {
		if (_fPlane.yaw < -180) _fPlane.yaw = 180 + (_fPlane.yaw + 180);
	}
	// Handle special case when aircraft pitch passes vertical.
	if ((_fPlane.pitch > 90) || (_fPlane.pitch < -90)) {
		if (_fPlane.roll >= 0) {
			_fPlane.roll -= 180;
		} else {
			if (_fPlane.roll < 0) _fPlane.roll += 180;
		}
		if (_fPlane.yaw >= 0) {
			_fPlane.yaw -= 180;
		} else {
			if (_fPlane.yaw < 0) _fPlane.yaw += 180;
		}
		if (_fPlane.pitch > 0) {
			_fPlane.pitch = (180 - _fPlane.pitch);
		} else {
			if (_fPlane.pitch < 0) _fPlane.pitch = (-180 - _fPlane.pitch);
		}
	}
	// Dampen everything out to 0 if they get close enough.
	if ((_fPlane.pitch > -0.5) && (_fPlane.pitch < 0.5)) _fPlane.pitch = 0;
	if ((_fPlane.roll  > -0.5) && (_fPlane.roll  < 0.5)) _fPlane.roll  = 0;
	if ((_fPlane.yaw   > -0.5) && (_fPlane.yaw   < 0.5)) _fPlane.yaw   = 0;
}


#define SEGMENT_FLIGHT_MODEL_3


void fMoveAircraft(void) {
	// Calculate new aircraft position.  Each coordinate is 1 foot in 3D space.
	_fPlane.tmpX = 0;
	_fPlane.tmpY = 0;
	_fPlane.tmpZ = _fPlane.deltaZ;

	// Order of these points is significant.
	// Rotate in X.
	_fPlane.newY = -(_fPlane.tmpZ * sinD(_fPlane.AOA));
	_fPlane.newZ = (_fPlane.tmpZ * cosD(_fPlane.AOA));
	_fPlane.tmpY = _fPlane.newY;
	_fPlane.tmpZ = _fPlane.newZ;
	// Rotate in Y.
	_fPlane.newX = (_fPlane.tmpZ * sinD(_fPlane.yaw));
	_fPlane.newZ = (_fPlane.tmpZ * cosD(_fPlane.yaw));

	// Translate rotated point back to where it should be
	// relative to it's last position.
	_fPlane.collectX += _fPlane.newX;
	if ((_fPlane.collectX > 1) || (_fPlane.collectX < -1)) {
		_fPlane.x -= _fPlane.collectX;
		_fPlane.collectX = 0;
	}
	_fPlane.collectY += _fPlane.newY;
	if ((_fPlane.collectY > 1) || (_fPlane.collectY < -1)) {
		_fPlane.y -= _fPlane.collectY;
		_fPlane.collectY = 0;
	}
	_fPlane.collectZ += _fPlane.newZ;
	if ((_fPlane.collectZ > 1) || (_fPlane.collectZ < -1)) {
		_fPlane.z += _fPlane.collectZ;
		_fPlane.collectZ = 0;
	}

	// Are we flying?
	if ((!_fPlane.airborne) && (_fPlane.y != 0)) _fPlane.airborne = true;
}


void fResetAircraft(void) {
	// Initialize airplane.
	memset(&_fPlane, 0, sizeof(fAirplaneT));

	_fPlane.brake    = true;
	_fPlane.loopTime = 40;
}


#define SEGMENT_MAIN


void fUpdateAircraft(void) {
	// Do the actual flying!
	fLightPlane();
	fLightPlane2();
	fMoveAircraft();
}