//  created:    2011/05/01
//  filename:   CircleFollower.h
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    demo cercle avec optitrack
//
//
/*********************************************************************/

#ifndef CIRCLEFOLLOWER_H
#define CIRCLEFOLLOWER_H

#include <UavStateMachine.h>

namespace framework {
    namespace gui {
        class PushButton;
    }
    namespace filter {
        class TrajectoryGenerator2DCircle;
    }
    namespace meta {
        class MetaVrpnObject;
    }
}

class CircleFollower : public framework::meta::UavStateMachine {
    public:
        CircleFollower(framework::meta::Uav* uav);
        ~CircleFollower();

    private:
        enum class BehaviourMode_t {
            Default,
            PositionHold,
            Circle
        };

        BehaviourMode_t behaviourMode;
        bool vrpnLost;

        void VrpnPositionHold(void);//flight mode
        void StartCircle(void);
        void StopCircle(void);
        void ExtraSecurityCheck(void);
        void ExtraCheckPushButton(void);
        void ExtraCheckJoystick(void);
        const framework::core::AhrsData *GetOrientation(void) const;
        void AltitudeValues(float &z,float &dz);
        void PositionValues(framework::core::Vector2D &pos_error,framework::core::Vector2D &vel_error,float &yaw_ref);
        framework::core::AhrsData *GetReferenceOrientation(void);
        void SignalEvent(Event_t event);

        framework::filter::Pid *uX, *uY;

        framework::core::Vector2D posHold;
        float yawHold;

        framework::gui::PushButton *startCircle,*stopCircle;
        framework::meta::MetaVrpnObject *targetVrpn;
        framework::filter::TrajectoryGenerator2DCircle *circle;
        framework::core::AhrsData *customReferenceOrientation,*customOrientation;
};

#endif // CIRCLEFOLLOWER_H