package dev.lumareth.improvedAerodynamics.flight;

import dev.lumareth.improvedAerodynamics.Config;
import org.joml.Matrix3dc;
import org.joml.Quaterniondc;
import org.joml.Vector3d;
import org.joml.Vector3dc;

/**
 * Auto trim for the pitch control surfaces of one control node: while the pilot gives no pitch input it adds a
 * pitch offset that brings the flight path back to level and then holds it there, cancelling whatever pitching
 * moment the craft has (off-center weight, thrust line, speed changes).
 * <p>
 * Two loops: the flight path angle sets a gentle target pitch rate, and a PI controller moves the trim to reach it.
 * The trim's effect is measured every substep (torque per unit of pitch input), which gives the pitch axis and lets
 * the gains be expressed as angular acceleration, so the same tuning works for any craft, speed and altitude.
 * The trim holds still while the pilot pitches, when too slow, and when wings stall, so it never fights maneuvers.
 */
final class AutoTrim {
    // Target pitch rate per radian of flight path angle, and its limit (rad/s)
    private static final double PATH_GAIN = 0.8;
    private static final double MAX_PITCH_RATE = 0.1;
    // Angular acceleration per rad/s of pitch rate error, and integral gain
    private static final double PROPORTIONAL_GAIN = 3.0;
    private static final double INTEGRAL_GAIN = 1.5;
    // Fastest the trim moves, in input per second
    private static final double TRIM_RATE = 1.0;
    private static final double PILOT_DEADBAND = 0.01;

    private AutoTrim() {
    }

    static void update(AeroState.NodeState state, NodeData node, Vector3dc worldVelocity, Quaterniondc rotation,
                       Matrix3dc inertia, double[] w, double dt, boolean stalled) {
        if (!node.autoTrim()) {
            state.trimIntegral = 0.0;
            state.trimOutput = AeroState.moveTowards(state.trimOutput, 0.0, TRIM_RATE * dt);
            state.savedTrim = 0.0;
            return;
        }
        double authority = Config.TRIM_AUTHORITY.get();
        double response = state.pitchResponse.length();
        double speed = worldVelocity.length();
        boolean hold = Math.abs(node.pitch()) > PILOT_DEADBAND || speed < Config.TRIM_MIN_SPEED.get() || stalled
                || response < 1.0E-6;

        double target = state.trimIntegral;
        if (!hold) {
            // Pitch axis in ship axes, oriented so that positive rotation is what more pitch input does
            Vector3d axis = state.pitchResponse.div(response, new Vector3d());
            double axisInertia = axis.dot(inertia.transform(axis, new Vector3d()));
            double pitchRate = axis.x * w[0] + axis.y * w[1] + axis.z * w[2];

            // Does rotating about that axis raise the flight path? Sign of the vertical part of axis x direction
            Vector3d worldAxis = rotation.transform(axis, new Vector3d());
            Vector3d direction = worldVelocity.normalize(new Vector3d());
            double noseUp = Math.signum(worldAxis.cross(direction, new Vector3d()).y);
            double pathAngle = Math.asin(Math.max(-1.0, Math.min(1.0, direction.y)));

            double targetRate = Math.max(-MAX_PITCH_RATE, Math.min(MAX_PITCH_RATE, -PATH_GAIN * pathAngle * noseUp));
            double error = targetRate - pitchRate;
            // Pitch input needed per rad/s² of angular acceleration
            double gain = axisInertia / response;
            state.trimIntegral = clamp(state.trimIntegral + INTEGRAL_GAIN * error * dt * gain, authority);
            target = clamp(state.trimIntegral + PROPORTIONAL_GAIN * error * gain, authority);
        }
        state.trimOutput = AeroState.moveTowards(state.trimOutput, target, TRIM_RATE * dt);
        state.savedTrim = state.trimIntegral;
    }

    private static double clamp(double value, double limit) {
        return Math.max(-limit, Math.min(limit, value));
    }
}