Untitled

using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.CompilerServices;
using Unity.Mathematics;
using Unity.VisualScripting;
using Unity.VisualScripting.FullSerializer;
using UnityEngine;

public class Wing : MonoBehaviour
{
    public Rigidbody rb;

    public AeroSurfaceConfig config;

    public Transform wingAerodynamicCenter;

    public float controllAngle;

    private List<gizmo> g = new List<gizmo>();  //only for debugging

    private float defaultBend;

    private void Start()
    {
        defaultBend = transform.localEulerAngles.z;

        if(wingAerodynamicCenter == null)
        {
            wingAerodynamicCenter = transform;
        }
    }

    public Total GetSegmentForce(Vector3 com, Vector3 wind, float p)
    {
        Total FM = new Total();
        if (!gameObject.activeInHierarchy || config == null) return FM;

        Vector3 r = wingAerodynamicCenter.position - com;

        //---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- ????
        //Calculate Relative Velocity to the segment
        Vector3 Vs = Vector3.zero; //slipstream velocity
        Vector3 Vind = Vector3.zero; //induced velocity by other segments 
        Vector3 Vqs = Vector3.zero; //Velocity pertrubation ? (Sec. III D.)

        Vector3 V = -rb.velocity - Vector3.Cross(rb.angularVelocity, r) + Vs + Vind + Vqs + wind;
        Vector3 localV = transform.InverseTransformDirection(V); //should be local now
        localV = new Vector3(0, localV.y, localV.z);

        //p = Weather.instance.GetAirDensity(wingAerodynamicCenter.position); //Air density, Moved to Plane.cs so instance only gets claled once
        float a = Mathf.Atan(localV.y/localV.z); //Alpha(Angle of Attack)


        //Calculate effect of wing aspect ratio
        float AR = config.aspectRatio;
        float Cla = config.liftSlope;
        float CLa = Cla * (AR / (AR + 2 * (AR + 4) / (AR + 2))); //corrected lift slope with effect of AR


        //Calculate impact of controll surfaces
        float n = Mathf.Lerp(0.8f, 0.4f, (Mathf.Abs(controllAngle) - 10) / 50); //Magic formula by Ivan Pensionerov (Estimated based on graph in the expensive book "emperial factor to account for the effects if viscosity")
        float flapChord = config.meanChord * config.flapFraction;
        float airfoildChord = config.meanChord * (1 - config.flapFraction);
        float theta = Mathf.Acos(2*flapChord/airfoildChord-1);
        float liftEffectivness = 1 - (theta - Mathf.Sin(theta)) / Mathf.PI;
        float DeltaCL = CLa * liftEffectivness * n * controllAngle * Mathf.Deg2Rad;
        float DeltaCLmax = DeltaCL * Mathf.Clamp01(1 - 0.5f * ((flapChord / airfoildChord) - 0.1f) / 0.3f); //Magic formula by Ivan Pensionerov (Estimated based on graph in the expensive book)

        float correctedZeroLiftAoA = config.zeroLiftAoA * Mathf.Deg2Rad - DeltaCL / CLa; // zero lift angle of attack after controll surface effects

        float CLmaxP = CLa * (config.stallAngleHigh * Mathf.Deg2Rad - config.zeroLiftAoA * Mathf.Deg2Rad) + DeltaCLmax;
        float CLmaxN = CLa * (config.stallAngleLow * Mathf.Deg2Rad - config.zeroLiftAoA * Mathf.Deg2Rad) + DeltaCLmax;

        float aStallP = correctedZeroLiftAoA + CLmaxP / CLa; //positive stall angle after controll surface effect
        float aStallN = correctedZeroLiftAoA + CLmaxN / CLa; //negative stall angle after controll surface effect


        float Cl = 0; //Lift Coefficient
        float Cd = 0; //Drag Coefficient
        float Cm = 0; //Moment(torque) Coefficient

        // Low angles of attack mode and stall mode curves are stitched together by a line segment. 
        float paddingAngle = Mathf.Deg2Rad * Mathf.Lerp(8, 14, Mathf.Abs(Mathf.Abs(controllAngle) - 50) / 50);
        float paddedStallAngleHigh = aStallP + paddingAngle;
        float paddedStallAngleLow = aStallN - paddingAngle;

        //calculate CL, Cd, Cm based on low/high angle of attack mode
        if (aStallN < a && a < aStallP) //low angle of attack
        {
            Vector3 coefficients = CalculateCoefficientsLowAoA(CLa, a, correctedZeroLiftAoA, AR);
            Cl = coefficients.x;
            Cd = coefficients.y;
            Cm = coefficients.z;
        }
        else
        {
            if (a > paddedStallAngleHigh || a < paddedStallAngleLow)
            {
                // Stall mode.
                Vector3 coefficients = CalculateCoefficientsStall(CLa, a, correctedZeroLiftAoA, AR, aStallP, aStallN);
                Cl = coefficients.x;
                Cd = coefficients.y;
                Cm = coefficients.z;

                g.Add(new gizmo() { 
                    c = Color.red,
                    p = wingAerodynamicCenter.position,
                    r = 0.2f
                });
            }
            else
            {
                g.Add(new gizmo()
                {
                    c = Color.yellow,
                    p = wingAerodynamicCenter.position,
                    r = 0.2f
                });

                // Linear stitching in-between stall and low angles of attack modes.
                Vector3 aerodynamicCoefficientsLow;
                Vector3 aerodynamicCoefficientsStall;
                float lerpParam;

                if (a >= aStallP)
                {
                    aerodynamicCoefficientsLow = CalculateCoefficientsLowAoA(CLa, aStallP, correctedZeroLiftAoA, AR);
                    aerodynamicCoefficientsStall = CalculateCoefficientsStall(CLa, paddedStallAngleHigh, correctedZeroLiftAoA, AR, aStallP, aStallN);
                    lerpParam = (a - aStallP) / (paddedStallAngleHigh - aStallP);
                }
                else
                {
                    aerodynamicCoefficientsLow = CalculateCoefficientsLowAoA(CLa, aStallN, correctedZeroLiftAoA, AR);
                    aerodynamicCoefficientsStall = CalculateCoefficientsStall(CLa, paddedStallAngleLow, correctedZeroLiftAoA, AR, aStallP, aStallN);
                    lerpParam = (a - aStallN) / (paddedStallAngleLow - aStallN);
                }
                Vector3 coefficients = Vector3.Lerp(aerodynamicCoefficientsLow, aerodynamicCoefficientsStall, lerpParam);
                Cl = coefficients.x;
                Cd = coefficients.y;
                Cm = coefficients.z;
            }
        }

        //float CFx = Cl * Mathf.Sin(a) - Cd * Mathf.Sin(a); //Force Coefficient x
        //float CFy = -Cl * Mathf.Sin(a) - Cd * Mathf.Sin(a); //Force Coefficient y

        //Vector3 CF = new Vector3(CFx,0,CFy); //Force Coefficient
        //Vector3 CM = new Vector3(0,Cm,0); //Moment(torque) Coefficient

        //FM.force = 0.5f * p * config.span * config.meanChord * Vector3.Scale(Vector3.Scale(V, V), CF);
        //FM.moment = 0.5f * p * config.span * config.meanChord* config.meanChord * Vector3.Scale(Vector3.Scale(V, V), CM);

        Vector3 dragDir = transform.TransformDirection(localV.normalized);
        Vector3 liftDir = Vector3.Cross(dragDir, transform.right);

        float dynamicPressure = 0.5f * p * localV.sqrMagnitude;
        float area = config.span * config.meanChord;

        Vector3 lift = liftDir * Cl * dynamicPressure * area;
        Vector3 drag = dragDir * Cd * dynamicPressure * area;
        Vector3 torque = -transform.forward * Cm * dynamicPressure * area * config.meanChord;

        Debug.DrawLine(wingAerodynamicCenter.position, wingAerodynamicCenter.position + lift /3000, Color.green);
        Debug.DrawLine(wingAerodynamicCenter.position, wingAerodynamicCenter.position + drag /3000, Color.red);

        FM.force = lift + drag;
        FM.moment = Vector3.Cross(r, FM.force) + torque;

        //bend wing
        if (Application.isPlaying)
        {
            float f = transform.InverseTransformDirection((lift + drag + torque) / 3000).y;
            float BendFac = Mathf.Clamp(f / 20, -1, 1);
            Vector3 currentEulerAngles = transform.localEulerAngles;
            transform.localEulerAngles = new Vector3(currentEulerAngles.x, currentEulerAngles.y, 3);
        }

        return FM;
    }

