#ifndef TRACKERRXTX_H
#define TRACKERRXTX_H
#include <Arduino.h>
#include <modules/my_mutex/Mutex.h>
#include <modules/mavlink/v2.0/ardupilotmega/mavlink.h>
#define PACKED __attribute__((__packed__))
class TrackerRxTx
{
public:
    TrackerRxTx();
    ~TrackerRxTx();
    static void TaskTrackerRxTx(void *pvParameters);
    void Init(uint32_t baud);
    bool isNewData(){return mDataRedy;}
    bool isNewCamData(){return mCamDataRedy;}
    void GetTrackerData(uint16_t &x,uint16_t &y,uint8_t &v,uint8_t &mode,uint16_t &cnt);
    void SetMavlikBuf(uint8_t *buf_in,int len);
    float GetCamData();
    
private:
    void SendMavlik();
    unsigned char crc8(unsigned char crc, unsigned char * dat, unsigned int len);
    void ParseChar(uint8_t c);
    void ReadSerial();
    HardwareSerial *mSerial;
    struct PACKED tracker_msg {
            uint16_t msg_x;
            uint16_t msg_y;
            uint8_t msg_valid;
            uint8_t msg_mode;
        };
    union PACKED {
        tracker_msg data;
        uint8_t buf[6];
    } mBufferIn;
    union PACKED {
        int32_t mInt;
        float mFloat;
    } mInt2Float;
    enum {
        ReadHeader,
        ReadData,
        CalcCrc
    };
    uint8_t mBuf[2*MAVLINK_MAX_PACKET_LEN];
    uint8_t mSenBuf[MAVLINK_MAX_PACKET_LEN];
    uint16_t mMsgSendLen=0;
    bool mRedySend=false; 
    uint8_t msg_buf[8];
    uint8_t msg_idx=0;
    uint8_t msg_state=ReadHeader;
    Mutex mMutex;
    bool mDataRedy=false;
    uint16_t mX=0;
    uint16_t mY=0;
    uint8_t mDataValid=0;
    uint8_t mMode=0;
    uint16_t mMsgCnt=0;
    uint8_t mBufferOutStream[2*MAVLINK_MAX_PACKET_LEN];
    bool mMavlinkNew=false;
    int mSendLen=0;
    bool mCamDataRedy=false;
    float mCamValue=0;

};


#endif