TrawlDoorControlledLift.h

#pragma once
 
#include "trawl/subroutines/PidWithExternalState.h"
 
#include <fhsim/simobject/SimObject.h>
#include <fhsim_environment/EnvironmentProvider.h>
#include <fhsim_environment/SeafloorForcesInterface.h>
#include <string>
#include <vector>
 
class TrawlDoorControlledLift : public SimObject
{
public:
   TrawlDoorControlledLift(const std::string& simObjectName, ISimObjectCreator* creator);
 
   void OdeFcn(const double T, const double* const X, double* const XDot) const override;
   void FinalSetup(const double T, const double* const X, ISimObjectCreator* const creator) override;
   void AcceptedStep(const double T, const double* const X) override;
 
#ifdef FH_VISUALIZATION
   void RenderInit(Ogre::Root* const ogreRoot, ISimObjectCreator* const creator);
   void RenderUpdate(const double T, const double* const X);
#endif
 
protected:
   enum class WeightModel
   {
      Controlled, 
      Mass,       
   };
 
   TrawlDoorControlledLift(const std::string& simObjectName, ISimObjectCreator* creator, WeightModel weightModel);
 
   double Mass(const double* const X) const;
 
   struct Loads
   {
      double lift;
      double drag;
      double weight;
      double seafloor[3];
      double total[3];
      double spreadError;
      double bottomForceError;
   };
 
   Loads ComputeLoads(const double T, const double* const X) const;
 
   const double* OutPos(const double T, const double* const X, const int index);
   const double* OutVel(const double T, const double* const X, const int index);
   const double* OutLift(const double T, const double* const X);
   const double* OutDrag(const double T, const double* const X);
   const double* OutWeight(const double T, const double* const X);
   const double* OutBottomContact(const double T, const double* const X);
 
   WeightModel m_weightModel;    
   bool   m_isPort;              
   bool   m_useWidthMeasurement; 
   double m_mass;                
   double m_massMin;             
   double m_massMax;             
   double m_liftDragRatio;       
   double m_size;                
 
   environment::SeafloorForcesInterface* m_seafloorForces = nullptr;
   environment::EnvironmentProvider*     m_environment    = nullptr;
 
   std::vector<ISignalPort*> m_inForces;
   ISignalPort*              m_inDistanceGoal;
   ISignalPort*              m_inDistance;
   ISignalPort*              m_inBottomForceGoal = nullptr; 
   ISignalPort*              m_inMassGoal        = nullptr; 
 
   int m_IStatePos;
   int m_IStateVel;
   int m_IStateIntegratedSpreadError;
   int m_IStateIntegratedBottomForceError = -1; 
   int m_IStateMass                       = -1; 
 
   PidWithExternalState m_distanceController; 
   PidWithExternalState m_weightController;   
 
   double m_outLift          = 0.0;
   double m_outDrag          = 0.0;
   double m_outWeight        = 0.0;
   double m_outBottomContact = 0.0;
 
#ifdef FH_VISUALIZATION
   Ogre::SceneNode* m_renderNode = nullptr;
#endif
};