/*
 *      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 "flight.h"


airplaneT _plane;


#define SEGMENT_MATH


#define PI  3.1415926

#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){
	return atan(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));
}


float sinD(float d) {
	return sin(Rads(d));
}


#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 lightPlane(void) {
	// Flight model.  Power Dynamics.
	if (_plane.ignition) {
		// Start engine.
		if (!_plane.engine) _plane.engine = true;
		// Adjust RPM.
		if (_plane.rpm < (375 + (_plane.throttle * 117))) _plane.rpm += _plane.loopTime * 0.5;
		if (_plane.rpm > (375 + (_plane.throttle * 117))) _plane.rpm -= _plane.loopTime * 0.5;
	} else {
		// Stop engine.
		if (_plane.engine) _plane.engine = false;
		// Run down engine.
		if (_plane.rpm > 0) _plane.rpm -= (int16_t)(_plane.loopTime / 2);
		if (_plane.rpm < 0) _plane.rpm = 0;
	}

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


#define SEGMENT_FLIGHT_MODEL_2


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

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

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


#define SEGMENT_FLIGHT_MODEL_3


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

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

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

	// Are we flying?
	if ((!_plane.airborne) && (_plane.y > PLANE_HEIGHT)) _plane.airborne = true;
}


void resetAircraft(void) {
	// Initialize airplane.
	memset(&_plane, 0, sizeof(airplaneT));

	_plane.brake    = true;
	/*
	_plane.rpm = 0;
	_plane.hSpeed = 0;
	_plane.vSpeed = 0;
	_plane.deltaZ = 0;
	_plane.ignition = false;
	_plane.engine = false;
	_plane.efAOF  = 0;
	_plane.x = 0;
	_plane.y = 0;
	_plane.z = 0;
	_plane.pitch = 0;
	_plane.roll = 0;
	_plane.yaw = 0;
	_plane.airborne = false;
	*/

	_plane.loopTime = 40;
}


#define SEGMENT_MAIN


void updateAircraft(void) {
	// Do the actual flying!
	lightPlane();
	lightPlane2();
	moveAircraft();
}