// Walk and kick: Sensor variables
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>
 
 // Ensure header is only included once
#ifndef WAK_SENSOR_VARS_H
#define WAK_SENSOR_VARS_H

// Includes
#include <walk_and_kick/wak_common.h>
#include <walk_and_kick/wak_config.h>
#include <walk_and_kick/wak_ros_interface.h>
#include <walk_and_kick/wak_gc_vars.h>
#include <walk_and_kick/wak_tc_vars.h>
#include <walk_and_kick/wak_utils.h>
#include <rc_utils/math_funcs.h>
#include <rc_utils/ros_time.h>

// Walk and kick namespace
namespace walk_and_kick
{
	/**
	* @class SensorVars
	* 
	* @brief An interface class for encapsulating all of the ROS input data to the walk and kick node.
	* 
	* This class is generated by WAKRosInterface based on the ROS inputs to the walk and kick node,
	* and used by the remainder of the walk and kick code.
	**/
	class SensorVars
	{
	public:
		//
		// General
		//

		// Constructor
		SensorVars(WAKConfig& config, WAKRosInterface& RI);

		// Config parameters
		WAKConfig& config;

		// Field dimensions
		const FieldDimensions field;

		// Update function
		void update(const ros::Time& now);

		//
		// Sensor variables
		//

		// Current cycle ROS time
		ros::Time now;

		// Robot state variables
		float timeStanding;      // The duration for which the robot has been in the standing state (zero if not standing right now)

		// Game controller variables
		GCVars GC;               // Note: All uses of this variable should be preceded by checking listenToGC
		bool GCDataFresh;        // Boolean flag whether the game controller base data is fresh
		bool GCExtraDataFresh;   // Boolean flag whether the game controller extra data is fresh
		bool listenToGC;         // Boolean flag whether the robot should listen to the game controller

		// Configuration variables
		bool isPenaltyShoot;     // Boolean flag whether the game is currently a penalty shootout, as opposed to normal soccer gameplay
		bool isPenaltyTaker;     // Boolean flag whether, assuming the game is a penalty shootout, we are the attacker
		KickoffType kickoffType; // Kickoff type (e.g. KT_ATTACK, KT_DEFEND, KT_DROPBALL, KT_MANUAL)
		bool playOnYellow;       // Boolean flag whether the robot should play on the positive yellow goal
		int goalSign;            // Sign of the goal the robot should play on (+1 = Positive yellow goal, -1 = Negative blue goal)
		bool playAsCyan;         // Boolean flag whether the robot is playing as the cyan team

		// Play state variables
		GameCommand gameCommand; // Game command (e.g. CMD_PLAY, CMD_STOP, CMD_POSE)
		bool gameCommandIsNew;   // Boolean flag whether the game command is new in this cycle
		GameRole gameRole;       // Game role (e.g. ROLE_FIELDPLAYER, ROLE_GOALIE)
		PlayState playState;     // Play state (e.g. PS_STOP, PS_READY, PS_SET, PS_BEGIN_PLAY, PS_PLAY)
		bool playStateIsNew;     // Boolean flag whether the play state is new in this cycle (true also for example if the play state didn't change, but the game command is new)
		float timePlaying;       // Time since the play state was last *not* PS_BEGIN_PLAY or PS_PLAY

		// Localisation variables
		float compassHeading;    // Heading of the robot measured by the compass in the range (-pi,pi] (0 is towards the positive yellow goal and CCW from there is positive)
		float robotPoseConf;     // Current confidence of the robot's estimated position on the field (always current)
		bool haveRobotPose;      // Boolean flag whether we are currently localised (always current)
		bool robotPoseIsNew;     // Boolean flag whether haveRobotPose turned from false to true in this cycle (always current)
		float robotPoseTimeAgo;  // Time since the robot pose was last valid (last valid)
		float robotPoseDur;      // Duration for which the robot has been/was localised the current/last time the robot was localised (last valid)
		Vec3f robotPose;         // Global position and orientation of the robot on the field, where x points to the positive yellow goal, y to the left, and z is a CCW rotation relative to the direction of the positive goal (last valid)
		Vec2f robotPose2D;       // Global position of the robot on the field, where x points to the positive yellow goal, and y to the left (last valid)

