public void GetClCdCmSteady(InstantConditionSimInput input, out InstantConditionSimOutput output, bool clear, bool reset_stall = false)
        {
            output = new InstantConditionSimOutput();

            double area = 0;
            double MAC = 0;
            double b_2 = 0;

            Vector3d forward = Vector3.forward;
            Vector3d up = Vector3.up;
            Vector3d right = Vector3.right;

            Vector3d CoM = Vector3d.zero;
            double mass = 0;
            List<Part> partsList = EditorLogic.SortedShipList;
            for (int i = 0; i < partsList.Count; i++)
            {
                Part p = partsList[i];

                if (FARAeroUtil.IsNonphysical(p))
                    continue;

                double partMass = p.mass;
                if (p.Resources.Count > 0)
                    partMass += p.GetResourceMass();

                //partMass += p.GetModuleMass(p.mass);
                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
                CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
                mass += partMass;
            }
            CoM /= mass;

            if (EditorDriver.editorFacility == EditorFacility.VAB)
            {
                forward = Vector3.up;
                up = -Vector3.forward;
            }

            double sinAlpha = Math.Sin(input.alpha * Math.PI / 180);
            double cosAlpha = Math.Sqrt(Math.Max(1 - sinAlpha * sinAlpha, 0));

            double sinBeta = Math.Sin(input.beta * Math.PI / 180);
            double cosBeta = Math.Sqrt(Math.Max(1 - sinBeta * sinBeta, 0));

            double sinPhi = Math.Sin(input.phi * Math.PI / 180);
            double cosPhi = Math.Sqrt(Math.Max(1 - sinPhi * sinPhi, 0));

            double alphaDot = input.alphaDot * Math.PI / 180;
            double betaDot = input.betaDot * Math.PI / 180;
            double phiDot = input.phiDot * Math.PI / 180;

            Vector3d AngVel = (phiDot - sinAlpha * betaDot) * forward;
            AngVel += (cosPhi * alphaDot + cosAlpha * sinPhi * betaDot) * right;
            AngVel += (sinPhi * alphaDot - cosAlpha * cosPhi * betaDot) * up;

            Vector3d velocity = forward * cosAlpha * cosBeta;
            velocity += right * (sinPhi * cosAlpha * cosBeta + cosPhi * sinBeta);
            velocity += -up * cosPhi * (sinAlpha * cosBeta + sinBeta);

            velocity.Normalize();

            //this is negative wrt the ground
            Vector3d liftVector = -forward * sinAlpha + right * sinPhi * cosAlpha - up * cosPhi * cosAlpha;

            Vector3d sideways = Vector3.Cross(velocity, liftVector).normalized;


            for (int i = 0; i < _wingAerodynamicModel.Count; i++)
            {
                FARWingAerodynamicModel w = _wingAerodynamicModel[i];
                if (!(w && w.part))
                    continue;

                w.ComputeForceEditor(velocity.normalized, input.machNumber, 2);

                if (clear)
                    w.EditorClClear(reset_stall);

                Vector3d relPos = w.GetAerodynamicCenter() - CoM;

                Vector3d vel = velocity + Vector3d.Cross(AngVel, relPos);

                if (w is FARControllableSurface)
                    (w as FARControllableSurface).SetControlStateEditor(CoM, vel, (float)input.pitchValue, 0, 0, input.flaps, input.spoilers);
                else if (w.isShielded)
                    continue;


                //w.ComputeForceEditor(velocity, input.machNumber);     //do this just to get the AC right

                Vector3d force = w.ComputeForceEditor(vel.normalized, input.machNumber, 2) * 1000;

                output.Cl += -Vector3d.Dot(force, liftVector);
                output.Cy += Vector3d.Dot(force, sideways);
                output.Cd += -Vector3d.Dot(force, velocity);

                Vector3d moment = -Vector3d.Cross(relPos, force);

                output.Cm += Vector3d.Dot(moment, sideways);
                output.Cn += Vector3d.Dot(moment, liftVector);
                output.C_roll += Vector3d.Dot(moment, velocity);

                //w.ComputeClCdEditor(vel.normalized, input.machNumber);

                /*double tmpCl = w.GetCl() * w.S;
                output.Cl += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), liftVector);
                output.Cy += tmpCl * -Vector3d.Dot(w.GetLiftDirection(), sideways);
                double tmpCd = w.GetCd() * w.S;
                output.Cd += tmpCd;
                output.Cm += tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), liftVector) + tmpCd * -Vector3d.Dot((relPos), liftVector);
                output.Cn += tmpCd * Vector3d.Dot((relPos), sideways) + tmpCl * Vector3d.Dot((relPos), velocity) * -Vector3d.Dot(w.GetLiftDirection(), sideways);
                output.C_roll += tmpCl * Vector3d.Dot((relPos), sideways) * -Vector3d.Dot(w.GetLiftDirection(), liftVector);*/
                area += w.S;
                MAC += w.GetMAC() * w.S;
                b_2 += w.Getb_2() * w.S;
            }
            FARCenterQuery center = new FARCenterQuery();
            for (int i = 0; i < _currentAeroSections.Count; i++)
            {
                _currentAeroSections[i].PredictionCalculateAeroForces(2, (float)input.machNumber, 10000, 0.005f, velocity.normalized, center);
            }

            Vector3d centerForce = center.force * 1000;

            output.Cl += -Vector3d.Dot(centerForce, liftVector);
            output.Cy += Vector3d.Dot(centerForce, sideways);
            output.Cd += -Vector3d.Dot(centerForce, velocity);

            Vector3d centerMoment = -center.TorqueAt(CoM) * 1000;

            output.Cm += Vector3d.Dot(centerMoment, sideways);
            output.Cn += Vector3d.Dot(centerMoment, liftVector);
            output.C_roll += Vector3d.Dot(centerMoment, velocity);
            

            /*for (int i = 0; i < FARAeroUtil.CurEditorParts.Count; i++)
            {
                Part p = FARAeroUtil.CurEditorParts[i];
                if (FARAeroUtil.IsNonphysical(p))
                    continue;

                Vector3 part_pos = p.transform.TransformPoint(p.CoMOffset) - CoM;
                double partMass = p.mass;
                if (p.Resources.Count > 0)
                    partMass += p.GetResourceMass();

                double stock_drag = partMass * p.maximum_drag * FlightGlobals.DragMultiplier * 1000;
                output.Cd += stock_drag;
                output.Cm += stock_drag * -Vector3d.Dot(part_pos, liftVector);
                output.Cn += stock_drag * Vector3d.Dot(part_pos, sideways);
            }*/

            if (area == 0)
            {
                area = _maxCrossSectionFromBody;
                b_2 = 1;
                MAC = _bodyLength;
            }

            double recipArea = 1 / area;

            MAC *= recipArea;
            b_2 *= recipArea;
            output.Cl *= recipArea;
            output.Cd *= recipArea;
            output.Cm *= recipArea / MAC;
            output.Cy *= recipArea;
            output.Cn *= recipArea / b_2;
            output.C_roll *= recipArea / b_2;
        }
        public StabilityDerivOutput CalculateStabilityDerivs(double u0, double q, double machNumber, double alpha, double beta, double phi, int flapSetting, bool spoilers, CelestialBody body, double alt)
        {
            StabilityDerivOutput stabDerivOutput = new StabilityDerivOutput();
            stabDerivOutput.nominalVelocity = u0;
            stabDerivOutput.altitude = alt;
            stabDerivOutput.body = body;

            Vector3d CoM = Vector3d.zero;
            double mass = 0;

            double MAC = 0;
            double b = 0;
            double area = 0;

            double Ix = 0;
            double Iy = 0;
            double Iz = 0;
            double Ixy = 0;
            double Iyz = 0;
            double Ixz = 0;

            InstantConditionSimInput input = new InstantConditionSimInput(alpha, beta, phi, 0, 0, 0, machNumber, 0, flapSetting, spoilers);
            InstantConditionSimOutput nominalOutput;
            InstantConditionSimOutput pertOutput = new InstantConditionSimOutput();

            _instantCondition.GetClCdCmSteady(input, out nominalOutput, true);

            List<Part> partsList = EditorLogic.SortedShipList;
            for (int i = 0; i < partsList.Count; i++)
            {
                Part p = partsList[i];

                if (FARAeroUtil.IsNonphysical(p))
                    continue;
                double partMass = p.mass;
                if (p.Resources.Count > 0)
                    partMass += p.GetResourceMass();

                //partMass += p.GetModuleMass(p.mass);
                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass
                CoM += partMass * (Vector3d)p.transform.TransformPoint(p.CoMOffset);
                mass += partMass;
                FARWingAerodynamicModel w = p.GetComponent<FARWingAerodynamicModel>();
                if (w != null)
                {
                    if (w.isShielded)
                        continue;

                    area += w.S;
                    MAC += w.GetMAC() * w.S;
                    b += w.Getb_2() * w.S;
                    if (w is FARControllableSurface)
                    {
                        (w as FARControllableSurface).SetControlStateEditor(CoM, p.transform.up, 0, 0, 0, input.flaps, input.spoilers);
                    }
                }
            }
            if (area == 0)
            {
                area = _instantCondition._maxCrossSectionFromBody;
                MAC = _instantCondition._bodyLength;
                b = 1;
            }
            MAC /= area;
            b /= area;
            CoM /= mass;
            mass *= 1000;

            stabDerivOutput.b = b;
            stabDerivOutput.MAC = MAC;
            stabDerivOutput.area = area;

            for (int i = 0; i < partsList.Count; i++)
            {
                Part p = partsList[i];

                if (p == null || FARAeroUtil.IsNonphysical(p))
                    continue;
                //This section handles the parallel axis theorem
                Vector3 relPos = p.transform.TransformPoint(p.CoMOffset) - CoM;
                double x2, y2, z2, x, y, z;
                x2 = relPos.z * relPos.z;
                y2 = relPos.x * relPos.x;
                z2 = relPos.y * relPos.y;
                x = relPos.z;
                y = relPos.x;
                z = relPos.y;

                double partMass = p.mass;
                if (p.Resources.Count > 0)
                    partMass += p.GetResourceMass();

                //partMass += p.GetModuleMass(p.mass);
                // If you want to use GetModuleMass, you need to start from p.partInfo.mass, not p.mass

                Ix += (y2 + z2) * partMass;
                Iy += (x2 + z2) * partMass;
                Iz += (x2 + y2) * partMass;

                Ixy += -x * y * partMass;
                Iyz += -z * y * partMass;
                Ixz += -x * z * partMass;

                //And this handles the part's own moment of inertia
                Vector3 principalInertia = p.Rigidbody.inertiaTensor;
                Quaternion prncInertRot = p.Rigidbody.inertiaTensorRotation;

                //The rows of the direction cosine matrix for a quaternion
                Vector3 Row1 = new Vector3(prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
                    2 * (prncInertRot.x * prncInertRot.y + prncInertRot.z * prncInertRot.w),
                    2 * (prncInertRot.x * prncInertRot.z - prncInertRot.y * prncInertRot.w));

                Vector3 Row2 = new Vector3(2 * (prncInertRot.x * prncInertRot.y - prncInertRot.z * prncInertRot.w),
                    -prncInertRot.x * prncInertRot.x + prncInertRot.y * prncInertRot.y - prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w,
                    2 * (prncInertRot.y * prncInertRot.z + prncInertRot.x * prncInertRot.w));

                Vector3 Row3 = new Vector3(2 * (prncInertRot.x * prncInertRot.z + prncInertRot.y * prncInertRot.w),
                    2 * (prncInertRot.y * prncInertRot.z - prncInertRot.x * prncInertRot.w),
                    -prncInertRot.x * prncInertRot.x - prncInertRot.y * prncInertRot.y + prncInertRot.z * prncInertRot.z + prncInertRot.w * prncInertRot.w);


                //And converting the principal moments of inertia into the coordinate system used by the system
                Ix += principalInertia.x * Row1.x * Row1.x + principalInertia.y * Row1.y * Row1.y + principalInertia.z * Row1.z * Row1.z;
                Iy += principalInertia.x * Row2.x * Row2.x + principalInertia.y * Row2.y * Row2.y + principalInertia.z * Row2.z * Row2.z;
                Iz += principalInertia.x * Row3.x * Row3.x + principalInertia.y * Row3.y * Row3.y + principalInertia.z * Row3.z * Row3.z;

                Ixy += principalInertia.x * Row1.x * Row2.x + principalInertia.y * Row1.y * Row2.y + principalInertia.z * Row1.z * Row2.z;
                Ixz += principalInertia.x * Row1.x * Row3.x + principalInertia.y * Row1.y * Row3.y + principalInertia.z * Row1.z * Row3.z;
                Iyz += principalInertia.x * Row2.x * Row3.x + principalInertia.y * Row2.y * Row3.y + principalInertia.z * Row2.z * Row3.z;
            }
            Ix *= 1000;
            Iy *= 1000;
            Iz *= 1000;

            stabDerivOutput.stabDerivs[0] = Ix;
            stabDerivOutput.stabDerivs[1] = Iy;
            stabDerivOutput.stabDerivs[2] = Iz;

            stabDerivOutput.stabDerivs[24] = Ixy;
            stabDerivOutput.stabDerivs[25] = Iyz;
            stabDerivOutput.stabDerivs[26] = Ixz;


            double effectiveG = _instantCondition.CalculateAccelerationDueToGravity(body, alt);     //This is the effect of gravity
            effectiveG -= u0 * u0 / (alt + body.Radius);                          //This is the effective reduction of gravity due to high velocity
            double neededCl = mass * effectiveG / (q * area);


            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);
            //Longitudinal Mess
            _instantCondition.SetState(machNumber, neededCl, CoM, 0, input.flaps, input.spoilers);

            alpha = FARMathUtil.BrentsMethod(_instantCondition.FunctionIterateForAlpha, -30d, 30d, 0.001, 500);
            input.alpha = alpha;
            nominalOutput = _instantCondition.iterationOutput;
            //alpha_str = (alpha * Mathf.PI / 180).ToString();

            input.alpha = (alpha + 2);

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            stabDerivOutput.stableCl = neededCl;
            stabDerivOutput.stableCd = nominalOutput.Cd;
            stabDerivOutput.stableAoA = alpha;
            stabDerivOutput.stableAoAState = "";
            if (Math.Abs((nominalOutput.Cl - neededCl) / neededCl) > 0.1)
                stabDerivOutput.stableAoAState = ((nominalOutput.Cl > neededCl) ? "<" : ">");

            Debug.Log("Cl needed: " + neededCl + ", AoA: " + alpha + ", Cl: " + nominalOutput.Cl + ", Cd: " + nominalOutput.Cd);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / (2 * FARMathUtil.deg2rad);                   //vert vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / (2 * FARMathUtil.deg2rad);
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cl += nominalOutput.Cd;
            pertOutput.Cd -= nominalOutput.Cl;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (Iy * u0);

            stabDerivOutput.stabDerivs[3] = pertOutput.Cl;  //Zw
            stabDerivOutput.stabDerivs[4] = pertOutput.Cd;  //Xw
            stabDerivOutput.stabDerivs[5] = pertOutput.Cm;  //Mw

            input.alpha = alpha;
            input.machNumber = machNumber + 0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);

            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05 * machNumber;                   //fwd vel derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05 * machNumber;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05 * machNumber;

            pertOutput.Cl += 2 * nominalOutput.Cl;
            pertOutput.Cd += 2 * nominalOutput.Cd;

            pertOutput.Cl *= -q * area / (mass * u0);
            pertOutput.Cd *= -q * area / (mass * u0);
            pertOutput.Cm *= q * area * MAC / (u0 * Iy);

            stabDerivOutput.stabDerivs[6] = pertOutput.Cl;  //Zu
            stabDerivOutput.stabDerivs[7] = pertOutput.Cd;  //Xu
            stabDerivOutput.stabDerivs[8] = pertOutput.Cm;  //Mu

            input.machNumber = machNumber;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.alphaDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
           
            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.05;                   //pitch rate derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.05;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.05;

            pertOutput.Cl *= q * area * MAC / (2 * u0 * mass);
            pertOutput.Cd *= q * area * MAC / (2 * u0 * mass);
            pertOutput.Cm *= q * area * MAC * MAC / (2 * u0 * Iy);

            stabDerivOutput.stabDerivs[9] = pertOutput.Cl;  //Zq
            stabDerivOutput.stabDerivs[10] = pertOutput.Cd; //Xq
            stabDerivOutput.stabDerivs[11] = pertOutput.Cm; //Mq

            input.alphaDot = 0;
            input.pitchValue = 0.1;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
            
            pertOutput.Cl = (pertOutput.Cl - nominalOutput.Cl) / 0.1;                   //elevator derivs
            pertOutput.Cd = (pertOutput.Cd - nominalOutput.Cd) / 0.1;
            pertOutput.Cm = (pertOutput.Cm - nominalOutput.Cm) / 0.1;

            pertOutput.Cl *= q * area / mass;
            pertOutput.Cd *= q * area / mass;
            pertOutput.Cm *= q * area * MAC / Iy;

            stabDerivOutput.stabDerivs[12] = pertOutput.Cl; //Ze
            stabDerivOutput.stabDerivs[13] = pertOutput.Cd; //Xe
            stabDerivOutput.stabDerivs[14] = pertOutput.Cm; //Me

            //Lateral Mess

            input.pitchValue = 0;
            input.beta = (beta + 2);

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
            pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / (2 * FARMathUtil.deg2rad);                   //sideslip angle derivs
            pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / (2 * FARMathUtil.deg2rad);
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / (2 * FARMathUtil.deg2rad);

            pertOutput.Cy *= q * area / mass;
            pertOutput.Cn *= q * area * b / Iz;
            pertOutput.C_roll *= q * area * b / Ix;

            stabDerivOutput.stabDerivs[15] = pertOutput.Cy;     //Yb
            stabDerivOutput.stabDerivs[17] = pertOutput.Cn;     //Nb
            stabDerivOutput.stabDerivs[16] = pertOutput.C_roll; //Lb

            input.beta = beta;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.phiDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false);
           
            pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05;                   //roll rate derivs
            pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05;

            pertOutput.Cy *= q * area * b / (2 * mass * u0);
            pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[18] = pertOutput.Cy;     //Yp
            stabDerivOutput.stabDerivs[20] = pertOutput.Cn;     //Np
            stabDerivOutput.stabDerivs[19] = pertOutput.C_roll; //Lp


            input.phiDot = 0;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, true);

            input.betaDot = -0.05;

            _instantCondition.GetClCdCmSteady(input, out pertOutput, true, false); pertOutput.Cy = (pertOutput.Cy - nominalOutput.Cy) / 0.05f;                   //yaw rate derivs
            pertOutput.Cn = (pertOutput.Cn - nominalOutput.Cn) / 0.05f;
            pertOutput.C_roll = (pertOutput.C_roll - nominalOutput.C_roll) / 0.05f;

            pertOutput.Cy *= q * area * b / (2 * mass * u0);
            pertOutput.Cn *= q * area * b * b / (2 * Iz * u0);
            pertOutput.C_roll *= q * area * b * b / (2 * Ix * u0);

            stabDerivOutput.stabDerivs[21] = pertOutput.Cy;     //Yr
            stabDerivOutput.stabDerivs[23] = pertOutput.Cn;     //Nr
            stabDerivOutput.stabDerivs[22] = pertOutput.C_roll; //Lr

            return stabDerivOutput;
        }