#include "MavlinkReciver.h"
#include <modules/my_task_creator/TaskCreator.h>
MavlinkReciver::MavlinkReciver()
{
    mSerial=&Serial1;
}

MavlinkReciver::~MavlinkReciver()
{
}

void MavlinkReciver::Init(uint32_t baud)
{
    mBaudRate=baud;
#ifdef ARDUINO_WT32_ETH01
    mSerial=&Serial1;
    mSerial->begin(mBaudRate,SERIAL_8N1,HWSerial1Eth_rx_pin,HWSerial1Eth_tx_pin);
    mSerial->setPins(HWSerial1Eth_rx_pin,HWSerial1Eth_tx_pin);//eth board
#elif defined ARDUINO_ESP32_DEV
    mSerial=&Serial1;
    mSerial->begin(mBaudRate,SERIAL_8N1,HWSerial1_rx_pin,HWSerial1_tx_pin);
#endif
    mavlink_set_proto_version(MAVLINK_COMM_0,2);
}

void MavlinkReciver::TaskMavlinkRx(void *pvParameters)
{
    log_i("Task started");
    TaskCreator *mCreator = (TaskCreator *)pvParameters;
    MavlinkReciver *obj=&(mCreator->mReciver);
    MavlinkTransmitter *mTransmitter=&(mCreator->mTransmitter);
    DefaultParametrs *mParams=&(mCreator->mParams);
    EepromParams *mEeprom=&(mCreator->mEeprom);
    StateMachine *mStateMachine=&(mCreator->mStateMachine);
    TrackerRxTx *mTracker=&(mCreator->mTracker);
    vTaskDelay(100);
    TickType_t mLastTick=xTaskGetTickCount();
    TickType_t mLastTick1=mLastTick;
    while (1)
    {
        int len=obj->mSerial->available();
        if(len>2*MAVLINK_MAX_PACKET_LEN)
        {
            obj->mSerial->read(obj->mBufferIn,2*MAVLINK_MAX_PACKET_LEN);
            log_i("MavlinkReciver buf over");
            continue;
        }
        if(len>0)
        {
            mavlink_status_t status;
            mavlink_message_t msg;
            len=obj->mSerial->read(obj->mBufferIn,len);
            for(int i=0;i<len;i++)
            {
                if(mavlink_parse_char(MAVLINK_COMM_0, obj->mBufferIn[i], &msg, &status))
                {
                    if(!obj->mStatus)
                    {
                        switch (msg.msgid)
                        {
                        case MAVLINK_MSG_ID_HEARTBEAT:
                        {
                            
                            mavlink_heartbeat_t hearbit={0};
                            mavlink_msg_heartbeat_decode(&msg, &hearbit);
                            
                            if ((hearbit.system_status == MAV_STATE_BOOT)
                                ||(hearbit.system_status == MAV_STATE_POWEROFF)
                                ||(hearbit.system_status == MAV_STATE_CALIBRATING))
                            {
                                obj->mStatus=0;
                                log_i("Auttopilot status cahnged=%d", hearbit.system_status);
                                log_i("Reboot");
                                obj->reboot();    
                            }
                            else
                            {
                                if (!obj->mSysId && msg.sysid != 255 && msg.sysid != 83 && msg.compid == MAV_COMP_ID_AUTOPILOT1)
                                {
                                    obj->mSysId = msg.sysid;
                                    mTransmitter->SetSysId(msg.sysid);
                                    mStateMachine->SetSysID(msg.sysid);
                                    log_i("Mavlink set sysid=%d", msg.sysid);
                                    mTransmitter->SendText("Navigation computer start!",MAV_SEVERITY_NOTICE);
                                    mTransmitter->SendText(SOFT_VER,MAV_SEVERITY_NOTICE);

                                    mTransmitter->SendStreamRateAll();
                                    obj->mStatus=1;
                                }
                                
                            }
                            
                        }
                        break;

                        default:
                            break;
                        }
                    }
                    else
                    {
                        switch (msg.msgid)
                        {
                        case MAVLINK_MSG_ID_HEARTBEAT:
                        {
                            if ( msg.sysid != obj->mSysId)
                            {
                                break;
                            }
                            mavlink_heartbeat_t hearbit;
                            mavlink_msg_heartbeat_decode(&msg, &hearbit);
                            if(hearbit.custom_mode!=0)
                            {
                                bool currentlyArmed = hearbit.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY;
                                if(obj->mLockMode)
                                {
                                    mStateMachine->SetBaseMode(hearbit.base_mode,PLANE_MODE_GUIDED);
                                    obj->mLockMode=false;
                                }
                                else
                                {
                                    mStateMachine->SetBaseMode(hearbit.base_mode,hearbit.custom_mode);
                                }
                                obj->mBaseMode=hearbit.base_mode;
                                //log_i("Mavlink arm=%d", currentlyArmed);
                            }
                            if ((hearbit.system_status == MAV_STATE_BOOT)
                                ||(hearbit.system_status == MAV_STATE_POWEROFF)
                                ||(hearbit.system_status == MAV_STATE_CALIBRATING))
                            {
                                obj->mStatus=0;
                                obj->mSysId=0;
                                mTransmitter->SetSysId(0);
                                log_i("Auttopilot status cahnged=%d", hearbit.system_status);
                                log_i("Reboot");
                                obj->reboot();   
                            }
                           
                            if (hearbit.custom_mode != 0)
                            {
                                if (obj->mAutopilotMode != hearbit.custom_mode)
                                {
                                    
                                        obj->mAutopilotMode = hearbit.custom_mode;
                                        obj->mBaseMode=hearbit.base_mode;
                                        log_i("Autopilot mode changed=%d", hearbit.custom_mode);
                                }
                            }
                        }
                        break;
                        
                        case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
                        {
                            uint8_t comp=mavlink_msg_param_request_list_get_target_component(&msg);
                            log_i("PARAM_REQUEST_LIST for %i",comp);
                            if(comp!=MY_COMP_ID)
                            {
                                
                                continue;
                            }
                            
                            uint16_t nums=mParams->GetParamSize();
                            
                            for(int i=0;i<nums;i++)
                            {
                                mavlink_param_value_t val={0};
                                String name=mParams->GetName(i);
                                for(int j=0;j<name.length();j++)
                                    if(j<16)
                                        val.param_id[j]=name[j];
                                if(name.length()<15)        
                                    val.param_id[name.length()]=0;        
                                val.param_count=nums;
                                val.param_index=i;
                                val.param_type=MAV_PARAM_TYPE_REAL32;
                                val.param_value=mParams->GetValue(i);
                                
                                mTransmitter->SendParameter(val);

                            }
                        }
                        break;
                        case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
                        {
                            uint8_t comp=mavlink_msg_param_request_read_get_target_component(&msg);
                            log_i("PARAM_REQUEST_READ for %i",comp);
                            if(comp!=MY_COMP_ID)
                            {
                                
                                continue;
                            }
                            mavlink_param_request_read_t param_to_read = {0};
                            mavlink_msg_param_request_read_decode(&msg,&param_to_read);
                            log_i("Param request:%s %d\n\r",param_to_read.param_id,param_to_read.param_index);
                            mavlink_param_value_t val={0};
                            uint16_t nums=mParams->GetParamSize();
                            uint16_t i = param_to_read.param_index;
                            if(i>nums-1)
                            {
                                log_i("BAD PARAM_REQUEST_READ NUM for %i",i);
                                continue;
                            }
                            String name = mParams->GetName(i);
                            for (int j = 0; j < name.length(); j++)
                                if (j < 16)
                                    val.param_id[j] = name[j];
                            if(name.length()<15)        
                                val.param_id[name.length()]=0;        
                            val.param_count = nums;
                            val.param_index = i;
                            val.param_type = MAV_PARAM_TYPE_REAL32;
                            val.param_value = mParams->GetValue(i);

                            mTransmitter->SendParameter(val);
                        }
                        break;
                        case MAVLINK_MSG_ID_ATTITUDE:
                        {
                            static uint16_t cnt1=0;
                            cnt1++;
                            mavlink_attitude_t attitude;
                            mavlink_msg_attitude_decode(&msg, &attitude);
                            mStateMachine->SetRPY(attitude.roll,attitude.pitch,attitude.yaw);
                            if(cnt1==1200)
                            {
                                uint32_t dt=xTaskGetTickCount()-mLastTick1;
                                dt/=1200;
                                
                                mLastTick1=xTaskGetTickCount();
                                cnt1=0;
                                String tmp("attitude dt=");
                                tmp+=dt;
                                mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
                                log_i("attitude dt=%d",dt);
                            }
                        }
                        break;
                        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
                        {
                            static uint16_t cnt=0;
                            cnt++;
                            mavlink_global_position_int_t pos;
                            mavlink_msg_global_position_int_decode(&msg, &pos);
                            mStateMachine->SetPos(pos.lat*1e-7,pos.lon*1e-7,pos.relative_alt/1000.0);
                            if(cnt==1000)
                            {
                                uint32_t dt=xTaskGetTickCount()-mLastTick;
                                dt/=1000;
                                
                                mLastTick=xTaskGetTickCount();
                                cnt=0;
                                String tmp("GLOBAL_POSITION_INT dt=");
                                tmp+=dt;
                                mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
                                log_i("GLOBAL_POSITION_INT dt=%d",dt);
                            }

                        }
                        break;
                        case MAVLINK_MSG_ID_PARAM_SET:
                        {
                            mavlink_param_set_t param_set = {0};
              
                            mavlink_msg_param_set_decode(&msg, &param_set);
                            log_v("Mavlink param set:%s %f\n\r",param_set.param_id,(float)param_set.param_value);
                            String name(param_set.param_id);
                            if(name.length()>16)
                            {
                                name.remove(16,name.length()-16);
                            }
                            uint16_t idx=mParams->SetParam(name,param_set.param_value);
                            uint16_t nums=mParams->GetParamSize();
                            if(idx<nums)
                            {
                                mEeprom->WriteParam(idx);
                                mavlink_param_value_t val={0};
                                
                                for (int j = 0; j < name.length(); j++)
                                    if (j < 16)
                                        val.param_id[j] = name[j];
                                val.param_id[name.length()]=0;        
                                val.param_count = nums;
                                val.param_index = idx;
                                val.param_type = MAV_PARAM_TYPE_REAL32;
                                val.param_value = mParams->GetValue(idx);

                                mTransmitter->SendParameter(val);
                                if(name=="CTRL_REBOOT")
                                {
                                    mTransmitter->SendText("Navigation computer reboot!",MAV_SEVERITY_CRITICAL);
                                }
                                if(name=="CTRL_TEST_ALT")
                                {
                                    uint8_t m=mParams->GetValue("CTRL_TEST_MODE");
                                    mStateMachine->SetTestMode(m,val.param_value);
                                }
                                if(name=="CTRL_K_YAW")
                                {
                                    
                                    float kp=mParams->GetValue("CTRL_K_YAW");
                                    mStateMachine->SetK(kp);
                                }
                                if(name=="CTRL_I_YAW")
                                {
                                    
                                    float kp=mParams->GetValue("CTRL_I_YAW");
                                    mStateMachine->SetI(kp);
                                }
                                if(name=="CTRL_D_YAW")
                                {
                                    
                                    float kp=mParams->GetValue("CTRL_D_YAW");
                                    mStateMachine->SetD(kp);
                                }
                                if(name=="CTRL_I_MAX")
                                {
                                    
                                    float kp=mParams->GetValue("CTRL_I_MAX");
                                    mStateMachine->SetImax(kp);
                                }
                                if(name=="CTRL_K_PITCH")
                                {
                                    
                                    float kp=mParams->GetValue("CTRL_K_PITCH");
                                    mStateMachine->SetKpitch(kp);
                                }
                                if(name=="CTRL_USE_KPITCH")
                                {
                                    bool m=mParams->GetValue("CTRL_USE_KPITCH");
                                    mStateMachine->SetUseKpitch(m);
                                }
                                if(name=="VIDEO_ENABLE")
                                {
                                    bool m=mParams->GetValue("VIDEO_ENABLE");
                                    mStateMachine->SetTrackerEnable(m);
                                }
                                if(name=="CTRL_ATTACK_SPD")
                                {
                                    float m=mParams->GetValue("CTRL_ATTACK_SPD");
                                    mStateMachine->SetAttackSpeed(m);
                                }
                                if(name=="CAM_SRV_CH")
                                {
                                    float m=mParams->GetValue("CAM_SRV_CH");
                                    mStateMachine->SetCamSrvCH(m);
                                }
                                if(name=="CTRL_SRV_CH")
                                {
                                    float m=mParams->GetValue("CTRL_SRV_CH");
                                    mStateMachine->SetCTRLSrvCH(m);
                                }
                                if(name=="CAM_SRV_MAX")
                                {
                                    float m=mParams->GetValue("CAM_SRV_MAX");
                                    mStateMachine->SetCamSrvMax(m);
                                }
                                if(name=="CAM_SRV_MIN")
                                {
                                    float m=mParams->GetValue("CAM_SRV_MIN");
                                    mStateMachine->SetCamSrvMin(m);
                                }
                                if(name=="CAM_ANG_CRUISE")
                                {
                                    float m=mParams->GetValue("CAM_ANG_CRUISE");
                                    mStateMachine->SetCamAngCruise(m);
                                }
                                if(name=="CAM_ANG_MAX")
                                {
                                    float m=mParams->GetValue("CAM_ANG_MAX");
                                    mStateMachine->SetCamAngMax(m);
                                }
                                if(name=="CAM_ANG_MIN")
                                {
                                    float m=mParams->GetValue("CAM_ANG_MIN");
                                    mStateMachine->SetCamAngMin(m);
                                }
                                if(name=="CTRL_FLAP_RC")
                                {
                                    float m=mParams->GetValue("CTRL_FLAP_RC");
                                    mStateMachine->SetFlapRC(m);
                                }
                                if(name=="CTRL_FLAP_VAL")
                                {
                                    float m=mParams->GetValue("CTRL_FLAP_VAL");
                                    mStateMachine->SetFlapVAL(m);
                                    
                                }
                                if(name=="TEST_SRV_CH")
                                {
                                    float m=mParams->GetValue("TEST_SRV_CH");
                                    mStateMachine->SetTestSrvCH(m);
                                    
                                }
                                if(name=="TEST_SRV_AMP")
                                {
                                    float m=mParams->GetValue("TEST_SRV_AMP");
                                    mStateMachine->SetTestSrvAmp(m);
                                    
                                }
                                if(name=="TEST_SRV_T")
                                {
                                    float m=mParams->GetValue("TEST_SRV_T");
                                    mStateMachine->SetTestSrvT(m);
                                    
                                }
                                if(name=="TEST_SRV_TRIM")
                                {
                                    float m=mParams->GetValue("TEST_SRV_TRIM");
                                    mStateMachine->SetTestSrvTrim(m);
                                    
                                }
                                if(name=="CTRL_LOITER_ALT")
                                {
                                    float m=mParams->GetValue("CTRL_LOITER_ALT");
                                    mStateMachine->SetLoiterAlt(m);
                                    
                                }
                                if(name=="CTRL_SAFE_SPD")
                                {
                                    float m=mParams->GetValue("CTRL_SAFE_SPD");
                                    if(m<20)
                                        m=20;
                                    mParams->SetParam("CTRL_SAFE_SPD",m);    
                                    mStateMachine->SetSafeSpeed(m);
                                    
                                }
                                if(name=="CTRL_SAFE_PITCH")
                                {
                                    float m=mParams->GetValue("CTRL_SAFE_PITCH");
                                    if(m<0)
                                        m=0;
                                    mParams->SetParam("CTRL_SAFE_PITCH",m);    
                                    mStateMachine->SetSafePitch(m);
                                    
                                }
                                if(name=="CTRL_SAFE_TIME")
                                {
                                    float m=mParams->GetValue("CTRL_SAFE_TIME");
                                    if(m<10)
                                        m=10;
                                    mParams->SetParam("CTRL_SAFE_TIME",m);    
                                    mStateMachine->SetSafeTime(m);
                                    
                                }
                                if(name=="CAM_SRV_REVERSE")
                                {
                                    float m=mParams->GetValue("CAM_SRV_REVERSE");
                                    mStateMachine->SetCamReverse(m);
                                    
                                }
                                if(name=="CAM_SRV_KI")
                                {
                                    float m=mParams->GetValue("CAM_SRV_KI");
                                    if(m<0)
                                    {
                                        m=0.1;
                                        mParams->SetParam("CAM_SRV_KI",m);
                                    }
                                    mStateMachine->SetCamSrvKI(m);
                                    
                                }
                                if(name=="CAM_SRV_KP")
                                {
                                    float m=mParams->GetValue("CAM_SRV_KP");
                                    if(m<0)
                                    {
                                        m=0.0;
                                        mParams->SetParam("CAM_SRV_KP",m);
                                    }
                                    mStateMachine->SetCamSrvKP(m);
                                    
                                }
                                if(name=="CAM_PITCH_DELTA")
                                {
                                    float m=mParams->GetValue("CAM_PITCH_DELTA");
                                    if(m<2.0)
                                    {
                                        m=2.0;
                                        mParams->SetParam("CAM_PITCH_DELTA",m);
                                    }
                                    mStateMachine->SetCamPitchDelta(m);
                                    
                                }
                                if(name=="CAM_RATE_MAX")
                                {
                                    float m=mParams->GetValue("CAM_RATE_MAX");
                                    if(m<1.0)
                                    {
                                        m=1.0;
                                        mParams->SetParam("CAM_RATE_MAX",m);
                                    }
                                    mStateMachine->SetCamRateMax(m);
                                    
                                }
                                if(name=="CTRL_TECS_DALT")
                                {
                                    float m=mParams->GetValue("CTRL_TECS_DALT");
                                    
                                    mStateMachine->SetTecsDalt(m);
                                    
                                }

                            }
                            else
                            {
                                log_v("Error set:%s %i ret idx=%i\n\r",name.c_str(),name.length(),idx);
                            }
                        }
                        break;
                        case MAVLINK_MSG_ID_PARAM_VALUE:
                        {
                            //mavlink_param_value_t val={0};
                            //mavlink_msg_param_value_decode(&msg,&val);
                            //mTransmitter->SendParameter(val);
                        }
                        break;
                        case MAVLINK_MSG_ID_MISSION_COUNT:
                            {
                                
                                mavlink_mission_count_t req;
                                mavlink_msg_mission_count_decode(&msg,&req);
                                mStateMachine->SetWpCount(req.count-1);
                                log_i("Mavlink readed wpnums=%d",req.count);
                            }
                            break;
                        case MAVLINK_MSG_ID_MISSION_ITEM:
                            {
                                mavlink_mission_item_t item;
                                mavlink_msg_mission_item_decode(&msg,&item);
                                mStateMachine->SetWp(item);
                                log_i("Mavlink readed wp num=%d",item.seq);
                            }
                        break;
                        case MAVLINK_MSG_ID_MISSION_REQUEST:
                            {
                                mavlink_mission_request_t req;
                                mavlink_msg_mission_request_decode(&msg,&req);
                                mTransmitter->SendMissionItem(req.seq);
                                log_i("Mavlink requesr wp num=%d",req.seq);
                            }
                            break;
                        case MAVLINK_MSG_ID_MISSION_ACK:
                            {
                                mavlink_mission_ack_t ack;
                                mavlink_msg_mission_ack_decode(&msg,&ack);
                                if(ack.type==0)
                                    mStateMachine->SetWpsSended(true);
                                log_i("Mavlink mission ack=%d",ack.type);    
                            }
                            break;
                        case MAVLINK_MSG_ID_MISSION_CURRENT:
                        {
                                mavlink_mission_current_t wpc;
                                mavlink_msg_mission_current_decode(&msg, &wpc);
                                mStateMachine->SetCurrWpNum(wpc.seq);
                        }
                        break;
                        case MAVLINK_MSG_ID_COMMAND_ACK:
                        {
                            mavlink_command_ack_t ack;
                            mavlink_msg_command_ack_decode(&msg,&ack);
                            //qDebug()<<"MAVLINK_MSG_ID_COMMAND_ACK "<<ack.command<<" res="<<ack.result<<" param2="<<ack.result_param2<<endl;
                            if(ack.command==MAV_CMD_GUIDED_CHANGE_SPEED)
                            {
                                String str("Set ATTACK_SPD=");
                                float m=mParams->GetValue("CTRL_ATTACK_SPD");
                                str+=String(m,1);
                                str+=String(" res=");
                                str+=String(ack.result);
                                mTransmitter->SendText(str,MAV_SEVERITY_INFO);
                            }
                            if(ack.command==MAV_CMD_DO_REPOSITION)
                            {
                                obj->mLockMode=true;
                                mStateMachine->SetBaseMode(obj->mBaseMode,PLANE_MODE_GUIDED);
                                log_i("Fast GUIDED %i",ack.result);
                            }
                        }
                        break;
                        case MAVLINK_MSG_ID_NAMED_VALUE_INT:
                        {
                            log_i("Recived cmd to tracker!!!!!!");
                        }
                        break;
                        default:
                            break;
                        }
                        int len = mavlink_msg_to_send_buffer(obj->mBufferOut, &msg);
                        mTracker->SetMavlikBuf(obj->mBufferOut,len);
                    }
                }
            }
        }
        else
            vTaskDelay(1);
    }
    
}

void MavlinkReciver::reboot()
{
    vTaskDelay(2000);
    ESP.restart();
}