#include "TrackerRxTx.h"
#include <modules/my_task_creator/TaskCreator.h>
#include <modules/my_mavlink_tx/MavlinkTransmitter.h>

TrackerRxTx::TrackerRxTx()
{
    mSerial=&Serial2;
}

TrackerRxTx::~TrackerRxTx()
{

}

void TrackerRxTx::Init(uint32_t baud)
{
    #ifdef ARDUINO_WT32_ETH01
    mSerial=&Serial2;
    mSerial->begin(baud,SERIAL_8N1,HWSerial2Eth_rx_pin,HWSerial2Eth_tx_pin);
    mSerial->setPins(HWSerial2Eth_rx_pin,HWSerial2Eth_tx_pin);//eth board
#elif defined ARDUINO_ESP32_DEV
    mSerial=&Serial2;
    mSerial->begin(baud,SERIAL_8N1,HWSerial2_rx_pin,HWSerial2_tx_pin);
#endif
}

void TrackerRxTx::TaskTrackerRxTx(void *pvParameters)
{
    log_i("Task started");
    
    TaskCreator *mCreator = (TaskCreator *)pvParameters;
    TrackerRxTx *obj=&(mCreator->mTracker);
    MavlinkTransmitter *mTransmiter=&(mCreator->mTransmitter);
    while (1)
    {
        obj->ReadSerial();
        obj->SendMavlik();
        if(obj->mRedySend)
        {
            obj->mRedySend=false;
            mTransmiter->SendMavlink(obj->mSenBuf,obj->mMsgSendLen);
        }
        vTaskDelay(1);
    }
    
}
void TrackerRxTx::SetMavlikBuf(uint8_t *buf_in,int len)
{
    mMutex.Lock();
    if((mSendLen+len)<2*MAVLINK_MAX_PACKET_LEN)
    {
        memcpy(mBufferOutStream+mSendLen,buf_in,len);
        mSendLen+=len;
        mMavlinkNew=true;
    }
    else
        log_i("TrackerTx buf over");
    mMutex.Unlock();
}
void TrackerRxTx::SendMavlik()
{
    if(mMavlinkNew)
    {
        mMutex.Lock();
        mSerial->write(mBufferOutStream,mSendLen);
        mMavlinkNew=false;
        mSendLen=0;
        mMutex.Unlock();
    }
}
void TrackerRxTx::ReadSerial()
{
    int len=mSerial->available();
    while(len>2*MAVLINK_MAX_PACKET_LEN)
    {
        mSerial->read(mBuf,2*MAVLINK_MAX_PACKET_LEN);
        len=mSerial->available();
    }

    if(len>0)
    {
        len=mSerial->read(mBuf,len);
        mavlink_status_t status;
        mavlink_message_t msg;
        for(int i=0;i<len;i++)
        {
            ParseChar(mBuf[i]);
            if(mavlink_parse_char(MAVLINK_COMM_1, mBuf[i], &msg, &status))
            {
                int mMsgSendLen= mavlink_msg_to_send_buffer(mSenBuf, &msg);
                mRedySend=true;
            }
        }
        

    }
}

void TrackerRxTx::GetTrackerData(uint16_t &x,uint16_t &y,uint8_t &v,uint8_t &mode,uint16_t &cnt)
{
    mMutex.Lock();
    x=mX;
    y=mY;
    v=mDataValid;
    cnt=mMsgCnt;
    mode=mMode;
    mDataRedy=false;
    mMutex.Unlock();
}
void TrackerRxTx::ParseChar(uint8_t c)
{
    static uint8_t cnt_good=0;
    static uint8_t cnt_bad=0;
    switch (msg_state)
    {
    case ReadHeader:
        if(c==0x1A)
        {
            msg_idx=0;
            msg_state=ReadData;
            msg_buf[0]=0x1A;
            msg_idx++;
        }
        break;
    case ReadData:
    {
        msg_idx++;
        if(msg_idx<7)
        {
            msg_buf[msg_idx-1]=c;
        }
        else
        {
            msg_buf[msg_idx-1]=c;
            msg_state=CalcCrc;
        }
    }
    break;
    case CalcCrc:
    {
        msg_buf[msg_idx]=c;
        uint8_t crc=0xFF;
        crc=crc8(crc,msg_buf,7);
        if(crc==msg_buf[7])
        {
            for(int i=0;i<6;i++)
                mBufferIn.buf[i]=msg_buf[i+1];
            msg_idx=0;
            msg_state=ReadHeader;
            mMutex.Lock();
            if(mBufferIn.data.msg_mode==0x80)
            {
                int32_t v=mBufferIn.data.msg_x+(mBufferIn.data.msg_y<<16);
                mInt2Float.mInt=v;
                mCamValue=mInt2Float.mFloat;
                mCamDataRedy=true;
            }
            else
            {
                mX=mBufferIn.data.msg_x;
                mY=mBufferIn.data.msg_y;
                mDataValid=mBufferIn.data.msg_valid;
                mMode=mBufferIn.data.msg_mode;
                mDataRedy=true;
                
            }
            mMutex.Unlock();
            cnt_good++;
            mMsgCnt++;
            if(cnt_good==20)
            {
                log_i("Tracker data x=%i y=%i v=%i m=%i",mX,mY,mDataValid,mMode);
                cnt_good=0;
            }
        }
        else
        {
            cnt_bad++;
            if(cnt_bad==10)
            {
                log_i("Tracker data bad crc_msg=%i crc_calc=%i",c,crc);
                cnt_bad=0;
            }
            mDataRedy=false;
            bool ok=true;
            uint8_t i=1;
            while (ok)
            {
                if(msg_buf[i]==0x1A)
                {
                    ok=false;
                }
                else
                {
                    i++;
                    if(i==8)
                        ok=false;
                }
            }
            if(i==8)
            {
                msg_idx=0;
                msg_state=ReadHeader;
            }
            else
            {
                for(int j=0;j<8-i;j++)
                {
                    msg_buf[j]=msg_buf[j+i];
                }
                msg_idx=8-i;
                if(msg_idx==7)
                    msg_state=CalcCrc;
                else
                    msg_state=ReadData;
            }
        }
    }
    break;
    default:
        break;
    }
}

float TrackerRxTx::GetCamData()
{
    float res;
    mMutex.Lock();
    res=mCamValue;
    mCamDataRedy=false;
    mMutex.Unlock();
    return res;
}

unsigned char TrackerRxTx::crc8(unsigned char crc, unsigned char * dat, unsigned int len)
{

    while(len--)
    {
        crc ^= *dat++;

        for (unsigned int i = 0; i < 8; i++) crc = crc & 0x80 ? (crc << 1) ^ 0x31 : crc << 1;
    }

    return crc;
}