		// Ball variables
		float ballConf;          // Current confidence of the ball (always current)
		bool haveBall;           // Boolean flag whether we currently have a ball (always current)
		bool ballIsNew;          // Boolean flag whether haveBall turned from false to true in this cycle (always current)
		float ballTimeAgo;       // Time since the ball was last valid (last valid)
		float ballDur;           // Duration for which the ball has been/was valid the current/last time it was valid (last valid)
		Vec2f ballDir;           // Egocentric vector from the robot to the ball (last valid)
		float ballAngle;         // Egocentric angle to the ball, measured CCW from straight ahead (last valid)
		float ballDist;          // Distance from the robot to the ball (last valid)
		bool ballStable;         // Boolean flag whether the ball is currently close and valid, and has been valid for long enough that it is considered to be stable (always current)
		bool haveBallPose;       // Boolean flag whether we currently have a ball pose (always current)
		bool ballPoseIsNew;      // Boolean flag whether haveBallPose turned from false to true in this cycle (always current)
		float ballPoseTimeAgo;   // Time since the ball pose was last valid (last valid)
		float ballPoseDur;       // Duration for which the ball pose has been/was valid the current/last time it was valid (last valid)
		Vec2f ballPose;          // Global position of the ball on the field (last valid)
		bool ballPoseStable;     // Boolean flag whether the ball pose is currently close and valid, and has been valid for long enough that it is considered to be stable (always current)

		// Goal variables
		GoalPostList goalPosts;  // List of currently detected goal posts, exactly as published by the vision

		// Obstacle variables
		ObstacleList obstacles;  // List of currently detected obstacles, exactly as published by the vision
		Obstacle obstClosest;    // The closest detected obstacle to the robot

		// Extended play state variables
		bool ballHasMoved;       // Boolean flag whether the ball has moved away from the field centre since the kickoff
		bool ballInPlay;         // Boolean flag whether the ball is in play
		bool directGoalAllowed;  // Boolean flag whether a direct kick of the ball into the goals is allowed and will result in a score

		// Team communication variables
		TCVars TC;               // Note: All uses of this variable should be preceded by checking listenToTC
		bool TCDataAvailable;    // Boolean flag whether fresh team communications data is available from at least one robot
		bool listenToTC;         // Boolean flag whether the robot should listen to the team communications

		// Miscellaneous variables
		DiveDirection diveDecision;

		//
		// Helper functions
		//

		// Functions to check the role of the robot
		bool isInvalidRole() const { return !gameRoleValid(gameRole); }
		bool isFieldPlayer() const { return gameRoleIsFieldPlayer(gameRole); }
		bool isGoalie() const { return gameRoleIsGoalie(gameRole); }

		// Robot state functions
		bool isRelaxed() const { return RI.stateRelaxed(); }
		bool isIniting() const { return RI.stateIniting(); }
		bool isStanding() const { return RI.stateStanding(); }
		bool isWalking() const { return RI.stateWalking(); }
		bool isKicking() const { return RI.stateKicking(); }
		bool isFallen() const { return RI.stateFallen(); }

	private:
		// ROS interface
		WAKRosInterface& RI;

		// Play state set function
		void setPlayState(PlayState state, const ros::Time& toldTime = ros::Time());

		// Play state timeout management functions
		void clearPlayStateTimeout() { rc_utils::zeroRosTime(m_playStateTimeout); }
		void setPlayStateTimeout(const ros::Time& timeout) { m_playStateTimeout = timeout; }
		bool havePlayStateTimeout() const { return !m_playStateTimeout.isZero(); }
		bool playStateWouldTimeOut(const ros::Time& timeout) const { return (!timeout.isZero() && now >= timeout); }
		bool playStateTimedOut() const { return playStateWouldTimeOut(m_playStateTimeout); }

		// Play state timeout condition functions
		bool timeoutTimeout(float timeSinceLastTold) const { return (timeSinceLastTold >= config.gcTimeoutTimeoutLast()); }
		bool timeoutReady() const { return (kickoffType == KT_MANUAL || !havePlayStateTimeout() || playStateTimedOut()); }
		bool timeoutSet(float timeSinceFirstIn, float timeSinceFirstTold, float timeSinceLastTold) const;
		bool timeoutBeginPlay(float timeSinceFirstTold) const { return (listenToGC || kickoffType == KT_MANUAL || timeSinceFirstTold >= rc_utils::coerceMin(config.sWaitTime() - config.waitTimeReduction(), 0.0f)); }

		// Play state variables
		ros::Time m_playStateFirstIn;
		ros::Time m_playStateFirstTold;
		ros::Time m_playStateLastTold;
		ros::Time m_playStateTimeout;
		ros::Time m_lastTimeNotPlaying;

		// Internal localisation variables
		ros::Time m_robotPoseTimeFirst;
		ros::Time m_robotPoseTimeLast;

		// Internal ball variables
		ros::Time m_ballTimeFirst;
		ros::Time m_ballTimeLast;
		ros::Time m_ballPoseTimeFirst;
		ros::Time m_ballPoseTimeLast;

		// Robot state variables
		int m_standingCount;

		// Ball has moved detection variables
		TheWorm m_ballHasMovedWorm;

		// Miscellaneous variables
		cycle_t m_updateCycle;
	};
}

#endif
// EOF