AuvUndulateMissionControl.h

#pragma once
 
#include "SampleClock.h"
 
#include <fhsim/simobject/SimObject.h>
#include <string>
#include <vector>
 
class PrintDuringExec;
 
class AuvUndulateMissionControl : public SimObject
{
public:
   enum EPhase_t
   {
      Phase_Transit = 0, 
      Phase_Return,      
      Phase_Finished     
   };
 
   struct Settings
   {
      std::vector<double> waypointsX;                    
      std::vector<double> waypointsY;                    
      double              recoveryX             = 0.0;   
      double              recoveryY             = 0.0;   
      double              waypointRadius        = 15.0;  
      double              cruiseSpeed           = 1.5;   
      double              batteryAtEndOfMission = 2.7e5; 
      bool                loopWaypoints         = false; 
   };
 
   struct Command
   {
      double   speed              = 0.0;           
      double   heading            = 0.0;           
      double   distanceToWaypoint = 0.0;           
      int      waypointIndex      = 0;             
      EPhase_t phase              = Phase_Transit; 
      bool     endMission         = false;         
   };
 
   AuvUndulateMissionControl(std::string simObjectName, ISimObjectCreator* creator);
 
   AuvUndulateMissionControl(std::string simObjectName, const Settings& settings, double updatePeriod = 1.0);
 
   static double WrapToPi(double angle);
 
   static std::string WaypointListError(int numX, int numY);
 
   static bool CanReachRecovery(const Settings& settings, double posX, double posY,
         double battery, double metresPerJoule);
 
   static Command CommandOf(const Settings& settings, const Command& previous,
         double posX, double posY, double battery, double headingMeasured, double metresPerJoule);
 
   void OdeFcn(const double dT, const double* const adX, double* const adXDot) const override;
 
   void FinalSetup(const double dT, const double* const adX, ISimObjectCreator* const creator) override;
 
   void AcceptedStep(const double dT, const double* const adX) override;
 
   void Update(double posX, double posY, double battery, double headingMeasured);
 
   const Command& HeldCommand() const { return m_command; }
 
   double MetresPerJoule() const;
 
protected:
   const double* OutSpeed(const double dT, const double* const adX);
 
   const double* OutHeading(const double dT, const double* const adX);
 
   const double* OutEndMission(const double dT, const double* const adX);
 
   const double* OutWaypointIndex(const double dT, const double* const adX);
 
   const double* OutDistanceToWaypoint(const double dT, const double* const adX);
 
   const double* OutPhase(const double dT, const double* const adX);
 
private:
   void AccumulateProgress(double posX, double posY, double battery);
 
   void ReportPhaseChange(EPhase_t previousPhase);
 
   void RefreshPortValues();
 
   ISignalPort* m_inPos     = nullptr; 
   ISignalPort* m_inBattery = nullptr; 
   ISignalPort* m_inHeading = nullptr; 
 
   PrintDuringExec* m_print = nullptr; 
 
   std::string m_name;     
   Settings    m_settings; 
   SampleClock m_clock;    
 
   Command m_command; 
 
   double m_outEndMission    = 0.0; 
   double m_outWaypointIndex = 0.0; 
   double m_outPhase         = 0.0; 
 
   bool   m_progressStarted   = false; 
   double m_startBattery      = 0.0;   
   double m_energyUsed        = 0.0;   
   double m_distanceTravelled = 0.0;   
   double m_lastX             = 0.0;   
   double m_lastY             = 0.0;   
};