#ifndef MAVLINKTRANSMITTER_H
#define MAVLINKTRANSMITTER_H

#include <Arduino.h>
#include <vector>
#include <queue>
#include <map>
#include <utility>
#include <modules/my_mutex/Mutex.h>

#include <modules/mavlink/v2.0/ardupilotmega/mavlink.h>

class MavlinkTransmitter
{

public:
    enum{
        SEND_MSG_ID_COMMAND_LONG,
        SEND_MSG_ID_PARAM_VALUE,
        SEND_MSG_ID_STATUSTEXT,
        SEND_MSG_ID_REQ_MISSION_COUNT,
        SEND_MSG_ID_REQ_MISSION_ITEM,
        SEND_MSG_ID_MISSION_COUNT,
        SEND_MSG_ID_MISSION_ITEM,
        SEND_MSG_ID_MANUAL_CONTROL,
        SEND_MSG_ID_GUIDED_MODE,
        SEND_MSG_ID_ATT_TARGET,
        SEND_MSG_ID_MODE_RTL,
        SEND_MSG_ID_STREAM_RATE,
        SEND_MSG_ID_MODE_LOITER,
        SEND_MSG_ID_GUIDED_CHANGE_SPEED,
        SEND_MSG_ID_DISTANCE_SENSOR,
        SEND_MSG_ID_RC_CHANNELS_OVERRIDE,
        SEND_MSG_MAVLINK,
        SEND_MSG_GUIDED_CHANGE_ALTITUDE,
    };

    MavlinkTransmitter();
    ~MavlinkTransmitter();
    static void TaskMavlinkTx(void *pvParameters);
    void SetSysId(uint8_t id){mSysId=id;}
    void UnsubscribeMsg(uint32_t msgId);
    void SubscribeMsg(uint32_t msgId,uint32_t interval);
    void SendParameter(mavlink_param_value_t par);
    void SendText(String str,uint16_t ser);
    void SendMissionReq();
    void SendRequestMissionItem(uint16_t n);
    void SetWps(std::map<uint16_t,mavlink_mission_item_t> map);
    void SendMissionCount();
    void SendMissionItem(uint16_t num);
    void SendManualCoontrool(int16_t x,int16_t y,int16_t z,int16_t r);
    void SendGuidedMode(uint8_t base_mode,mavlink_mission_item_t wpg);
    void SendGuidedControll(float q0,float q1,float q2,float q3,float Alt,bool sendAlt);
    void SendRtl();
    void SendStreamRateAll();
    void SendLoiter(float alt);
    void SendGuidedSpeed(float speed);
    void SendDistanceSensor(float distance, bool isValid, float maxDist);
    void SendCamPWM(uint8_t n,u_int16_t pwm);
    void SendFlapPWM(uint8_t n,u_int16_t pwm);
    void SendMavlink(uint8_t *p,u_int16_t len);
    void SendLoiterSpeed(float spd);
private:
    void SendHearbit();
    bool SendFromQueue();
    void SendCmdLong();
    void SendParams();
    void SendStatusText();
    void SendReqMissionCount();
    void SendReqMissionItem();
    void SendCurrentMissionCount();
    void SendNextMissionItem();
    void SendManualCoontrool();
    void SendGuidedMode();
    void SendLoiterWp();
    void SendGuidedControll();
    void SendRtlMode();
    void SendStreamRate();
    void SendLoiterMode();
    void SendGuidedChangeSpeed();
    void SendDistanceSensorUpdate();
    void SendFlapOverride();
    void SendMsgMavlink();
    void SendGuidedAlt();
    Mutex mMutex;
    uint8_t mBufferOut[MAVLINK_MAX_PACKET_LEN];
    uint8_t mBufferCmd[MAVLINK_MAX_PACKET_LEN];
    uint16_t mMavMsgLen=0;
    uint32_t mMsgLen=0;
    HardwareSerial *mSerial;
    std::queue<uint32_t> mQueueMsgId;
    std::queue<mavlink_command_long_t> mQueueCmdLong;
    std::queue<mavlink_param_value_t> mQueueParams;
    std::queue<std::pair<String,uint16_t>> mQueueText;
    std::map<uint16_t,mavlink_mission_item_t> mMissionMap;
    uint8_t mSysId=0;
    uint16_t mItemNum=0;
    mavlink_message_t msg;
    uint16_t mCurrentItemSend=0;
    int16_t mX,mY,mZ,mR;
    mavlink_mission_item_t mWpGuided;
    uint8_t mBaseMode;
    mavlink_set_attitude_target_t att_target;
    uint32_t mCurrentTick=0;
    uint32_t mLastTick=0;
    float mLoiterAlt=100;
    float mGuidedSpeed=30.0;
    float mLoiterSpeed=30.0;
    uint32_t mCurrMsgId=0;
    float mRangefinderDistance=0.0;
    float mRangefinderMax = 0;
    bool mRangefinderValid=false;
    uint8_t mFLAP_RC=0;
    uint16_t mFLAP_VAL=0;
    float mGuidedAlt=1;
};




#endif