// Capture step gait
// File: cap_gait.h
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Ensure header is only included once
#ifndef CAP_GAIT_H
#define CAP_GAIT_H

// Includes - Local
#include <cap_gait/cap_gait_config.h>
#include <cap_gait/cap_com_filter.h>

// Includes - Gait
#include <gait/gait_common.h>
#include <gait/gait_engine.h>
#include <gait/util/gait_joint_pose.h>
#include <gait/util/gait_inverse_pose.h>
#include <gait/util/gait_abstract_pose.h>

// Includes - Misc
#include <plot_msgs/plot_manager.h>
#include <rc_utils/ew_integrator.h>
#include <rc_utils/mean_filter.h>
#include <rc_utils/wlbf_filter.h>
#include <rc_utils/wlbf_filter_nd.h>
#include <rc_utils/hold_filter.h>
#include <feed_gait/model/tilt_phase/tripendulum_model.h>

// Includes - Library
#include <Eigen/Core>

// Includes - Contrib [These are included last because they potentially contain Qt includes that break the Boost signals library... (e.g. used by TF)]
#include <cap_gait/contrib/RobotModel.h>
#include <cap_gait/contrib/RobotModelVis.h>
#include <cap_gait/contrib/LimpModel.h>
#include <cap_gait/contrib/Limp.h>
#include <cap_gait/contrib/ComFilter.h>

/**
* @namespace cap_gait
*
* @brief This namespace defines everything that is required for the capture step gait.
**/
namespace cap_gait
{
	/**
	* @class CapGait
	*
	* @brief A capture step gait, implemented as a `GaitEngine` plugin for the `Gait` motion module.
	**/
	class CapGait : public gait::GaitEngine
	{
	public:
		//
		// Public functions
		//

		// Constructor
		CapGait(); //!< Default constructor

		// Reset function
		virtual void reset();

		// Step function
		virtual void step();

		// Halt pose function
		virtual void updateHaltPose();

		// Set odometry function
		virtual void setOdometry(double posX, double posY, double rotZ);

		// Update odometry function
		virtual void updateOdometry();

		// Handle joystick button function
		virtual void handleJoystickButton(int button);

		// Constants
		static const std::string RESOURCE_PATH;
		static const std::string CONFIG_PARAM_PATH;

	private:
		//
		// Structs
		//
		
		// Common motion data struct
		struct CommonMotionData
		{
			// Constructor
			CommonMotionData() // All data that is initialised here is just to avoid divisions by zero etc in case a bug causes an uninitialised value in this struct to be used by accident
			 : gcvX(0.0)
			 , gcvY(0.0)
			 , gcvZ(0.0)
			 , absGcvX(0.0)
			 , absGcvY(0.0)
			 , absGcvZ(0.0)
			 , gaitPhase(0.0)
			 , oppGaitPhase(M_PI)
			 , limbPhase(0.0)
			 , absPhase(0.0)
			 , doubleSupportPhase(0.1)
			 , swingStartPhase(0.0)
			 , swingStopPhase(M_PI)
			 , suppTransStartPhase(0.0)
			 , suppTransStopPhase(doubleSupportPhase)
			 , liftingPhaseLen(M_PI - doubleSupportPhase)
			 , suppPhaseLen(M_PI + doubleSupportPhase)
			 , nonSuppPhaseLen(M_2PI - suppPhaseLen)
			 , sinusoidPhaseLen(M_PI)
			 , linearPhaseLen(M_PI)
			 , swingAngle(0.0)
			{}
			
			// Gait command vector variables
			double gcvX;
			double gcvY;
			double gcvZ;
			double absGcvX;
			double absGcvY;
			double absGcvZ;
			
			// Gait phase variables
			double gaitPhase;
			double oppGaitPhase;
			double limbPhase;
			double absPhase;
			
			// Phase marks
			double doubleSupportPhase;
			double swingStartPhase;
			double swingStopPhase;
			double suppTransStartPhase;
			double suppTransStopPhase;
			
			// Extra swing variables
			double liftingPhaseLen;
			double suppPhaseLen;
			double nonSuppPhaseLen;
			double sinusoidPhaseLen;
			double linearPhaseLen;
			
