#ifndef WALKMODULEHEADINGVONMISES_H
#define WALKMODULEHEADINGVONMISES_H

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

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


/** keep the state's heading */
template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingVonMises : public WalkModule<Node, WalkState> {

private:

    /** van-Mises distribution */
    Distribution::VonMises<float> dist;

    /** random noise */
    Distribution::Normal<float> distNoise;

    const Control* ctrl;

public:

    /** ctor 3.0 should be OK! */
    WalkModuleHeadingVonMises(const Control* ctrl, const float sensorNoiseDegreesSigma) :
        dist(Distribution::VonMises<float>(0.0f, 2.0f)),
        distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
        ctrl(ctrl) {

        // ensure the template WalkState inherits from 'WalkStateHeading'!
        StaticAssert::AinheritsB<WalkState, WalkStateHeading>();

    }


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

        // NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
        // reset this one after every transition!
        Assert::isBetween(ctrl->turnSinceLastTransition_rad, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");

        // sensor noise
        float var = distNoise.draw();

        // stair? -> increase variance
        if (startNode.getType() == GridNode::TYPE_STAIR) {var *= 3;}

        // adjust the state's heading using the control-data
        state.heading.direction += ctrl->turnSinceLastTransition_rad + var;

        //set kappa of mises
        float kappa = 5 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad));
        dist.setKappa(kappa);

    }

    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;

        // ignore for stairs?
        //if (nextNode.getType() == GridNode::TYPE_STAIR) {return;}

        // for elevator edges [same (x,y) but different z] do not adjust anything
        if (nextNode.getType() == GridNode::TYPE_ELEVATOR)	{return;}
        if (curNode.getType() == GridNode::TYPE_ELEVATOR)	{return;}
        //if (curNode.x_cm == nextNode.x_cm && curNode.y_cm == nextNode.y_cm && curNode.z_cm != nextNode.z_cm) {return;}

        // get the heading denoted by the way from curNode to nextNode
        const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);

        // get the heading requested by the state
        const Heading stateHead = state.heading.direction;

        // get the error (signed difference) between both
        const float angularDiff = stateHead.getSignedDiff(head);

        // adjust the error.
        // note:	the error may get > +/- 2PI but this is not an issue!
        //			when the error is added to the current heading within getProbability(),
        //			it is ensured their sum is within [0:2pi]
        state.heading.error += angularDiff;

    }

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

        (void) startNode;

        // ignore for stairs?
        //if (potentialNode.getType() == GridNode::TYPE_STAIR)	{return 1.0;}

        // for elevator edges [same (x,y) but different z] just return 1
        if (potentialNode.getType() == GridNode::TYPE_ELEVATOR)	{return 1.0;}
        if (curNode.getType() == GridNode::TYPE_ELEVATOR)		{return 1.0;}
        //if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}

        // get the heading between curNode and potentialNode
        const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);

        // compare the heading against the state's heading - the last error
        const Heading stateHead = state.heading.direction + state.heading.error;

        // get the difference
        const float angularDiff = head.getDiffHalfRAD(stateHead);

        // determine probability
        return dist.getProbability(angularDiff);

    }


};

#endif // WALKMODULEHEADING_H