#ifndef WALKMODULEACTIVITYCONTROLPERCENT_H
#define WALKMODULEACTIVITYCONTROLPERCENT_H

#include "WalkModule.h"
#include "WalkStateHeading.h"

#include "../../../../geo/Heading.h"
#include "../../../../math/Distributions.h"
#include "../../../../sensors/pressure/ActivityButterPressurePercent.h"
#include "../../../../grid/GridNode.h"


/** favor z-transitions */
template <typename Node, typename WalkState, typename Control> class WalkModuleActivityControlPercent : public WalkModule<Node, WalkState> {

private:

    Control* ctrl;

public:

	/** ctor */
    WalkModuleActivityControlPercent(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;
	}

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

		(void) state;
		(void) startNode;


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

        //floor and doors
        if(potentialNode.getType() == 0 || potentialNode.getType() == 3){
            return ctrl->activityPercent.stay;
        }

        //stairs
        if(potentialNode.getType() == 1){

//            if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.stairsDown +  ctrl->barometer.actProbs.stairsUp){
//                return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp);
//            }

            if (deltaZ_cm > 0){return ctrl->activityPercent.stairsDown;}
            if (deltaZ_cm < 0){return ctrl->activityPercent.stairsUp;}
            return (ctrl->activityPercent.stairsDown > ctrl->activityPercent.stairsUp ? ctrl->activityPercent.stairsDown : ctrl->activityPercent.stairsUp);
        }

        //elevators
        if(potentialNode.getType() == 2){

//            //we need to do this, that particles are able to walk into an elevator even if the prob for that is low,
//            //that happens often since the activity has some delay.
//            if(ctrl->barometer.actProbs.stay > ctrl->barometer.actProbs.elevatorDown +  ctrl->barometer.actProbs.elevatorUp){
//                return ctrl->barometer.actProbs.stay * 0.75;//(ctrl->barometer.actProbs.stairsDown > ctrl->barometer.actProbs.stairsUp ? ctrl->barometer.actProbs.stairsDown : ctrl->barometer.actProbs.stairsUp);
//            }

            if (deltaZ_cm > 0){return ctrl->activityPercent.elevatorDown;}
            if (deltaZ_cm < 0){return ctrl->activityPercent.elevatorUp;}

            //for walking out of the elevator
            return (ctrl->activityPercent.elevatorDown > ctrl->activityPercent.elevatorUp ? ctrl->activityPercent.elevatorDown : ctrl->activityPercent.elevatorUp);
        }

        std::cout << "Node has unknown Type" << std::endl;
        return 1.0;
    }
};

#endif // WALKMODULEACTIVITYCONTROLPERCENT_H