			// Swing angle
			double swingAngle;
		};
		
		//
		// Private functions
		//

		// Reset walking function (a lighter version of reset(), used for internal resets when starting and stopping walking)
		void resetWalking(bool walking, const Eigen::Vector3d& gcvBias);

		// Input processing
		void processInputs();
		void updateRobot(const Eigen::Vector3d& gcvBias);

		// Helper functions
		CommonMotionData calcCommonMotionData(bool isFirst) const;

		// Motion functions
		void abstractLegMotion(gait::AbstractLegPose& leg);
		void abstractArmMotion(gait::AbstractArmPose& arm);
		void inverseLegMotion(gait::InverseLegPose& leg);
		
		// Coercion functions
		void coerceAbstractPose(gait::AbstractPose& pose);
		void coerceAbstractArmPose(gait::AbstractArmPose& arm);
		void coerceAbstractLegPose(gait::AbstractLegPose& leg);

		// Output processing
		void updateOutputs();

		// Blending functions
		void resetBlending(double b = USE_HALT_POSE);
		void setBlendTarget(double target, double phaseTime);
		double blendFactor();

		// Motion stance functions
		void resetMotionStance();

		//
		// Gait variables
		//

		// Constants
		static const double USE_HALT_POSE;
		static const double USE_CALC_POSE;

		// Gait configuration struct
		CapConfig config;

		// Pose variables
		gait::JointPose m_jointPose;           // Joint representation of the pose to command in a step
		gait::JointPose m_jointHaltPose;       // Joint representation of the gait halt pose
		gait::JointPose m_lastJointPose;       // The last joint pose to have been commanded during walking
		gait::InversePose m_inversePose;       // Inverse representation of the pose to command in a step
		gait::AbstractPose m_abstractPose;     // Abstract representation of the pose to command in a step
		gait::AbstractPose m_abstractHaltPose; // Abstract representation of the gait halt pose

		// Gait command vector variables
		Eigen::Vector3d m_gcv;      // Gait command vector (slope-limited command velocities actually followed by the gait engine)
		Eigen::Vector3d m_gcvInput; // Gait command vector input (raw velocities commanded by the gait motion module)
		rc_utils::GolayDerivative<Eigen::Vector3d, 1, 5, Eigen::aligned_allocator<Eigen::Vector3d> > m_gcvDeriv; // Derivative filter for the GCV to calculate the gait acceleration
		rc_utils::MeanFilter m_gcvAccSmoothX;
		rc_utils::MeanFilter m_gcvAccSmoothY;
		rc_utils::MeanFilter m_gcvAccSmoothZ;
		Eigen::Vector3d m_gcvAcc;

		// Gait flags
		bool m_walk;                // True if the gait engine should walk, false if it should not walk
		bool m_walking;             // True if the gait engine is currently producing a walking motion output
		bool m_leftLegFirst;        // True if the left leg should be the first to be lifted when starting walking

		// Step motion variables
		double m_gaitPhase;         // Current walking phase of the gait motion

		// Blending variables
		bool m_blending;
		double m_b_current;
		double m_b_initial;
		double m_b_target;
		double m_blendPhase;
		double m_blendEndPhase;
		
		// Motion stance variables
		double m_motionLegAngleXFact; // Interpolation factor between feet narrow (= 0) and feet normal (= 1) legAngleX values
		
		//
		// Basic feedback variables
		//
		
		// Basic feedback filters
		rc_utils::MeanFilter fusedXFeedFilter;
		rc_utils::MeanFilter fusedYFeedFilter;
		rc_utils::WLBFFilter dFusedXFeedFilter;
		rc_utils::WLBFFilter dFusedYFeedFilter;
		rc_utils::MeanFilter iFusedXFeedFilter;
		rc_utils::MeanFilter iFusedYFeedFilter;
		rc_utils::WLBFFilter gyroXFeedFilter;
		rc_utils::WLBFFilter gyroYFeedFilter;
		
