#define TELLODISCONNECTION 274
#include "../include/Air_Ground_CEC/mainwindow.hpp"
#include <QtConcurrent/QtConcurrent>
#include <iostream>
#include "../include/Djitellowhite.h"
#include "../include/Hardwarelistener.h"
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
    QMainWindow(parent), qnode(argc, argv),
    ui(new Ui::MainWindow)

{
    QtConcurrent::run([=](){
        qnode.init();
    });


    ui->setupUi(this);
    initUis();
    timer = new QTimer(this);

    connections();


}

MainWindow::~MainWindow()
{
    delete ui;
}


void MainWindow::initUis(){
    setBtnStyles();


    /*
     * init
     */

    rock_widget = new JoyStick(ui->JoyStick_widget);
    rock_widget->show();


    m_DashBoard_x = new DashBoard(ui->widget_speed_x);
    m_DashBoard_x->setGeometry(ui->widget_speed_x->rect());

    m_DashBoard_th = new DashBoard(ui->widget_speed_th);
    m_DashBoard_th->setGeometry(ui->widget_speed_th->rect());


}





void MainWindow::slot_keyboard_control(){
    QPushButton *btn = qobject_cast<QPushButton *>(sender());
    std::string btn_name = btn->text().toStdString();
    char key = ' ';

    if (btn_name == "↙")
        key = '1';
    else if (btn_name == "↓")
        key = '2';
    else if (btn_name == "↘")
        key = '3';
    else if (btn_name == "←")
        key = '4';
    //else if (btn_name == "")
      //  key = '5';
    else if (btn_name == "→")
        key = '6';
    else if (btn_name == "↖")
        key = '7';
    else if (btn_name == "↑")
        key = '8';
    else if (btn_name == "↗")
        key = '9';

    //速度
    float liner = ui->horizontalSlider_linear->value() * 0.01;
    float turn = ui->horizontalSlider_raw->value() * 0.01;
    //bool is_rage_mode = ui->checkBox_rage_mode->isChecked();
    //std::cout<<liner<<" "<<turn<<std::endl;
    //qnode.AutoMove(0.2,0,1,0);
    qnode.KeyboardMove(key, liner, turn);


}


void MainWindow::slot_rockKeyChange(int key){
    //速度
    float liner = ui->horizontalSlider_linear->value() * 0.01;
    float turn = ui->horizontalSlider_raw->value() * 0.01;
    switch (key) {
    case upleft:
        qnode.KeyboardMove('7', liner, turn);
        break;
    case up:
        qnode.KeyboardMove('8', liner, turn);
        break;
    case upright:
        qnode.KeyboardMove('9', liner, turn);
        break;
    case left:
        qnode.KeyboardMove('4', liner, turn);
        break;
    case right:
        qnode.KeyboardMove('6', liner, turn);
        break;
    case downleft:
        qnode.KeyboardMove('1', liner, turn);
        break;
    case down:
        qnode.KeyboardMove('2', liner, turn);
        break;
    case downright:
        qnode.KeyboardMove('3', liner, turn);
        break;
    }

}

void MainWindow::slot_batteryState(sensor_msgs::BatteryState msg) {
    ui->label_power->setText("10V");
  ui->label_power->setText(QString::number(msg.voltage).mid(0, 5) + "V");
  double percentage = msg.percentage;
  m_DashBoard_x->set_oil(percentage);
  ui->progressBar->setValue(percentage > 100 ? 100 : percentage);
  //当电量过低时发出提示
  if (percentage <= 20) {
    ui->progressBar->setStyleSheet(
        "QProgressBar::chunk {background-color: red;width: 20px;} QProgressBar "
        "{border: 2px solid grey;border-radius: 5px;text-align: center;}");
    // QMessageBox::warning(NULL, "电量不足", "电量不足，请及时充电！",
    // QMessageBox::Yes , QMessageBox::Yes);
  } else {
    ui->progressBar->setStyleSheet(
        "QProgressBar {border: 2px solid grey;border-radius: 5px;text-align: "
        "center;}");
  }
}

