#include <Arduino.h>
#include <RMTT_Libs.h>
#include <RMTT_Protocol.h>
#include <Wire.h>

#include <DFRobot_IRremote.h>

 // #define __UART0_DEBUG__

#ifdef __UART0_DEBUG__
#define CommonSerial Serial
#else
#define CommonSerial Serial1
#endif

#define FLIGHT      "1FE20DF"
#define DOWN        "1FE48B7"

#define YAW_LEFT    "1FE58A7"
#define YAW_RIGHT   "1FEA05F"
#define HIGHT_UP    "1FEC03F"
#define HIGHT_DOWN  "1FE807F"

#define VY_LEFT    "1FED827"
#define VY_RIGHT   "1FEF00F"
#define VX_UP      "1FE708F"
#define VY_DOWN    "1FE30CF"

#define ZERO        "1FE7887"


RMTT_RGB tt_rgb;
RMTT_Protocol tt_sdk;

// 动态变量
String mind_s_CMD;
volatile float mind_n_yaw, mind_n_hight, mind_n_vx, mind_n_vy;
// 创建对象
IRremote_Receive remoteReceive_26;

void WaitTelloReady();

TaskHandle_t userTaskHandle = NULL;
void user_task(void* arg);

void setup()
{
    Serial.begin(115200);
    Wire.begin(27, 26);
    Wire.setClock(400000);
    Serial1.begin(1000000, 23, 18, SERIAL_8N1);
    Serial.println();

    remoteReceive_26.begin(26);

    RMTT_RGB::Init();

    RMTT_RGB::SetRGB(255, 0, 0);
    tt_sdk.SDKOn();
    WaitTelloReady();
    /* 阻塞线程并一直读取无人机串口返回数据，此后Tello可以确保已经初始化完毕 */
    RMTT_RGB::SetRGB(0, 255, 255);

    /* Create tasks for multi-tasking  */
    /* Larger number represents higher priority  */

    xTaskCreateUniversal(user_task, "user_task", 8192, NULL, 1, &userTaskHandle, 1);
}

void user_task(void* arg)
{
    while (1)
    {
        /* Put your code here ! */
        mind_s_CMD = (remoteReceive_26.getIrCommand());
        if ((mind_s_CMD == String(FLIGHT))) {
            RMTT_RGB::SetGreen(255);
            tt_sdk.TakeOff();

        }
        if ((mind_s_CMD == String(DOWN))) {
            RMTT_RGB::SetGreen(0);
            tt_sdk.Land();
        }

        if ((mind_s_CMD == String(YAW_LEFT))) {
            tt_sdk.sendTelloCtrlMsg((char*)String(String("ccw ") + int(10)).c_str());
        }
        if ((mind_s_CMD == String(YAW_RIGHT))) {
            tt_sdk.sendTelloCtrlMsg((char*)String(String("cw ") + int(10)).c_str());
        }
        if ((mind_s_CMD == String(HIGHT_UP))) {
            tt_sdk.sendTelloCtrlMsg((char*)String(String("up ") + int(20)).c_str());
        }
        if ((mind_s_CMD == String(HIGHT_DOWN))) {
            tt_sdk.sendTelloCtrlMsg((char*)String(String("down ") + int(20)).c_str());
        }

        if ((mind_s_CMD == String(VY_LEFT))) {
            tt_sdk.sendTelloCtrlMsg((char*)String(String("left ") + int(20)).c_str());
        }
        if ((mind_s_CMD == String(VY_RIGHT))) {
            tt_sdk.sendTelloCtrlMsg((char*)String(String("right ") + int(20)).c_str());
        }
        if ((mind_s_CMD == String(VX_UP))) {
            tt_sdk.sendTelloCtrlMsg((char*)String(String("forward ") + int(20)).c_str());
        }
        if ((mind_s_CMD == String(VY_DOWN))) {
            tt_sdk.sendTelloCtrlMsg((char*)String(String("back ") + int(20)).c_str());
        }

        delay(10);
    }
}

void loop()
{
    /* Serial receive process from drone */
    pinMode(34, INPUT_PULLUP);
    // 等待按键被按下
    while (digitalRead(34) == 1);
    /* -------!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-------  */
    /* DO NOT ADD ANY CODE HERE FOR NOT BLOCKING THE RECEIVE FROM THE SERIAL */
    /*     YOU CAN ADD YOUR USER CODE TO THE 'user_task' FUNCTION ABOVE      */
    /* -------!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-------- */
}


void WaitTelloReady()
{
    while (1)
    {
        if (Serial1.available())
        {
            String ret = Serial1.readString();
            if (!strncmp(ret.c_str(), "ETT ok", 6))
            {
                Serial.println(ret.c_str());
                return;
            }
        }
        delay(500);
        tt_sdk.SDKOn();
    }
}