		// Integrators
		rc_utils::EWIntegrator iFusedXFeedIntegrator;
		rc_utils::EWIntegrator iFusedYFeedIntegrator;
		config_server::Parameter<bool> m_resetIntegrators;    // Rising edge triggered flag to reset any integrated or learned values in the gait that are not necessarily reset during start/stop of walking
		config_server::Parameter<bool> m_saveIFeedToHaltPose; // Rising edge triggered flag to save the current integrated feedback values as offsets to the halt pose (only the ones in current use)
		bool m_savedLegIFeed;   // Flag that specifies within a cycle whether the integrated leg feedback has already been saved
		bool m_savedArmIFeed;   // Flag that specifies within a cycle whether the integrated arm feedback has already been saved
		double iFusedXLastTime; // The last in.timestamp where iFusedX made a non-zero contribution to the CPG gait
		double iFusedYLastTime; // The last in.timestamp where iFusedY made a non-zero contribution to the CPG gait
		bool haveIFusedXFeed;   // Boolean flag whether the iFusedX integrator had any effect on iFusedXFeed in the current cycle
		bool haveIFusedYFeed;   // Boolean flag whether the iFusedY integrator had any effect on iFusedYFeed in the current cycle
		bool usedIFusedX;       // Boolean flag whether the iFusedX integrator had any effect on the produced joint commands in the current cycle
		bool usedIFusedY;       // Boolean flag whether the iFusedY integrator had any effect on the produced joint commands in the current cycle
		void resetIntegrators();
		void resetSaveIntegrals();
		
		// Callbacks for updating the filter sizes
		void resizeFusedFilters (int numPoints) { if(numPoints < 1) numPoints = 1; fusedXFeedFilter.resize(numPoints);  fusedYFeedFilter.resize(numPoints);  }
		void resizeDFusedFilters(int numPoints) { if(numPoints < 1) numPoints = 1; dFusedXFeedFilter.resize(numPoints); dFusedYFeedFilter.resize(numPoints); }
		void resizeIFusedFilters(int numPoints) { if(numPoints < 1) numPoints = 1; iFusedXFeedFilter.resize(numPoints); iFusedYFeedFilter.resize(numPoints); }
		void resizeGyroFilters  (int numPoints) { if(numPoints < 1) numPoints = 1; gyroXFeedFilter.resize(numPoints);   gyroYFeedFilter.resize(numPoints);   }
		
		// Basic feedback values
		double fusedXFeed;
		double fusedYFeed;
		double dFusedXFeed;
		double dFusedYFeed;
		double iFusedXFeed;
		double iFusedYFeed;
		double gyroXFeed;
		double gyroYFeed;

		// Basic step size adaptation
		void resetStepSize();
		void configStepSize();
		double updateStepSize(double fusedX, double fusedY);
		rc_utils::WLBFFilter2D syncPhaseFilter;
		tripendulum::TriPendModel stepTriPendModelSag;
		rc_utils::HoldMinFilter stepGcvXBHoldFilter;
		rc_utils::HoldMaxFilter stepGcvXFHoldFilter;

		//
		// Capture step variables
		//

		// Reset function
		void resetCaptureSteps(bool resetRobotModel);

		// Capture step robot model
		margait_contrib::RobotModel rxRobotModel;
		margait_contrib::RobotModelVis m_rxVis;
		config_server::Parameter<bool> m_showRxVis;
		void callbackShowRxVis();

		// Linear inverted pendulum robot models
		margait_contrib::LimpModel rxModel;
		margait_contrib::LimpModel mxModel;
		margait_contrib::LimpModel txModel;

		// Linear inverted pendulum model class for isolated calculations
		margait_contrib::Limp limp;

		// CoM filter
		ComFilter<5> m_comFilter;

		// Miscellaneous
		config_server::Parameter<float> m_gcvZeroTime;
		margait_contrib::Vec2f adaptation;
		double lastSupportOrientation;
		double oldGcvTargetY;
		double virtualSlope;
		double stepTimeCount;
		double lastStepDuration;
		int stepCounter;
		int noCLStepsCounter;
		int resetCounter;
		int cycleNumber;

		//
		// Plotting
		//

