/*
 *      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 "a23d2.h"
#include "mem.h"

#include "flight.h"


// Extern.
airplaneT _plane;


#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;
	_plane.FiSpeed = fixDiv(iToFix(_plane.rpm), fToFix(17.5));
	// Modify speed by pitch.
	//_plane.iSpeed += (_plane.pitch * 1.5);
	_plane.FiSpeed = fixAdd(_plane.FiSpeed, fixMul(_plane.Fpitch, fToFix(1.5)));
	// Horizontal acceleration - thrust.
	//_plane.hAccel = ((_plane.rpm * (_plane.iSpeed - _plane.hSpeed)) / 10000);
	//_plane.hAccel /= 1000;
	//_plane.hAccel *= _plane.loopTime;
	_plane.FtmpX   = fixSub(_plane.FiSpeed, _plane.FhSpeed);
	_plane.FtmpX   = fixMul(iToFix(_plane.rpm), _plane.FtmpX);
	_plane.FhAccel = fixDiv(_plane.FtmpX, iToFix(10000));
	_plane.FhAccel = fixDiv(_plane.FhAccel, iToFix(1000));
	_plane.FhAccel = fixMul(_plane.FhAccel, iToFix(_plane.loopTime));
	if (_plane.brake && !_plane.airborne) {
		// Handle brakes.
		if (_plane.FhSpeed > 0) {
			//_plane.hSpeed -= 1;
			_plane.FhSpeed = fixSub(_plane.FhSpeed, iToFix(1));
		} else {
			//_plane.hSpeed = 0;
			_plane.FhSpeed = iToFix(0);
		}
	} else {
		// Accelerate normally.
		//_plane.hSpeed += _plane.hAccel;
		_plane.FhSpeed = fixAdd(_plane.FhSpeed, _plane.FhAccel);
	}
	// Force speed to range -1..1.
	//_plane.lSpeed = (_plane.hSpeed / 65) - 1;
	_plane.FlSpeed = fixSub(fixDiv(_plane.FhSpeed, iToFix(65)), iToFix(1));
	//if (_plane.lSpeed > 1) _plane.lSpeed = 1;
	if (_plane.FlSpeed > iToFix(1)) _plane.FlSpeed = iToFix(1);
	// Lift curve.
	//_plane.lVeloc = Degs(ourAtan(_plane.lSpeed));
	_plane.FlVeloc = fixDegs(fixAtan(_plane.FlSpeed));
	// Force lift to range 0..90.
	//_plane.lVeloc += 45;
	_plane.FlVeloc = fixAdd(_plane.FlVeloc, iToFix(45));
	// Shift range to 0..-17.
	//_plane.lVeloc /= 5.29;
	_plane.FlVeloc = fixDiv(_plane.FlVeloc, fToFix(5.29));
	// Multiply by pitch modifier.
	//_plane.lVeloc *= (-(_plane.pitch * 0.157) + 1);
	_plane.FlVeloc = fixAdd(_plane.FlVeloc, fixAdd(-(fixMul(_plane.Fpitch, fToFix(0.157))), iToFix(1)));
	// Time slice.
	//_plane.lVeloc /= 1000;
	//_plane.lVeloc *= _plane.loopTime;
	_plane.FlVeloc = fixDiv(_plane.FlVeloc, iToFix(1000));
	_plane.FlVeloc = fixMul(_plane.FlVeloc, iToFix(_plane.loopTime));
	// Gravity.
	//_plane.gVeloc = _plane.loopTime * (-16.0 / 1000);  // -16.0 is ft/sec for gravity.
	_plane.FgVeloc = fixMul(iToFix(_plane.loopTime), fToFix(-0.016));  // -16.0 is ft/sec for gravity.
	// Sum vertical velocity.
	//_plane.vSpeed = _plane.gVeloc + _plane.lVeloc;
	_plane.FvSpeed = fixAdd(_plane.FgVeloc, _plane.FlVeloc);
	// No vertical speed if we're on the ground.
	if ((!_plane.airborne) && (_plane.FvSpeed < 0)) _plane.FvSpeed = 0;
	// Save climb rate in ft/min.
	//_plane.climbRate = _plane.vSpeed / _plane.loopTime;
	//_plane.climbRate *= 60000L;
	_plane.FclimbRate = fixDiv(_plane.FvSpeed, iToFix(_plane.loopTime));
	_plane.FclimbRate = fixMul(_plane.FclimbRate, iToFix(60000L));  //***TODO*** This long could be an issue.
	// Expand to ft/hr.
	//_plane.deltaZ = _plane.hSpeed * 5280;
	_plane.FdeltaZ = fixMul(_plane.FhSpeed, iToFix(5280));
	// Get ft/ms.
	//_plane.deltaZ /= 3600000L;
	//_plane.deltaZ *= _plane.loopTime;
	_plane.FdeltaZ = fixDiv(_plane.FdeltaZ, iToFix(3600000L));  //***TODO*** This long could be an issue.
	_plane.FdeltaZ = fixMul(_plane.FdeltaZ, iToFix(_plane.loopTime));
	// Find effective angle of flight.
	if (_plane.FdeltaZ) {
		//_plane.efAOF = -(ourAtan(_plane.vSpeed / _plane.deltaZ));
		_plane.FefAOF = -(fixAtan(fixDiv(_plane.FvSpeed, _plane.FdeltaZ)));
	} else {
		//_plane.efAOF = -(ourAtan(_plane.vSpeed));
		_plane.FefAOF = -(fixAtan(_plane.FvSpeed));
	}
	// Convert to degrees.
	//_plane.AOA = Degs(_plane.efAOF);
	_plane.FAOA = fixDegs(_plane.FefAOF);
	// Handle stalling.
	//if (((_plane.pitch < _plane.AOA) && (_plane.AOA < 0)) && (_plane.hSpeed < 40)) {
	if (((_plane.Fpitch < _plane.FAOA) && (_plane.FAOA < iToFix(0))) && (_plane.FhSpeed < iToFix(40))) {
		//if ((_plane.pitch - _plane.AOA) < -20) _plane.stall = true;
		if (fixSub(_plane.Fpitch, _plane.FAOA) < iToFix(-20)) _plane.stall = true;
	}
	if (_plane.stall) {
		//if (_plane.pitch > 30) {
		if (_plane.Fpitch > iToFix(30)) {
			_plane.stall = false;
		} else {
			//_plane.pitch++;
			_plane.Fpitch = fixAdd(_plane.Fpitch, iToFix(1));
		}
	}
}


#define SEGMENT_FLIGHT_MODEL_2


void lightPlane2(void) {
	// Flight model.  Inertial Damping.
	if (_plane.FdPitch != iToFix(0)) {
		//_plane.dPitch -= _plane.dPitch / 10;
		_plane.FdPitch = fixSub(_plane.FdPitch, fixDiv(_plane.FdPitch, iToFix(10)));
		//if (((_plane.dPitch > 0) && (_plane.dPitch < 0.01)) || ((_plane.dPitch < 0) && (_plane.dPitch > -0.01))) _plane.dPitch = 0;
		if (((_plane.FdPitch > iToFix(0)) && (_plane.FdPitch < fToFix(0.01))) || ((_plane.FdPitch < iToFix(0)) && (_plane.FdPitch > fToFix(-0.01)))) _plane.FdPitch = iToFix(0);
	}
	if (_plane.FdYaw != iToFix(0)) {
		//_plane.dYaw -= _plane.dYaw / 10;
		_plane.FdYaw = fixSub(_plane.FdYaw, fixDiv(_plane.FdYaw, iToFix(10)));
		//if (((_plane.dYaw > 0) && (_plane.dYaw < 0.01)) || ((_plane.dYaw < 0) && (_plane.dYaw > -0.01))) _plane.dYaw = 0;
		if (((_plane.FdYaw > iToFix(0)) && (_plane.FdYaw < fToFix(0.01))) || ((_plane.FdYaw < iToFix(0)) && (_plane.FdYaw > fToFix(-0.01)))) _plane.FdYaw = iToFix(0);
	}
	if (_plane.FdRoll != iToFix(0)) {
		//_plane.dRoll -= _plane.dRoll / 10;
		_plane.FdRoll = fixSub(_plane.FdRoll, fixDiv(_plane.FdRoll, iToFix(10)));
		//if (((_plane.dRoll > 0) && (_plane.dRoll < 0.01)) || ((_plane.dRoll < 0) && (_plane.dRoll > -0.01))) _plane.dRoll = 0;
		if (((_plane.FdRoll > iToFix(0)) && (_plane.FdRoll < fToFix(0.01))) || ((_plane.FdRoll < iToFix(0)) && (_plane.FdRoll > fToFix(-0.01)))) _plane.FdRoll = iToFix(0);
	}

	// Flight model.  Rate of Change.
	if (_plane.airborne) {
		if (_plane.aileron != 0) {
			//_plane.torque = ((_plane.hSpeed * _plane.aileron) / 10000);
			_plane.Ftorque = fixDiv(fixAdd(_plane.FhSpeed, iToFix(_plane.aileron)), iToFix(10000));
			//if (_plane.dRoll != (_plane.torque * _plane.loopTime)) _plane.dRoll += _plane.torque * 6;
			if (_plane.FdRoll != fixMul(_plane.Ftorque, iToFix(_plane.loopTime))) _plane.FdRoll = fixAdd(_plane.FdRoll, fixMul(_plane.Ftorque, iToFix(6)));
		}
	}
	if (_plane.elevator != 0) {
		//_plane.torque = ((_plane.hSpeed * _plane.elevator) / 10000);
		_plane.Ftorque = fixDiv(fixAdd(_plane.FhSpeed, iToFix(_plane.elevator)), iToFix(10000));
		//if ((!_plane.airborne) && (_plane.torque > 0)) _plane.torque = 0;  //***FIX*** This is dumb.
		if ((!_plane.airborne) && (_plane.Ftorque > iToFix(0))) _plane.Ftorque = iToFix(0);  //***FIX*** This is dumb.
		//if (_plane.dPitch != (_plane.torque * _plane.loopTime)) _plane.dPitch += _plane.torque * 1.5;
		if (_plane.FdPitch != fixMul(_plane.Ftorque, iToFix(_plane.loopTime))) _plane.FdPitch = fixAdd(_plane.FdRoll, fixMul(_plane.Ftorque, fToFix(1.5)));
	}
	if (_plane.FhSpeed != iToFix(0)) {
		_plane.Ftorque = iToFix(0);
		//if (_plane.rudder != 0) _plane.torque = -((_plane.hSpeed * _plane.rudder) / 10000);
		if (_plane.rudder != 0) _plane.Ftorque = -(fixDiv(fixMul(_plane.FhSpeed, iToFix(_plane.rudder)), iToFix(10000)));
		_plane.Ftorque2 = iToFix(0);
		if ((_plane.Froll > iToFix(0)) && (_plane.Froll <= iToFix(90))) {  //***FIX*** This is dumb.
			//_plane.torque2 = _plane.roll * 0.00050;
			_plane.Ftorque2 = fixMul(_plane.Froll, fToFix(0.00050));
		} else {
			if ((_plane.Froll < iToFix(0)) && (_plane.Froll >= iToFix(-90))) {
				//_plane.torque2 = _plane.roll * 0.00050;
				_plane.Ftorque2 = fixMul(_plane.Froll, fToFix(0.00050));
			}
		}
		//_plane.torque += _plane.torque2;
		_plane.Ftorque = fixAdd(_plane.Ftorque, _plane.Ftorque2);
		//if (_plane.dYaw != (_plane.torque * _plane.loopTime)) _plane.dYaw += _plane.torque * 1.5;
		if (_plane.FdYaw != fixMul(_plane.Ftorque, iToFix(_plane.loopTime))) _plane.FdYaw = fixAdd(_plane.FdYaw, fixMul(_plane.Ftorque, fToFix(1.5)));
	}

	// Flight model.  Apply Rotations.
	// Transform pitch into components of yaw and pitch based on roll.
	_plane.Froll  = fixAdd(_plane.Froll, _plane.FdRoll);
	_plane.Fyaw   = fixAdd(_plane.Fyaw, _plane.FdYaw);
	//_plane.pitch += (_plane.dPitch * cosD(_plane.roll));
	_plane.Fpitch = fixAdd(_plane.Fpitch, fixMul(_plane.FdPitch, fixCosD(_plane.Froll)));
	//_plane.yaw   += -(_plane.dPitch * sinD(_plane.roll));
	_plane.Fyaw   = fixAdd(_plane.Fyaw, -fixMul(_plane.FdPitch, fixSinD(_plane.Froll)));
	if (_plane.Froll > iToFix(180)) {
		//_plane.roll = -180 + (_plane.roll - 180);
		_plane.Froll = fixAdd(iToFix(-180), fixSub(_plane.Froll, iToFix(180)));
	} else {
		//if (_plane.roll < -180) _plane.roll = 180 + (_plane.roll + 180);
		if (_plane.Froll < iToFix(-180)) _plane.Froll = fixAdd(iToFix(180), fixAdd(_plane.Froll, iToFix(180)));
	}
	if (_plane.Fyaw > iToFix(180)) {
		//_plane.yaw = -180 + (_plane.yaw - 180);
		_plane.Fyaw = fixAdd(iToFix(-180), fixSub(_plane.Fyaw, iToFix(180)));
	} else {
		//if (_plane.yaw < -180) _plane.yaw = 180 + (_plane.yaw + 180);
		if (_plane.Fyaw < iToFix(-180)) _plane.Fyaw = fixAdd(iToFix(180), fixAdd(_plane.Fyaw, iToFix(180)));
	}
	// Handle special case when aircraft pitch passes vertical.
	if ((_plane.Fpitch > iToFix(90)) || (_plane.Fpitch < iToFix(-90))) {
		if (_plane.Froll >= iToFix(0)) {
			_plane.Froll = fixSub(_plane.Froll, iToFix(180));
		} else {
			if (_plane.Froll < iToFix(0)) _plane.Froll = fixAdd(_plane.Froll, iToFix(180));
		}
		if (_plane.Fyaw >= iToFix(0)) {
			_plane.Fyaw = fixSub(_plane.Fyaw, iToFix(180));
		} else {
			if (_plane.Fyaw < iToFix(0)) _plane.Fyaw = fixAdd(_plane.Fyaw, iToFix(180));
		}
		if (_plane.Fpitch > iToFix(0)) {
			_plane.Fpitch = fixSub(iToFix(180), _plane.Fpitch);
		} else {
			if (_plane.Fpitch < iToFix(0)) _plane.Fpitch = fixSub(iToFix(-180), _plane.Fpitch);
		}
	}
	// Dampen everything out to 0 if they get close enough.
	if ((_plane.Fpitch > fToFix(-0.5)) && (_plane.Fpitch < fToFix(0.5))) _plane.Fpitch = iToFix(0);
	if ((_plane.Froll  > fToFix(-0.5)) && (_plane.Froll  < fToFix(0.5))) _plane.Froll  = iToFix(0);
	if ((_plane.Fyaw   > fToFix(-0.5)) && (_plane.Fyaw   < fToFix(0.5))) _plane.Fyaw   = iToFix(0);
}


#define SEGMENT_FLIGHT_MODEL_3


void moveAircraft(void) {
	// Calculate new aircraft position.  Each coordinate is 1 foot in 3D space.
	_plane.FtmpX = iToFix(0);
	_plane.FtmpY = iToFix(0);
	_plane.FtmpZ = _plane.FdeltaZ;

	// Order of these points is significant.
	// Rotate in X.
	//_plane.newY = -(_plane.tmpZ * sinD(_plane.AOA));
	_plane.FnewY = -fixMul(_plane.FtmpZ, fixSinD(_plane.FAOA));
	//_plane.newZ = (_plane.tmpZ * cosD(_plane.AOA));
	_plane.FnewZ = fixMul(_plane.FtmpZ, fixCosD(_plane.FAOA));
	_plane.FtmpY = _plane.FnewY;
	_plane.FtmpZ = _plane.FnewZ;
	// Rotate in Y.
	//_plane.newX = (_plane.tmpZ * sinD(_plane.yaw));
	_plane.FnewX = fixMul(_plane.FtmpZ, fixSinD(_plane.Fyaw));
	//_plane.newZ = (_plane.tmpZ * cosD(_plane.yaw));
	_plane.FnewZ = fixMul(_plane.FtmpZ, fixCosD(_plane.Fyaw));

	// Translate rotated point back to where it should be
	// relative to it's last position.
	_plane.FcollectX = fixAdd(_plane.FcollectX, _plane.FnewX);
	if ((_plane.FcollectX > iToFix(1)) || (_plane.FcollectX < iToFix(-1))) {
		_plane.x -= fixToI(_plane.FcollectX);
		_plane.FcollectX = iToFix(0);
	}
	_plane.FcollectY = fixAdd(_plane.FcollectY, _plane.FnewY);
	if ((_plane.FcollectY > iToFix(1)) || (_plane.FcollectY < iToFix(-1))) {
		_plane.y -= fixToI(_plane.FcollectY);
		_plane.FcollectY = iToFix(0);
	}
	_plane.FcollectZ = fixAdd(_plane.FcollectZ, _plane.FnewZ);
	if ((_plane.FcollectZ > iToFix(1)) || (_plane.FcollectZ < iToFix(-1))) {
		_plane.z += fixToI(_plane.FcollectZ);
		_plane.FcollectZ = iToFix(0);
	}

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


#define SEGMENT_SETUP


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

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


#define SEGMENT_MAIN


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