#ifndef WALKMODULEACTIVITYCONTROL_H
#define WALKMODULEACTIVITYCONTROL_H

#include "WalkModule.h"

#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"

#include "../../../../sensors/pressure/ActivityButterPressure.h"


/**
 * use the currently detected activity (stay-on-floor, walk-up, walk-down, ..)
 * from the system's control data to favor edges that resemble this activity
 */
template <typename Node, typename WalkState, typename Control> class WalkModuleActivityControl : public WalkModule<Node, WalkState> {

private:

	const Control* ctrl;

public:

	/** ctor */
	WalkModuleActivityControl(const Control* ctrl) : ctrl(ctrl) {
		;
	}

	virtual void updateBefore(WalkState& state, const Node& startNode) override {
		(void) state;
		(void) startNode;
	}

	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {
		(void) state;
		(void) startNode;
		(void) endNode;

	}

	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
		(void) state;
		(void) curNode;
		(void) nextNode;
	}

//	static double getProbability(const Node& lastNode, const Node& nextNode, const ActivityButterPressure::Activity activity) {

//		switch (activity) {
//		    case ActivityButterPressure::Activity::DOWN:
//			    if (deltaZ_cm < 0)	{return 0.85;}
//				if (deltaZ_cm == 0)	{return 0.10;}
//		                            {return 0.05;}
//		    case ActivityButterPressure::Activity::UP:
//			    if (deltaZ_cm > 0)	{return 0.85;}
//				if (deltaZ_cm == 0)	{return 0.10;}
//		                            {return 0.05;}
//		    case ActivityButterPressure::Activity::STAY:
//			    if (deltaZ_cm == 0) {return 0.85;}
//		                            {return 0.15;}
//		    default:
//			    throw Exception("not yet implemented");
//		}

//		return 1.0;

//	}

	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {

		(void) state;
		(void) startNode;

		const int deltaZ_cm = potentialNode.z_cm - curNode.z_cm;

		// TODO: general activity enum and activity-detector based on barometer and accelerometer?
		const ActivityButterPressure::Activity activity = ctrl->activity;

//		const float kappa = 0.75;

		switch (activity) {
			case ActivityButterPressure::Activity::DOWN:
			    if (deltaZ_cm < 0)	{return 0.60;}
				if (deltaZ_cm == 0)	{return 0.25;}
		                            {return 0.15;}
			case ActivityButterPressure::Activity::UP:
			    if (deltaZ_cm > 0)	{return 0.60;}
				if (deltaZ_cm == 0)	{return 0.25;}
		                            {return 0.15;}
			case ActivityButterPressure::Activity::STAY:
			    if (deltaZ_cm == 0) {return 0.60;}
		                            {return 0.40;}
//			case ActivityButterPressure::Activity::DOWN:
//			case ActivityButterPressure::Activity::UP:
//				if (potentialNode.getType() == GridNode::TYPE_STAIR)	{return kappa;}
//				if (potentialNode.getType() == GridNode::TYPE_ELEVATOR)	{return kappa;}
//																		{return 1-kappa;}
//			case ActivityButterPressure::Activity::STAY:
//				if (potentialNode.getType() == GridNode::TYPE_DOOR)		{return kappa;}
//				if (potentialNode.getType() == GridNode::TYPE_FLOOR)	{return kappa;}
//																		{return 1-kappa;}
			default:
			    throw Exception("not yet implemented activity within WalkModuleActivityControl::getProbability");
		}

	}


};

#endif // WALKMODULEACTIVITYCONTROL_H