void MainWindow::connections(){
    QObject::connect(ui->btn_main, &QPushButton::clicked, this, [=](){
        ui->stackedWidget_main->setCurrentIndex(0);
    });
    QObject::connect(ui->btn_uav, &QPushButton::clicked, this, [=](){
        ui->stackedWidget_main->setCurrentIndex(1);
    });
    QObject::connect(ui->btn_ugv, &QPushButton::clicked, this, [=](){
        ui->stackedWidget_main->setCurrentIndex(2);
    });

    QObject::connect(ui->btn_system, &QPushButton::clicked, this, [=](){
        ui->stackedWidget_ros_control->setCurrentIndex(1);
    });
    QObject::connect(ui->btn_keyboard, &QPushButton::clicked, this, [=](){
        ui->stackedWidget_ros_control->setCurrentIndex(0);
    });

    //label -> stream
    connect(timer, SIGNAL(timeout()), this, SLOT(importFrame()));

    /*
     * 绑定速度控制按钮
     */

    // 8 -> Forward: ↑
    connect(ui->pushButton_Forward, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
    // 2 -> Back: ↓
    connect(ui->pushButton_Back, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
    // 4 -> Left: ←
    connect(ui->pushButton_Left, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
    // 6 -> Right: →
    connect(ui->pushButton_Right, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
    // 7 -> Left_Forward: ↖
    connect(ui->pushButton_Left_Forward, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
    // 9 -> Right_Forward: ↗
    connect(ui->pushButton_Right_Forward, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
    // 1 -> Left_Back: ↙
    connect(ui->pushButton_Left_Back, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
    // 3 -> Right_Back: ↘
    connect(ui->pushButton_Right_Back, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));

    connect(rock_widget, SIGNAL(keyNumchanged(int)), this,
            SLOT(slot_rockKeyChange(int)));

    connect(ui->getimage, SIGNAL(clicked()), this, SLOT(slot_get_slovedImage()));

    //绑定slider的函数
    connect(ui->horizontalSlider_raw, SIGNAL(valueChanged(int)), this,
            SLOT(Slider_raw_valueChanged(int)));
    connect(ui->horizontalSlider_linear, SIGNAL(valueChanged(int)), this,
            SLOT(Slider_linear_valueChanged(int)));

    connect(&qnode, SIGNAL(speed_x(double)), this, SLOT(slot_speed_x(double)));
    connect(&qnode, SIGNAL(speed_th(double)), this, SLOT(slot_speed_th(double)));

    //电源的信号
    connect(&qnode, SIGNAL(batteryState(sensor_msgs::BatteryState)), this,
            SLOT(slot_batteryState(sensor_msgs::BatteryState)));
    // image
    connect(&qnode, SIGNAL(show_image(QImage)), this, SLOT(slot_show_image(QImage)));

    // solvedimage
    connect(&atr, SIGNAL(show_solvedImage(QImage)), this, SLOT(slot_show_solvedImage(QImage)));
}

void MainWindow::slot_get_slovedImage(){
  qnode.setissaveImageTrue();
  while(true){
    if(qnode.issaveImage == false){
      atr.runCommand();
      atr.getSolvedPic();
      break;
    }
  }
}

void MainWindow::slot_speed_x(double x) {
  m_DashBoard_x->set_speed(abs(x * 100));
  if (x > 0.001) {
    m_DashBoard_x->set_gear(DashBoard::kGear_D);
  } else if (x < -0.001) {
    m_DashBoard_x->set_gear(DashBoard::kGear_R);
  } else {
    m_DashBoard_x->set_gear(DashBoard::kGear_N);
  }
  QString number = QString::number(abs(x * 100)).mid(0, 2);
  if (number[1] == ".") {
    number = number.mid(0, 1);
  }
  //ui.label_speed->setText(number);
}


void MainWindow::slot_speed_th(double th){
    m_DashBoard_th->set_speed(abs(th * 100));
    if (th > 0.001) {
      m_DashBoard_x->set_gear(DashBoard::kGear_D);
    } else if (th < -0.001) {
      m_DashBoard_x->set_gear(DashBoard::kGear_R);
    } else {
      m_DashBoard_x->set_gear(DashBoard::kGear_N);
    }
    QString number = QString::number(abs(th * 100)).mid(0, 2);
    if (number[1] == ".") {
      number = number.mid(0, 1);
    }
}


//滑动条处理槽函数
void MainWindow::Slider_raw_valueChanged(int v) {
  ui->label_raw->setText(QString::number(v));
}
//滑动条处理槽函数
void MainWindow::Slider_linear_valueChanged(int v) {
  ui->label_linear->setText(QString::number(v));
}

void MainWindow::slot_show_image(const QImage& image)
{
    //qimage_mutex_.lock();
    //qimage_ = image.copy();
    ui->label_camera->setScaledContents(true);
    ui->label_camera->setPixmap(QPixmap::fromImage(image));
    ui->label_camera->resize(ui->label_camera->pixmap()->size());
    //qimage_mutex_.unlock();
}

void MainWindow::slot_show_solvedImage(const QImage& solvedImage){
  ui->label_2->setScaledContents(true);
  ui->label_2->setPixmap(QPixmap::fromImage(solvedImage));
  ui->label_2->resize(ui->label_2->pixmap()->size());

  ui->solvedimage->setScaledContents(true);
  ui->solvedimage->setPixmap(QPixmap::fromImage(solvedImage));
  ui->solvedimage->resize(ui->solvedimage->pixmap()->size());
}

void MainWindow::setBtnStyles(){
    /*
     * set PushButton state
     */


    ui->btn_main->setStyleSheet(
                "QPushButton:hover{"
                "background-color:rgb(186, 189, 182);"
                "border-bottom:2px solid rgb(67, 154, 246);}"
                "QPushButton:checked{"
                "background-color:cyan;"
                "border-bottom:2px solid white}"
                "QPushButton:pressed{"
                "background-color:rgb(67, 154, 246)}"
                "QPushButton{"
                "background-color:rgb(238, 238, 236);"
                "border:none;"
                "padding:0px 0px 0px 0px;"
                "margin:0px 0px 0px 0px;}");

    ui->btn_uav->setStyleSheet(
                "QPushButton:hover{"
                "background-color:rgb(186, 189, 182);"
                "border-bottom:2px solid rgb(67, 154, 246);}"
                "QPushButton:checked{"
                "background-color:cyan;"
                "border-bottom:2px solid white}"
                "QPushButton:pressed{"
                "background-color:rgb(67, 154, 246)}"
                "QPushButton{"
                "background-color:rgb(238, 238, 236);"
                "border:none;"
                "padding:0px 0px 0px 0px;"
                "margin:0px 0px 0px 0px;}");

    ui->btn_ugv->setStyleSheet(
                "QPushButton:hover{"
                "background-color:rgb(186, 189, 182);"
                "border-bottom:2px solid rgb(67, 154, 246);}"
                "QPushButton:checked{"
                "background-color:cyan;"
                "border-bottom:2px solid white}"
                "QPushButton:pressed{"
                "background-color:rgb(67, 154, 246)}"
                "QPushButton{"
                "background-color:rgb(238, 238, 236);"
                "border:none;"
                "padding:0px 0px 0px 0px;"
                "margin:0px 0px 0px 0px;}");

    // 8 -> Forward: ↑
    ui->pushButton_Forward->setShortcut(Qt::Key_8);
    ui->pushButton_Forward->setStyleSheet(
                "QPushButton{border-image: url(://images/up.png)}"
                "QPushButton{border:none}"
                "QPushButton:pressed{border-image: url(://images/up_2.png)}");
    ui->pushButton_Forward->setFlat(true);

    // 2 -> Back: ↓
    ui->pushButton_Back->setShortcut(Qt::Key_2);
    ui->pushButton_Back->setStyleSheet(
                "QPushButton{border-image: url(://images/down.png)}"
                "QPushButton{border:none}"
                "QPushButton:pressed{border-image: url(://images/down_2.png)}");
    ui->pushButton_Back->setFlat(true);

    // 4 -> Left: ←
    ui->pushButton_Left->setShortcut(Qt::Key_4);
    ui->pushButton_Left->setStyleSheet(
                "QPushButton{border-image: url(://images/left.png)}"
                "QPushButton{border:none}"
                "QPushButton:pressed{border-image: url(://images/left_2.png)}");
    ui->pushButton_Left->setFlat(true);

    // 6 -> Right: →
    ui->pushButton_Right->setShortcut(Qt::Key_6);
    ui->pushButton_Right->setStyleSheet(
                "QPushButton{border-image: url(://images/right.png)}"
                "QPushButton{border:none}"
                "QPushButton:pressed{border-image: url(://images/right_2.png)}");
    ui->pushButton_Right->setFlat(true);

    // 7 -> Left_Forward: ↖
    ui->pushButton_Left_Forward->setShortcut(Qt::Key_7);
    ui->pushButton_Left_Forward->setStyleSheet(
                "QPushButton{border-image: url(://images/up_left.png)}"
                "QPushButton{border:none}"
                "QPushButton:pressed{border-image: url(://images/up_left_2.png)}");
    ui->pushButton_Left_Forward->setFlat(true);

    // 9 -> Right_Forward: ↗
    ui->pushButton_Right_Forward->setShortcut(Qt::Key_9);
    ui->pushButton_Right_Forward->setStyleSheet(
                "QPushButton{border-image: url(://images/up_right.png)}"
                "QPushButton{border:none}"
                "QPushButton:pressed{border-image: url(://images/up_right_2.png)}");
    ui->pushButton_Right_Forward->setFlat(true);

    // 1 -> Left_Back: ↙
    ui->pushButton_Left_Back->setShortcut(Qt::Key_1);
    ui->pushButton_Left_Back->setStyleSheet(
                "QPushButton{border-image: url(://images/down_left.png)}"
                "QPushButton{border:none}"
                "QPushButton:pressed{border-image: url(://images/down_left_2.png)}");
    ui->pushButton_Left_Back->setFlat(true);

    // 3 -> Right_Back: ↘
    ui->pushButton_Right_Back->setShortcut(Qt::Key_3);
    ui->pushButton_Right_Back->setStyleSheet(
                "QPushButton{border-image: url(://images/down_right.png)}"
                "QPushButton{border:none}"
                "QPushButton:pressed{border-image: url(://images/down_right_2.png)}");
    ui->pushButton_Right_Back->setFlat(true);


}
void MainWindow::importFrame()
{
    capture >> frame;
    cvtColor(frame, frame, CV_BGR2RGB);
    QImage srcQImage = QImage((uchar*)(frame.data), frame.cols, frame.rows, QImage::Format_RGB888);
    ui->tello_receiver->setPixmap(QPixmap::fromImage(srcQImage));
    ui->tello_receiver->resize(srcQImage.size());
    ui->tello_receiver->show();
}

void MainWindow::on_Open_receive_clicked()
{
  //capture.open(0);//video test
  capture.open("udp://@0.0.0.0:11111");
  timer->start(30);
}

void MainWindow::on_Stop_receive_clicked()
{
  timer->stop();
  capture.release();
}




static void* Telloconnect(void *pt){
  Tello T("192.168.10.1");
  int socket_fd, err;

  if((socket_fd = socket(PF_INET,SOCK_DGRAM,IPPROTO_UDP))==-1)
  {
    cout << "initialized error" << endl;
    cout << 0;

  }

  //cout << 1 << socket_fd;
  T.set_conf(socket_fd);

  pthread_t thread_receive;

  err = pthread_create(&thread_receive,nullptr,T.udp_response_receiver,&socket_fd);

  sleep(3);
  struct sockaddr_in serveraddr;
  memset(&serveraddr,0,sizeof(serveraddr));
  serveraddr.sin_family=AF_INET;
  serveraddr.sin_addr.s_addr=T.gethost();
  serveraddr.sin_port=htons(CONTROL_UDP_PORT);



  if(err!=0)
  {
    cout << err << " " << "can't create thread";
  }

  T.get_connect(serveraddr);
  //sleep(1);
  //T.takeoff(serveraddr);
  T.get_video(serveraddr);
  sleep(5);
  //T.land(serveraddr);

  pthread_join(thread_receive,NULL);
  listenkeyboard(T,serveraddr);
  return 0;
}
\
void MainWindow::on_ConnectTello_clicked()
{
  /*
  Tello T("192.168.10.1");
  int socket_fd, err;

  if((socket_fd = socket(PF_INET,SOCK_DGRAM,IPPROTO_UDP))==-1)
  {
    cout << "initialized error" << endl;
    cout << 0;
    exit(TELLODISCONNECTION);

  }
  */

  pthread_t thread_connect;
  int con = pthread_create(&thread_connect,NULL,Telloconnect,NULL);

}