    public Vector3 GetSegmentForceCoeficients(Vector3 com, float a)
    {
        controllAngle = 3;

        //Calculate effect of wing aspect ratio
        float AR = config.aspectRatio;
        float Cla = config.liftSlope;
        float CLa = Cla * (AR / (AR + 2 * (AR + 4) / (AR + 2))); //corrected lift slope with effect of AR


        //Calculate impact of controll surfaces
        float n = Mathf.Lerp(0.8f, 0.4f, (Mathf.Abs(controllAngle) - 10) / 50); //Magic formula by Ivan Pensionerov (Estimated based on graph in the expensive book "emperial factor to account for the effects if viscosity")

        Debug.Log("nf: " + n);//correct
        
        float flapChord = config.meanChord * config.flapFraction;
        float airfoildChord = config.meanChord * (1 - config.flapFraction);
        float theta = Mathf.Acos(2 * flapChord / airfoildChord - 1);

        Debug.Log("Theta : " + theta);//correct

        float liftEffectivness = 1 - (theta - Mathf.Sin(theta)) / Mathf.PI;

        Debug.Log("Lift Effectivness: " + liftEffectivness);//correct

        float DeltaCL = CLa * liftEffectivness * n * controllAngle * Mathf.Deg2Rad;

        Debug.Log("Delta CL: " + DeltaCL);//correct

        float DeltaCLmax = DeltaCL * Mathf.Clamp01(1 - 0.5f * ((flapChord/airfoildChord) - 0.1f) / 0.3f); //Magic formula by Ivan Pensionerov (Estimated based on graph in the expensive book)

        Debug.Log("Delta CL max: " + DeltaCLmax); //correct

        float correctedZeroLiftAoA = config.zeroLiftAoA * Mathf.Deg2Rad - DeltaCL / CLa; // zero lift angle of attack after controll surface effects

        Debug.Log("Corrected zero lift aoa: " + correctedZeroLiftAoA); //correct

        float CLmaxP = CLa * (config.stallAngleHigh * Mathf.Deg2Rad - config.zeroLiftAoA * Mathf.Deg2Rad) + DeltaCLmax;

        Debug.Log("Delta CL max P: " + CLmaxP); //correct

        float CLmaxN = CLa * (config.stallAngleLow * Mathf.Deg2Rad - config.zeroLiftAoA * Mathf.Deg2Rad) + DeltaCLmax;

        Debug.Log("Delta CL max N: " + CLmaxN); //correct

        float aStallP = correctedZeroLiftAoA + CLmaxP / CLa; //positive stall angle after controll surface effect
        float aStallN = correctedZeroLiftAoA + CLmaxN / CLa; //negative stall angle after controll surface effect


        float Cl = 0; //Lift Coefficient
        float Cd = 0; //Drag Coefficient
        float Cm = 0; //Moment(torque) Coefficient

        // Low angles of attack mode and stall mode curves are stitched together by a line segment. 
        float paddingAngle = Mathf.Deg2Rad * Mathf.Lerp(8, 14, Mathf.Abs(Mathf.Abs(controllAngle) - 50) / 50);
        float paddedStallAngleHigh = aStallP + paddingAngle;
        float paddedStallAngleLow = aStallN - paddingAngle;

        Debug.Log("Stall Angle High: " + aStallP); //correct
        Debug.Log("Stall Angle Low: " + aStallN); //correct

        Debug.Log("padding Angle: " + paddingAngle * Mathf.Rad2Deg); //correct

        Debug.Log("Padded Stall Angle High: " + paddedStallAngleHigh); //correct
        Debug.Log("Padded Stall Angle Low: " + paddedStallAngleLow); //correct

        //calculate CL, Cd, Cm based on low/high angle of attack mode
        if (aStallN < a && a < aStallP) //low angle of attack
        {
            Vector3 coefficients = CalculateCoefficientsLowAoA(CLa, a, correctedZeroLiftAoA, AR);
            Cl = coefficients.x;
            Cd = coefficients.y;
            Cm = coefficients.z;
        }
        else
        {
            if (a > paddedStallAngleHigh || a < paddedStallAngleLow)
            {
                // Stall mode.
                Vector3 coefficients = CalculateCoefficientsStall(CLa, a, correctedZeroLiftAoA, AR, aStallP, aStallN);
                Cl = coefficients.x;
                Cd = coefficients.y;
                Cm = coefficients.z;
            }
            else
            {
                // Linear stitching in-between stall and low angles of attack modes.
                Vector3 aerodynamicCoefficientsLow;
                Vector3 aerodynamicCoefficientsStall;
                float lerpParam;

                if (a >= aStallP)
                {
                    aerodynamicCoefficientsLow = CalculateCoefficientsLowAoA(CLa, aStallP, correctedZeroLiftAoA, AR);
                    aerodynamicCoefficientsStall = CalculateCoefficientsStall(CLa, paddedStallAngleHigh, correctedZeroLiftAoA, AR, aStallP, aStallN);
                    lerpParam = (a - aStallP) / (paddedStallAngleHigh - aStallP);
                }
                else
                {
                    aerodynamicCoefficientsLow = CalculateCoefficientsLowAoA(CLa, aStallN, correctedZeroLiftAoA, AR);
                    aerodynamicCoefficientsStall = CalculateCoefficientsStall(CLa, paddedStallAngleLow, correctedZeroLiftAoA, AR, aStallP, aStallN);
                    lerpParam = (a - aStallN) / (paddedStallAngleLow - aStallN);
                }
                Vector3 coefficients = Vector3.Lerp(aerodynamicCoefficientsLow, aerodynamicCoefficientsStall, lerpParam);
                Cl = coefficients.x;
                Cd = coefficients.y;
                Cm = coefficients.z;
            }
        }

        return new Vector3(Cl, Cd, Cm);
    }

