#ifndef STATEMASHINE_H
#define STATEMASHINE_H

#include <Arduino.h>
#include <map>
#include <utility>
#include <vector>
#include <modules/my_mutex/Mutex.h>
#include <modules/my_math/matrix/math.hpp>
#include <modules/mavlink/v2.0/ardupilotmega/mavlink.h>
#include <modules/my_state_machine/AC_PID_Basic.h>
using namespace matrix;

class StateMachine
{
public:
    StateMachine();
    static void TaskStateMachine(void *pvParameters);
    enum{
        StateWait,
        StateReadWps,
        StateCalcWp,
        StateSendWps,
        StateNavigation,
        StateTargetAttack,
        StateTestSrv,
        StateSafeMode,
    };
    enum{
        CalcErrorNone,
        CalcErrorDist,
        CalcErrorAngle,
        CalcErrorAlt,
        CalcErrorWpNums,

    };
    enum{
        ResContinue,
        ResSwitchGuided,
        ResSendControl,
        ResSwitchRtl,
        ResSwitchCustomGuided,
        ResSwitchLoiter,
        ResSendCamPWM,
        ResSendCamPWM_Control,
        ResSwitchSafeMode,
        ResSendWpAndControl,
        ResSwitchNavigation,
    };
    
    void SetWp(mavlink_mission_item_t wp);
    mavlink_mission_item_t GetWp(uint16_t num);
    uint16_t GetWpCount(){return mWpMaxNum;}
    void SetWpCount(uint16_t n);
    void SetWpsSended(bool ok){mWpsSended=ok;}
    void SetRPY(float r,float p,float y);
    void SetPos(float lat,float lon,float alt);
    void SetCurrWpNum(uint16_t n){mCurrWpNum=n;}
    void SetBaseMode(uint8_t bm,uint8_t cm){mBaseMode=bm;mCustomMode=cm;}
    void SetTestMode(uint8_t mode,float alt){mTestMode=mode;mTestAlt=alt;}
    void SetKpitch(float kpitch){mK_Pitch=kpitch;}
    void SetK(float k){mMutex.Lock();mYawPID->kP(k);mMutex.Unlock();};
    void SetI(float k){mMutex.Lock();mYawPID->kI(k);mMutex.Unlock();};
    void SetD(float k){mMutex.Lock();mYawPID->kD(k);mMutex.Unlock();};
    void SetImax(float k){mMutex.Lock();mYawPID->imax(k);mMutex.Unlock();};
    void SetUseKpitch(bool ok){mUseKpitch=ok;}
    void SetVideoParam(float fovx,float fovy,float w,float h){mFovX=fovx;mFovY=fovy;mWidth=w;mHeight=h;mKx=fovx/w;mKy=fovy/h;}
    void SetTrackerData(float x,float y,uint8_t v,uint8_t mode,uint16_t cnt);
    void SetTrackerEnable(bool ok){mEnableTracker=ok;}
    void SetAttackSpeed(float speed){mAttackSpeed=speed;}
    void SetCamSrvCH(float ch){mMutex.Lock();mCAM_SRV_CH=ch;mMutex.Unlock();}
    void SetCamSrvMax(float max){mMutex.Lock();mCAM_SRV_MAX=max;mMutex.Unlock();}
    void SetCamSrvMin(float min){mMutex.Lock();mCAM_SRV_MIN=min;mMutex.Unlock();}
    void SetCamAngMax(float max){mMutex.Lock();mCAM_ANG_MAX=max;mMutex.Unlock();}
    void SetCamAngMin(float min){mMutex.Lock();mCAM_ANG_MIN=min;mMutex.Unlock();}
    void SetCamAngCruise(float cr){mMutex.Lock();mCAM_ANG_CRUISE=cr;mMutex.Unlock();}
    void SetFlapRC(float ch){mMutex.Lock();mFLAP_RC=ch;mMutex.Unlock();}
    void SetCTRLSrvCH(float ch){mMutex.Lock();mCTRL_SRV_CH=ch;mMutex.Unlock();}
    void SetFlapVAL(float val);
    void SetTestSrvCH(float ch){mMutex.Lock();mTEST_SRV_CH=ch;mMutex.Unlock();}
    void SetTestSrvAmp(float a){mMutex.Lock();mTEST_SRV_AMP=a;mMutex.Unlock();}
    void SetTestSrvT(float t){mMutex.Lock();mTEST_SRV_T=t;mMutex.Unlock();}
    void SetTestSrvTrim(float trim){mMutex.Lock();mTEST_SRV_TRIM=trim;mMutex.Unlock();}
    void SetLoiterAlt(float alt){mMutex.Lock();mLoiterAltMin=alt;mMutex.Unlock();}
    void SetSysID(uint8_t id){mSysID=id;}
    void SetCamReverse(uint8_t r){mCAM_SRV_REVERSE=r;}
    void SetSafeSpeed(float spd){mMutex.Lock();mSafeSpeed=spd;mMutex.Unlock();}
    void SetSafePitch(float p){mMutex.Lock();mSafePitch=p;mMutex.Unlock();}
    void SetSafeTime(float t){mMutex.Lock();mSafeTime=t;mMutex.Unlock();}
    void SetCamSrvKI(float ki){mMutex.Lock();mCAM_SRV_KI=ki;mMutex.Unlock();}
    void SetCamSrvKP(float kp){mMutex.Lock();mCAM_SRV_KP=kp;mMutex.Unlock();}
    void SetCamPitchDelta(float dp){mMutex.Lock();mCAM_PITCH_DELTA=dp;mMutex.Unlock();}
    void SetCamRateMax(float rate){mMutex.Lock();mCAM_RATE_MAX=rate*0.033f;mMutex.Unlock();}
    void SetTecsDalt(float dDlat){mMutex.Lock();mTECS_DALT=dDlat;mMutex.Unlock();}

private:
    static int8_t CalckTargetWp(void *pvParameters);
    uint8_t Correction();
    uint8_t SafeMode();
    void CreateMavlink(float r,float p,float y,float X,float Y,float Z,float mode);
    void Ang2PWM(float v);
    void CamYTrecker(float camPitch,bool on_attack);
    enum{
        AttackNone,
        AttackWp,
        AttackWpTracker,
        AttackTracker,
        CameraManual
    };
    Mutex mMutex;
    u_int8_t mState;
    uint16_t mWpMaxNum;
    uint16_t mCurWpRead=0;
    uint16_t mWpReadedCount=0;
    //bool mSystemArmed;
    //bool mSystemArmedLast;
    bool mWpsSended;
    std::map<uint16_t,mavlink_mission_item_t> mWpsMap;
    float mRoll,mPitch,mYaw;
    float mLat,mLon,mAlt;
    uint16_t mCurrWpNum=0;
    mavlink_mission_item_t WpD,TargetWp,GuidedWp,CustomGuidedWp;
    float mTanWp=0;
    float mAngleWp=0;
    int16_t mX,mY,mZ,mR;
    uint8_t mBaseMode=0;
    uint8_t mCustomMode=0;
    uint8_t mTestMode=0;
    float mTestAlt=100;
    float mQ0,mQ1,mQ2,mQ3;
    float mK_Yaw=2.0;
    float mI_Yaw=0.0;
    float mD_Yaw=0.0;
    float mI_Max=0;
    float mK_Pitch=1.5;
    float mAltError=0;
    float mDistans=0;
    uint8_t mTargetWpNum=0;
    bool mUseKpitch=0.0;
    float mFovX,mFovY,mWidth,mHeight,mKx,mKy;
    float mTrackerX=0;
    float mTrackerY=0;
    uint8_t mTrackerValid=0;
    uint8_t mTrackerMode=0;
    bool mNewTrackerData=false;
    bool mUseTracker=false;
    bool mUseCustomTrackerMode=false;
    bool mEnableTracker=false;
    float mCam_yaw=0;
    float mCam_pitch=0;
    float mPitchError=0;
    float mYawError=0;
    float mKpitcKorr=0;
    uint16_t mTrackerMsgCnt=0;
    float mLoiterAlt=100;
    float mAttackSpeed=30.0;
    float mSafeSpeed=30.0;
    float mSafePitch=0;
    float mSafeTime=10;
    uint8_t mBufferOutStream[MAVLINK_MAX_PACKET_LEN];
    bool mMavlinkNew=false;
    int mSendLen=0;
    uint8_t mAttackMode=0;
    AC_PID_Basic *mYawPID;
    uint8_t mCAM_SRV_CH=0;
    uint8_t mCTRL_SRV_CH=0;
    float mCAM_SRV_MAX;
    float mCAM_SRV_MIN;
    float mCAM_ANG_CRUISE;
    float mCAM_ANG_MAX;
    float mCAM_ANG_MIN;
    float mCAM_TARGET=-500.0f;
    float mCAM_CURRENT=-500.0f;
    float mCAM_SRV_KI=0.1;
    float mCAM_SRV_KP=0.5;
    float mCAM_PITCH_DELTA=5.0;
    float mCAM_RATE_MAX=45.0*0.033f;
    uint8_t mCAM_SRV_REVERSE=0;
    bool mNewCamData=false;
    uint16_t mCamPWM=0;
    float mRollMax=40;
    uint8_t mFLAP_RC=0;
    uint16_t mFLAP_VAL=0;
    uint16_t mFLAP_PWM=0;
    bool mUnlock=false;
    uint8_t mTEST_SRV_CH=0;
    u_int16_t mTEST_SRV_AMP=0;
    float mTEST_SRV_T=0;
    u_int16_t mTEST_SRV_TRIM=1500;
    float mLoiterAltMin=100;
    uint8_t mSysID=0;
    uint8_t mSysID_old=0;
    bool mSendPWM=false;
    bool mEnterSafeMode=true;
    float mGuidedAlt=1.0f;
    float mTECS_DALT=10.0f;
};
#endif