context: #region Copyright(C)  Licensed under GNU GPL.
  /// Copyright (C) 2005-2006 Agustin Santos Mendez
  /// 
  /// JSBSim was developed by Jon S. Berndt, Tony Peden, and
  /// David Megginson. 
  /// Agustin Santos Mendez implemented and maintains this C# version.
  /// 
  /// This program is free software; you can redistribute it and/or
  ///  modify it under the terms of the GNU General Public License
  /// as published by the Free Software Foundation; either version 2
  /// of the License, or (at your option) any later version.
  ///  
  /// This program is distributed in the hope that it will be useful,
  /// but WITHOUT ANY WARRANTY; without even the implied warranty of
  /// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  /// GNU General Public License for more details.
  ///  
  /// You should have received a copy of the GNU General Public License
  /// along with this program; if not, write to the Free Software
  /// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  #endregion
  #region Identification
  /// $Id:$
  #endregion
  namespace JSBSim
  {
      using System;
      using System.Collections;
      using System.Collections.Generic;
      using CommonUtils.MathLib;
  
      // Import log4net classes.
      using log4net;
  
  
      public enum TrimMode
      {
          Longitudinal,
          Full,
          Ground,
          Pullup,
          Custom,
          None,
          Turn
      };
  
  
      /// <summary>
      /// Trim -- the trimming routine for JSBSim.
      /// Trim finds the aircraft attitude and control settings needed to maintain
      /// the steady state described by the InitialCondition object .  It does this
      /// iteratively by assigning a control to each state and adjusting that control
      /// until the state is within a specified tolerance of zero. States include the
      /// recti-linear accelerations udot, vdot, and wdot, the angular accelerations
      /// qdot, pdot, and rdot, and the difference between heading and ground track.
      /// Controls include the usual flight deck controls available to the pilot plus
      /// angle of attack (alpha), sideslip angle(beta), flight path angle (gamma),
      /// pitch attitude(theta), roll attitude(phi), and altitude above ground.  The
      /// last three are used for on-ground trimming. The state-control pairs used in
      /// a given trim are completely user configurable and several pre-defined modes
      /// are provided as well. They are:
      /// - tLongitudinal: Trim wdot with alpha, udot with thrust, qdot with elevator
      /// - tFull: tLongitudinal + vdot with phi, pdot with aileron, rdot with rudder
      /// and heading minus ground track (hmgt) with beta
      /// - tPullup: tLongitudinal but adjust alpha to achieve load factor input
      /// with SetTargetNlf()
      /// - tGround: wdot with altitude, qdot with theta, and pdot with phi
      /// 
      /// The remaining modes include <b>tCustom</b>, which is completely user defined and
      /// <b>tNone</b>.
      /// 
      /// Note that trims can (and do) fail for reasons that are completely outside
      /// the control of the trimming routine itself. The most common problem is the
      /// initial conditions: is the model capable of steady state flight
      /// at those conditions?  Check the speed, altitude, configuration (flaps,
      /// gear, etc.), weight, cg, and anything else that may be relevant.
      /// 
      /// Example usage:<pre>
      /// FGFDMExec* FDMExec = new FGFDMExec();
      /// 
      /// FGInitialCondition* fgic = new FGInitialCondition(FDMExec);
      /// FGTrim fgt(FDMExec, fgic, tFull);
      /// fgic.SetVcaibratedKtsIC(100);
      /// fgic.SetAltitudeFtIC(1000);
      /// fgic.SetClimbRate(500);
      /// if( !fgt.DoTrim() ) 
      /// {
      /// cout + "Trim Failed" + endl;
      /// }
      /// fgt.Report(); </pre>
      /// @author Tony Peden
      /// </summary>
      public class Trim
      {
          /// <summary>
          /// Define a static logger variable so that it references the
          ///	Logger instance.
          /// 
          /// NOTE that using System.Reflection.MethodBase.GetCurrentMethod().DeclaringType
          /// is equivalent to typeof(LoggingExample) but is more portable
          /// i.e. you can copy the code directly into another class without
          /// needing to edit the code.
          /// </summary>
          private static readonly ILog log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
  
          /// <summary>
          /// Initializes the trimming class
          /// </summary>
          /// <param name="fdmexec">a JSBSim executive object</param>
          /// <param name="tm">trim mode</param>
          public Trim(FDMExecutive exec, TrimMode tm)
          {
  
              N = Nsub = 0;
              max_iterations = 60;
              max_sub_iterations = 100;
              Tolerance = 1E-3;
              A_Tolerance = Tolerance / 10;
  
              Debug = 0; DebugLevel = 0;
              fdmex = exec;
              fgic = fdmex.GetIC();
              total_its = 0;
              trimudot = true;
              gamma_fallback = true;
              axis_count = 0;
              mode = tm;
              xlo = xhi = alo = ahi = 0.0;
              targetNlf = 1.0;
              debug_axis = StateType.All;
              SetMode(tm);
              if (log.IsDebugEnabled)
                  log.Debug("Instantiated: FGTrim");
          }
  
  
          /// <summary>
          /// Execute the trim
          /// </summary>
          /// <returns></returns>
          public bool DoTrim()
          {
  #if TODO
              trim_failed = false;
              int i;
  
              for (i = 0; i < fdmex.GroundReactions.NumGearUnits; i++)
              {
                  fdmex.GroundReactions.GetGearUnit(i).SetReport(false);
              }
  
              fdmex.DisableOutput();
  
              fgic.PRadpsIC = 0.0;
              fgic.QRadpsIC = 0.0;
              fgic.RRadpsIC = 0.0;
  
              //clear the sub iterations counts & zero out the controls
              for (current_axis = 0; current_axis < TrimAxes.Count; current_axis++)
              {
                  //cout + current_axis + "  " + TrimAxes[current_axis].GetStateName()
                  //+ "  " + TrimAxes[current_axis].GetControlName()+ endl;
                  if (TrimAxes[current_axis].GetStateType() == StateType.Qdot)
                  {
                      if (mode == TrimMode.Ground)
                      {
                          TrimAxes[current_axis].initTheta();
                      }
                  }
                  xlo = TrimAxes[current_axis].GetControlMin();
                  xhi = TrimAxes[current_axis].GetControlMax();
                  TrimAxes[current_axis].SetControl((xlo + xhi) / 2);
                  TrimAxes[current_axis].Run();
                  //TrimAxes[current_axis].AxisReport();
                  sub_iterations[current_axis] = 0;
                  successful[current_axis] = 0;
                  solution[current_axis] = false;
              }
  
  
              if (mode == TrimMode.Pullup)
              {
                  log.Debug("Setting pitch rate and nlf... ");
                  setupPullup();
                  log.Debug("pitch rate done ... ");
                  TrimAxes[0].SetStateTarget(targetNlf);
                  log.Debug("nlf done");
              }
              else if (mode == TrimMode.Turn)
              {
                  setupTurn();
                  //TrimAxes[0].SetStateTarget(targetNlf);
              }
  
              do
              {
                  axis_count = 0;
                  for (current_axis = 0; current_axis < TrimAxes.Count; current_axis++)
                  {
                      SetDebug();
                      UpdateRates();
                      Nsub = 0;
                      if (!solution[current_axis])
                      {
                          if (CheckLimits())
                          {
                              solution[current_axis] = true;
                              Solve();
                          }
                      }
                      else if (FindInterval())
                      {
                          Solve();
                      }
                      else
                      {
                          solution[current_axis] = false;
                      }
                      sub_iterations[current_axis] += Nsub;
                  }
                  for (current_axis = 0; current_axis < TrimAxes.Count; current_axis++)
                  {
                      //these checks need to be done after all the axes have run
                      if (Debug > 0)
                          TrimAxes[current_axis].AxisReport();
                      if (TrimAxes[current_axis].InTolerance())
                      {
                          axis_count++;
                          successful[current_axis]++;
                      }
                  }
  
  
                  if ((axis_count == TrimAxes.Count - 1) && (TrimAxes.Count > 1))
                  {
                      //cout + TrimAxes.size()-1 + " out of " + TrimAxes.size() + "!" + endl;
                      //At this point we can check the input limits of the failed axis
                      //and declare the trim failed if there is no sign change. If there
                      //is, keep going until success or max iteration count
  
                      //Oh, well: two out of three ain't bad
                      for (current_axis = 0; current_axis < TrimAxes.Count; current_axis++)
                      {
                          //these checks need to be done after all the axes have run
                          if (!TrimAxes[current_axis].InTolerance())
                          {
                              if (!CheckLimits())
                              {
                                  // special case this for now -- if other cases arise proper
                                  // support can be added to TrimAxis
                                  if ((gamma_fallback) &&
                                      (TrimAxes[current_axis].GetStateType() == StateType.Udot) &&
                                      (TrimAxes[current_axis].GetControlType() == ControlType.Throttle))
                                  {
                                      if (log.IsErrorEnabled)
                                          log.Error("  Can't trim udot with throttle, trying flight"
                                              + " path angle. (" + N + ")");
                                      if (TrimAxes[current_axis].GetState() > 0)
                                          TrimAxes[current_axis].SetControlToMin();
                                      else
                                          TrimAxes[current_axis].SetControlToMax();
                                      TrimAxes[current_axis].Run();
                                      //TODO delete TrimAxes[current_axis];
                                      TrimAxes[current_axis] = new TrimAxis(fdmex, fgic, StateType.Udot,
                                          ControlType.Gamma);
                                  }
                                  else
                                  {
                                      if (log.IsErrorEnabled)
                                          log.Error("  Sorry, " + TrimAxes[current_axis].GetStateName()
                                          + " doesn't appear to be trimmable");
                                      //total_its=k;
                                      trim_failed = true; //force the trim to fail
                                  } //gamma_fallback
                              }
                          } //solution check
                      } //for loop
                  } //all-but-one check
                  N++;
                  if (N > max_iterations)
                      trim_failed = true;
              } while ((axis_count < TrimAxes.Count) && (!trim_failed));
              if ((!trim_failed) && (axis_count >= TrimAxes.Count))
              {
                  total_its = N;
                  if (log.IsDebugEnabled)
                      log.Debug("  Trim successful");
              }
              else
              {
                  total_its = N;
                  if (log.IsDebugEnabled)
                      log.Debug("  Trim failed");
              }
              for (i = 0; i < fdmex.GroundReactions.NumGearUnits; i++)
              {
                  fdmex.GroundReactions.GetGearUnit(i).SetReport(true);
              }
              fdmex.EnableOutput();
              return !trim_failed;
  #endif
              throw new NotImplementedException("Pending upgrade to lastest version of JSBSIM");
          }
  
          /** Print the results of the trim. For each axis trimmed, this
  			includes the final state value, control value, and tolerance
  			used.
  			@return true if trim succeeds
  		*/
          public void Report()
          {
  #if TODO
              log.Info("  Trim Results: ");
              for (current_axis = 0; current_axis < TrimAxes.Count; current_axis++)
                  TrimAxes[current_axis].AxisReport();
  #endif
              throw new NotImplementedException("Pending upgrade to lastest version of JSBSIM");
          }
  
          /** Iteration statistics
  		*/
          public void TrimStats()
          {
              int run_sum = 0;
              log.Info("  Trim Statistics: ");
              log.Info("    Total Iterations: " + total_its);
              if (total_its > 0)
              {
                  log.Info("    Sub-iterations:");
                  int axis = 0;
                  foreach (TrimAxis currentAxis in TrimAxes)
                  {
                      run_sum += currentAxis.GetRunCount();
                      log.Info(currentAxis.GetStateName() + ": " +
                          sub_iterations[axis] + " average: " +
                          sub_iterations[axis] / (double)(total_its) + "  successful: " +
                          successful[axis] + "  stability: " +
                          currentAxis.GetAvgStability());
                      axis++;
                  }
                  log.Info("    Run Count: " + run_sum);
              }
          }
  
          /// <summary>
          /// Clear all state-control pairs and set a predefined trim mode
          /// </summary>
          /// <param name="tm">the set of axes to trim. Can be:
          /// tLongitudinal, tFull, tGround, tCustom, or tNone</param>
          public void SetMode(TrimMode tm)
          {
              ClearStates();
              mode = tm;
              switch (tm)
              {
                  case TrimMode.Full:
                      if (log.IsDebugEnabled)
                          log.Debug("  Full Trim");
                      TrimAxes.Add(new TrimAxis(fdmex, fgic, StateType.Wdot, ControlType.Alpha));
                      TrimAxes.Add(new TrimAxis(fdmex, fgic, StateType.Udot, ControlType.Throttle));
                      TrimAxes.Add(new TrimAxis(fdmex, fgic, StateType.Qdot, ControlType.PitchTrim));
                      TrimAxes.Add(new TrimAxis(fdmex, fgic, StateType.Hmgt, ControlType.Beta));
                      TrimAxes.Add(new TrimAxis(fdmex, fgic, StateType.Vdot, ControlType.Phi));
                      TrimAxes.Add(new TrimAxis(fdmex, fgic, StateType.Pdot, ControlType.Aileron));
                      TrimAxes.Add(new TrimAxis(fdmex, fgic, StateType.Rdot, ControlType.Rudder));
                      break;
                  case TrimMode.Longitudinal:
                      if (log.IsDebugEnabled)
  
gt:                         log.Debug("  Longitudinal Trim");