    private Vector3 CalculateCoefficientsLowAoA(float CLa, float a, float correctedZeroLiftAoA, float AR)
    {
        Vector3 c;

        c.x = CLa * (a - correctedZeroLiftAoA); //Lift coefficient

        float ai = c.x / (Mathf.PI * AR); //induced angle of attack
        float aeff = a - correctedZeroLiftAoA - ai; //efficint angle of attack

        float CT = config.skinFriction * Mathf.Cos(aeff); //Normal force coefficient
        float CN = (c.x + CT * Mathf.Sin(aeff)) / Mathf.Cos(aeff);//Tangent force coefficient

        c.y = CN * Mathf.Sin(aeff) + CT * Mathf.Cos(aeff); //Drag coefficient
        c.z = -CN * (0.25f - 0.175f * (1 - 2 * Mathf.Abs(aeff) / Mathf.PI)); //Moment coefficient

        return c;
    }

    private Vector3 CalculateCoefficientsStall(float CLa, float a, float correctedZeroLiftAoA, float AR, float aStallP, float aStallN)
    {
        Vector3 c;

        float CLlowAoA;
        float fac;
        if (a >= aStallP)
        {
            CLlowAoA = CLa * (aStallP - correctedZeroLiftAoA);
            fac = 1 - (a - aStallP) * 1 / (90 - aStallP);
        }
        else
        {
            CLlowAoA = CLa * (aStallN - correctedZeroLiftAoA);
            fac = 1 - (a - aStallN) * 1 / (-90 - aStallN);
        }

        float ai = (CLlowAoA / (Mathf.PI * AR)) * fac; //induced angle of attack, tappered to 0 at a+-90
        float aeff = a - correctedZeroLiftAoA - ai; //efficint angle of attack

        float controllAngleRad = controllAngle * Mathf.Deg2Rad; //be carefull with Radians and Degree
        float Cd90 = -0.0426f * controllAngleRad * controllAngleRad + 0.21f * controllAngleRad + 1.98f; //drag at 90° based on flap angle(convex or concave wing structure)
        float CN = Cd90 * Mathf.Sin(aeff) * (1 / (0.56f + 0.44f * Mathf.Abs(Mathf.Sin(aeff))) - 0.41f * (1 - Mathf.Exp(-17 / AR)));
        float CT = 0.5f * config.skinFriction * Mathf.Cos(aeff);

        c.x = CN * Mathf.Cos(aeff) - CT * Mathf.Sin(aeff);
        c.y = CN * Mathf.Sin(aeff) + CT * Mathf.Cos(aeff);
        c.z = -CN * (0.25f - 0.175f * (1 - 2 * Mathf.Abs(aeff) / Mathf.PI));

        return c;
    }

    private void OnDrawGizmos()
    {
        foreach(gizmo g in g)
        {
            Gizmos.color = g.c;
            Gizmos.DrawSphere(g.p, g.r);
        }

        g.Clear();
    }
}


[Serializable]
public class gizmo
{
    public Color c;
    public Vector3 p;
    public float r;
}