		// Note: To add a variable for plotting you need to do the following...
		// 1) Add an enum value below ==> PM_MODEL_MY_DATA,
		// 2) Set the name of the plotted data in configurePlotManager() ==> m_PM.setName(PM_MODEL_MY_DATA, "model/myData");
		// 3) Actually plot the required data in the function where you calculate it ==> m_PM.plotScalar(myData, PM_MODEL_MY_DATA);
		// It is recommended to enclose the plotScaler() call in ==> if(m_PM.getEnabled()) { ... }

		// Plot manager
		config_server::Parameter<bool> m_plotData; 
		plot_msgs::PlotManagerFS m_PM;
		void configurePlotManager();
		void callbackPlotData();
		enum PMIDS
		{
			PM_GCV = 0,
			PM_GCV_X = PM_GCV,
			PM_GCV_Y,
			PM_GCV_Z,
			PM_GCV_ACC,
			PM_GCV_ACC_X = PM_GCV_ACC,
			PM_GCV_ACC_Y,
			PM_GCV_ACC_Z,
			PM_GAIT_PHASE,
			PM_USED_IFUSEDX,
			PM_USED_IFUSEDY,
			PM_HALT_BLEND_FACTOR,
			PM_LEG_EXTENSION_R,
			PM_LEG_EXTENSION_L,
			PM_LEG_SWING_ANGLE_R,
			PM_LEG_SWING_ANGLE_L,
			PM_LEG_SAG_SWING_R,
			PM_LEG_SAG_SWING_L,
			PM_LEG_LAT_SWING_R,
			PM_LEG_LAT_SWING_L,
			PM_LEG_ROT_SWING_R,
			PM_LEG_ROT_SWING_L,
			PM_LEG_LAT_HIP_SWING_R,
			PM_LEG_LAT_HIP_SWING_L,
			PM_LEG_SAG_LEAN_R,
			PM_LEG_SAG_LEAN_L,
			PM_LEG_LAT_LEAN_R,
			PM_LEG_LAT_LEAN_L,
			PM_LEG_FEED_HIPANGLEX_R,
			PM_LEG_FEED_HIPANGLEX_L,
			PM_LEG_FEED_HIPANGLEY_R,
			PM_LEG_FEED_HIPANGLEY_L,
			PM_LEG_FEED_FOOTANGLEX_R,
			PM_LEG_FEED_FOOTANGLEX_L,
			PM_LEG_FEED_FOOTANGLEY_R,
			PM_LEG_FEED_FOOTANGLEY_L,
			PM_LEG_FEED_FOOTANGLECX_R,
			PM_LEG_FEED_FOOTANGLECX_L,
			PM_LEG_FEED_FOOTANGLECY_R,
			PM_LEG_FEED_FOOTANGLECY_L,
			PM_LEG_ABS_LEGEXT_R,
			PM_LEG_ABS_LEGEXT_L,
			PM_LEG_ABS_LEGANGLEX_R,
			PM_LEG_ABS_LEGANGLEX_L,
			PM_LEG_ABS_LEGANGLEY_R,
			PM_LEG_ABS_LEGANGLEY_L,
			PM_LEG_ABS_LEGANGLEZ_R,
			PM_LEG_ABS_LEGANGLEZ_L,
			PM_LEG_ABS_FOOTANGLEX_R,
			PM_LEG_ABS_FOOTANGLEX_L,
			PM_LEG_ABS_FOOTANGLEY_R,
			PM_LEG_ABS_FOOTANGLEY_L,
			PM_LEG_FEED_COMSHIFTX_R,
			PM_LEG_FEED_COMSHIFTX_L,
			PM_LEG_FEED_COMSHIFTY_R,
			PM_LEG_FEED_COMSHIFTY_L,
			PM_LEG_VIRTUAL_SLOPE_R,
			PM_LEG_VIRTUAL_SLOPE_L,
			PM_LEG_VIRTUAL_COMP_R,
			PM_LEG_VIRTUAL_COMP_L,
			PM_ARM_SWING_ANGLE_R,
			PM_ARM_SWING_ANGLE_L,
			PM_ARM_SAG_SWING_R,
			PM_ARM_SAG_SWING_L,
			PM_ARM_FEED_ARMANGLEX_R,
			PM_ARM_FEED_ARMANGLEX_L,
			PM_ARM_FEED_ARMANGLEY_R,
			PM_ARM_FEED_ARMANGLEY_L,
			PM_ARM_ABS_ARMEXT_R,
			PM_ARM_ABS_ARMEXT_L,
			PM_ARM_ABS_ARMANGLEX_R,
			PM_ARM_ABS_ARMANGLEX_L,
			PM_ARM_ABS_ARMANGLEY_R,
			PM_ARM_ABS_ARMANGLEY_L,
			PM_RXRMODEL_SUPPVEC_X,
			PM_RXRMODEL_SUPPVEC_Y,
			PM_RXRMODEL_SUPPVEC_Z,
			PM_RXRMODEL_STEPVEC_X,
			PM_RXRMODEL_STEPVEC_Y,
			PM_RXRMODEL_STEPVEC_Z,
			PM_RXRMODEL_STEPVEC_FYAW,
			PM_FUSED_X,
			PM_FUSED_Y,
			PM_COMFILTER_X,
			PM_COMFILTER_Y,
			PM_COMFILTER_VX,
			PM_COMFILTER_VY,
			PM_RXMODEL_X,
			PM_RXMODEL_Y,
			PM_RXMODEL_VX,
			PM_RXMODEL_VY,
			PM_RXMODEL_SUPPLEG,
			PM_RXMODEL_TIMETOSTEP,
			PM_MXMODEL_X,
			PM_MXMODEL_Y,
			PM_MXMODEL_VX,
			PM_MXMODEL_VY,
			PM_MXMODEL_SUPPLEG,
			PM_MXMODEL_TIMETOSTEP,
			PM_MXMODEL_ZMP_X,
			PM_MXMODEL_ZMP_Y,
			PM_TXMODEL_X,
			PM_TXMODEL_Y,
			PM_TXMODEL_VX,
			PM_TXMODEL_VY,
			PM_TXMODEL_SUPPLEG,
			PM_TXMODEL_TIMETOSTEP,
			PM_TXMODEL_STEPSIZEX,
			PM_TXMODEL_STEPSIZEY,
			PM_TXMODEL_STEPSIZEZ,
			PM_ADAPTATION_X,
			PM_ADAPTATION_Y,
			PM_EXP_FUSED_X,
			PM_EXP_FUSED_Y,
			PM_DEV_FUSED_X,
			PM_DEV_FUSED_Y,
			PM_FEEDBACK_FUSED_X,
			PM_FEEDBACK_FUSED_Y,
			PM_FEEDBACK_DFUSED_X,
			PM_FEEDBACK_DFUSED_Y,
			PM_FEEDBACK_IFUSED_X,
			PM_FEEDBACK_IFUSED_Y,
			PM_FEEDBACK_GYRO_X,
			PM_FEEDBACK_GYRO_Y,
			PM_TIMING_FEED_WEIGHT,
			PM_TIMING_FREQ_DELTA,
			PM_STEPSIZE_SYNCPHASEY,
			PM_STEPSIZE_SYNCDERIVY,
			PM_STEPSIZE_THETABM,
			PM_STEPSIZE_THETAMF,
			PM_STEPSIZE_CROSSENERGY_B,
			PM_STEPSIZE_CROSSENERGY_F,
			PM_STEPSIZE_GCVDELTAX_B,
			PM_STEPSIZE_GCVDELTAX_F,
			PM_STEPSIZE_GCVDELTAX,
			PM_GAIT_FREQUENCY,
			PM_REM_GAIT_PHASE,
			PM_TIMETOSTEP,
			PM_STEPSIZE_X,
			PM_STEPSIZE_Y,
			PM_STEPSIZE_Z,
			PM_GCVTARGET_X,
			PM_GCVTARGET_Y,
			PM_GCVTARGET_Z,
			PM_LAST_STEP_DURATION,
			PM_COUNT
		};
	};
}

#endif
// EOF