#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "calPoint.h"

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    pos = new PosDevice(this);
    cam = new Camera(this);

    rs = new RotationStage(this);
    tz = new QTimer(this);
    tz_ = new QTimer(this);
    tu = new QTimer(this);
    tu_ = new QTimer(this);

    topLeft=0;
    topRight=0;
    selectWidth=0;
    selectHeight=0;
    isFirstFrame=false;
    loopState = false;


    ui->lineEdit->setMaxLength(500);
    ui->lineEdit_2->setMaxLength(500);
    ui->lineEdit_3->setMaxLength(500);
    ui->lineEdit_9->setMaxLength(500);
    ui->lineEdit_10->setMaxLength(500);
    ui->lineEdit_11->setMaxLength(500);




    tz->setInterval(10);
    tz_->setInterval(10);
    tu->setInterval(10);
    tu_->setInterval(10);




    connect(pos,&PosDevice::sendPosData,this,&MainWindow::revPosData);
    connect(cam,&Camera::sendPicData,this,&MainWindow::revPicData);
    connect(rs,&RotationStage::sendRotateData,this,&MainWindow::revRotateData);

    connect(tz,&QTimer::timeout,this,&MainWindow::zmove);
    connect(tz_,&QTimer::timeout,this,&MainWindow::zmove_);
    connect(tu,&QTimer::timeout,this,&MainWindow::umove);
    connect(tu_,&QTimer::timeout,this,&MainWindow::umove_);

    connect(ui->graphicsView, &ImageView::roiSelected, this, &MainWindow::onRoiSelected);

}

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

cv::Rect MainWindow::trackRoiWithCorrelation(const cv::Mat &frame)
{
    if(roiTemplate.empty()) {
        qDebug() << "模板为空";
        return cv::Rect();
    }

    // 定义搜索区域（在上一帧ROI周围扩大区域）
    int searchMargin = 30;  // 搜索边界，可根据需要调整
    cv::Rect searchArea;
    searchArea.x = std::max(0, lastRoi.x - searchMargin);
    searchArea.y = std::max(0, lastRoi.y - searchMargin);
    searchArea.width = std::min(frame.cols - searchArea.x,
                                lastRoi.width + 2 * searchMargin);
    searchArea.height = std::min(frame.rows - searchArea.y,
                                 lastRoi.height + 2 * searchMargin);

    // 检查搜索区域有效性
    if(searchArea.width <= 0 || searchArea.height <= 0) {
        qDebug() << "搜索区域无效";
        return lastRoi;
    }

    // 提取搜索区域
    cv::Mat searchRegion = frame(searchArea);

    // 检查模板和搜索区域大小
    if(searchRegion.rows < roiTemplate.rows || searchRegion.cols < roiTemplate.cols) {
        // 搜索区域比模板小，调整模板大小
        double scaleX = static_cast<double>(searchRegion.cols) / roiTemplate.cols;
        double scaleY = static_cast<double>(searchRegion.rows) / roiTemplate.rows;
        double scale = std::min(scaleX, scaleY) * 0.9;  // 稍微缩小一点确保安全

        cv::Mat resizedTemplate;
        cv::resize(roiTemplate, resizedTemplate, cv::Size(), scale, scale, cv::INTER_AREA);

        // 模板匹配
        cv::Mat result;
        cv::matchTemplate(searchRegion, resizedTemplate, result, cv::TM_CCOEFF_NORMED);

        // 找到最佳匹配位置
        double maxVal;
        cv::Point maxLoc;
        cv::minMaxLoc(result, nullptr, &maxVal, nullptr, &maxLoc);

        // 设置匹配阈值
        double correlationThreshold = 0.5;
        if(maxVal < correlationThreshold) {
            qDebug() << "匹配度低:" << maxVal;
            return lastRoi;
        }

        // 计算新的ROI位置
        cv::Rect newRoi;
        newRoi.x = searchArea.x + maxLoc.x;
        newRoi.y = searchArea.y + maxLoc.y;
        newRoi.width = resizedTemplate.cols;
        newRoi.height = resizedTemplate.rows;

        qDebug() << "模板匹配成功, 相似度:" << maxVal << "位置:" << newRoi.x << newRoi.y;
        lastRoi = newRoi;
        return newRoi;
    } else {
        // 正常情况下的模板匹配
        cv::Mat result;
        cv::matchTemplate(searchRegion, roiTemplate, result, cv::TM_CCOEFF_NORMED);

        // 找到最佳匹配位置
        double maxVal;
        cv::Point maxLoc;
        cv::minMaxLoc(result, nullptr, &maxVal, nullptr, &maxLoc);

        // 设置匹配阈值
        double correlationThreshold = 0.6;
        if(maxVal < correlationThreshold) {
            qDebug() << "匹配度低:" << maxVal;
            return lastRoi;
        }

        // 计算新的ROI位置
        cv::Rect newRoi;
        newRoi.x = searchArea.x + maxLoc.x;
        newRoi.y = searchArea.y + maxLoc.y;
        newRoi.width = roiTemplate.cols;
        newRoi.height = roiTemplate.rows;

        qDebug() << "模板匹配成功, 相似度:" << maxVal << "位置:" << newRoi.x << newRoi.y;
        lastRoi = newRoi;
        return newRoi;
    }
}

void MainWindow::updateTemplate(const cv::Mat &frame, const cv::Rect &roi)
{
    static int updateCounter = 0;
    updateCounter++;

    // 每N帧更新一次模板
    if(updateCounter % 15 == 0) {  // 每15帧更新一次
        if(roi.x >= 0 && roi.y >= 0 &&
            roi.x + roi.width <= frame.cols &&
            roi.y + roi.height <= frame.rows) {

            cv::Mat newTemplate = frame(roi).clone();

            // 使用加权平均更新模板
            double alpha = 0.1;  // 学习率，较小的值使模板更新更慢

            if(roiTemplate.size() == newTemplate.size()) {
                cv::addWeighted(roiTemplate, 1.0 - alpha, newTemplate, alpha, 0, roiTemplate);
                qDebug() << "模板已更新";
            }
        }
        updateCounter = 0;
    }
}

QImage MainWindow::cvMatToQImage(const cv::Mat &mat)
{
    switch(mat.type()) {
    case CV_8UC1: {  // 灰度图
        QImage img(mat.data, mat.cols, mat.rows,
                   static_cast<int>(mat.step), QImage::Format_Grayscale8);
        return img.copy();  // 创建副本以避免内存问题
    }
    case CV_8UC3: {  // 彩色图
        QImage img(mat.data, mat.cols, mat.rows,
                   static_cast<int>(mat.step), QImage::Format_RGB888);
        return img.rgbSwapped().copy();
    }
    case CV_8UC4: {  // 带透明度的彩色图
        QImage img(mat.data, mat.cols, mat.rows,
                   static_cast<int>(mat.step), QImage::Format_ARGB32);
        return img.copy();
    }
    default:
        qWarning() << "不支持的图像格式:" << mat.type();
        return QImage();
    }
}

cv::Point2f MainWindow::calculateCentroid(const cv::Mat &image, const cv::Rect &roi)
{
    // 提取ROI区域
    cv::Mat roiRegion = image(roi);

    // 使用灰度图像直接计算矩
    cv::Moments m = cv::moments(roiRegion, true);

    if(m.m00 == 0) {
        qDebug() << "无法计算质心：区域可能为全黑";
        return cv::Point2f(-1, -1);
    }

    // 计算质心坐标
    float centroidX = static_cast<float>(m.m10 / m.m00);
    float centroidY = static_cast<float>(m.m01 / m.m00);

    // 转换为图像坐标系
    centroidX += roi.x;
    centroidY += roi.y;

    return cv::Point2f(centroidX, centroidY);
}

void MainWindow::closeloop()
{


    // if(ui->lineEdit_4->text().isEmpty()||ui->lineEdit_5->text().isEmpty()||ui->lineEdit_6->text().isEmpty()){
    //     qDebug()<<"Error --> B Longtitude/Lantitude/Altitude is empty.";
    //     return ;
    // }

    while(loopState){

        double ALon =ui->lineEdit_9->text().toDouble();
        double ALan = ui->lineEdit_10->text().toDouble();
        double AAlt = ui->lineEdit_11->text().toDouble();

        double AHeading = ui->lineEdit->text().toDouble();
        double APitch = ui->lineEdit_2->text().toDouble();
        double ARoll = ui->lineEdit_3->text().toDouble();

        // ALon = 120.30071541667;  // A机经度，deg
        // ALan = 30.2623007866115;   // A机纬度，deg
        // AAlt = 60.0/1000;                       // A机高度，km



        double BLon = ui->lineEdit_4->text().toDouble();
        double BLan = ui->lineEdit_5->text().toDouble();
        double BAlt= ui->lineEdit_6->text().toDouble();




        // qDebug()<<"A Longtitude is "<<ALon<<"°, Lantitude is "<<ALan<<"°,Altitude is "<<AAlt<<"m.";
        // qDebug()<<"B Longtitude is "<<BLon<<"°, Lantitude is "<<BLan<<"°,Altitude is "<<BAlt<<"m.";
        // qDebug()<<"A Heading is "<<AHeading<<"°, Pitch is "<<APitch<<"°,Roll is "<<ARoll<<"°.";

        QVector<double> res = calRes(ALon,ALan,AAlt,BLon,BLan,BAlt,AHeading,APitch,ARoll); //返回指向的方位角和俯仰角度数 E-->Z, N-->U,DEGEE

        rs->move(0,2,res.at(0)*67108864/360);
        rs->move(0,3,res.at(1)*67108864/360);
        std::this_thread::sleep_for(std::chrono::milliseconds((int)(50)));

    }
}

void MainWindow::on_pushButton_clicked()
{
    pos->open();
}

void MainWindow::revPosData(QVector<QString> posData)
{
    ui->lineEdit->setText(posData.at(0));   //Heading
    ui->lineEdit_2->setText(posData.at(1)); //Pitch
    ui->lineEdit_3->setText(posData.at(2)); //Roll

    ui->lineEdit_10->setText(posData.at(3)); //Lantitude
    ui->lineEdit_9->setText(posData.at(4)); //Longtitude
    ui->lineEdit_11->setText(QString::number(posData.at(5).toDouble()/1000.0)); //Altitude

}


void MainWindow::on_pushButton_3_clicked()
{
    cam->open();
}


void MainWindow::on_pushButton_5_clicked()
{
    pos->close();
}


void MainWindow::on_pushButton_4_clicked()
{
    cam->close();
}

void MainWindow::revPicData(QImage img){
    QPixmap tmppximg = QPixmap::fromImage(img);
    if(!ui->graphicsView->state){
        ui->graphicsView->pximg->setPixmap(tmppximg);
    }else{
         cv::Mat cvImg; //origin pic
         cvImg = cv::Mat(img.height(), img.width(), CV_8UC1,
                        (void*)img.constBits(), img.bytesPerLine());

         // 定义ROI区域
         cv::Rect currentRoi;

         if(isFirstFrame) {
             // 第一帧，使用初始ROI
             currentRoi = cv::Rect(topLeft, topRight,
                                   selectWidth, selectHeight);

             // 保存模板
             if(currentRoi.x >= 0 && currentRoi.y >= 0 &&
                 currentRoi.x + currentRoi.width <= cvImg.cols &&
                 currentRoi.y + currentRoi.height <= cvImg.rows) {
                 roiTemplate = cvImg(currentRoi).clone();
                 lastRoi = currentRoi;
                 isFirstFrame = false;

                 qDebug() << "初始ROI设置: " << currentRoi.x << currentRoi.y
                          << currentRoi.width << "x" << currentRoi.height;

                 centroidInit = calculateCentroid(cvImg, currentRoi);
                  qDebug() << "初始ROI质心坐标: (" << centroidInit.x << "," << centroidInit.y << ")";
             }
         } else {
             // 后续帧使用模板匹配
             currentRoi = trackRoiWithCorrelation(cvImg);
         }

         // 在图像上绘制ROI矩形
         if(currentRoi.width > 0 && currentRoi.height > 0) {
             // 为了在灰度图上显示彩色矩形，先转换为BGR
             cv::Mat colorImg;
             cv::cvtColor(cvImg, colorImg, cv::COLOR_GRAY2BGR);
             cv::rectangle(colorImg, currentRoi, cv::Scalar(255, 0, 0), 1);

             centroid = calculateCentroid(cvImg, currentRoi);

             if(centroid.x >= 0 && centroid.y >= 0) {
                 // 在质心位置绘制红色圆点
                 cv::circle(colorImg, centroid, 5, cv::Scalar(255, 0, 0), -1); // 红色实心圆

                 // 绘制十字线
                 cv::line(colorImg,
                          cv::Point(centroid.x - 10, centroid.y),
                          cv::Point(centroid.x + 10, centroid.y),
                          cv::Scalar(0, 0, 255), 2); // 水平线
                 cv::line(colorImg,
                          cv::Point(centroid.x, centroid.y - 10),
                          cv::Point(centroid.x, centroid.y + 10),
                          cv::Scalar(0, 0, 255), 2); // 垂直线

                 // 显示质心坐标
                 std::string centroidText = "Centroid: (" +
                                            std::to_string(centroid.x).substr(0, 5) + ", " +
                                            std::to_string(centroid.y).substr(0, 5) + ")";

                 cv::putText(colorImg, centroidText,
                             cv::Point(currentRoi.x, currentRoi.y - 20),
                             cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1);




                 std::string xTBL = "xTBL: " + std::to_string((centroid.x-centroidInit.x)*0.003953).substr(0, 5)+" degree";
                 std::string yTBL = "yTBL: " + std::to_string((centroid.y-centroidInit.y)*0.003953).substr(0, 5)+" degree";


                 cv::putText(colorImg, xTBL,
                             cv::Point(20, 20),
                             cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1);

                 cv::putText(colorImg, yTBL,
                             cv::Point(20, 40),
                             cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1);

                 // qDebug() << "ROI质心坐标: (" << centroid.x << "," << centroid.y << ")";
                 qDebug()<<"X 脱靶量（像素）"<< centroid.x-centroidInit.x;
                 qDebug()<<"Y 脱靶量（像素）"<< centroid.y-centroidInit.y;


             // 更新模板
             updateTemplate(cvImg, currentRoi);

             // 转换回QImage显示
             QImage resultImg = cvMatToQImage(colorImg);
             ui->graphicsView->pximg->setPixmap(QPixmap::fromImage(resultImg));
         } else {
             // 如果没有有效ROI，直接显示原图
             ui->graphicsView->pximg->setPixmap(tmppximg);
         }
         }
    }
}


void MainWindow::revRotateData(QVector<double> rData)
{
    ui->lineEdit_7->setText(QString::number(rData[0],'f',3));
    ui->lineEdit_8->setText(QString::number(rData[1],'f',3));

}

void MainWindow::on_pushButton_10_clicked()
{
    rs->open();
}


void MainWindow::on_pushButton_6_pressed()
{
    tz->start();
}


void MainWindow::on_pushButton_6_released()
{
    tz->stop();
}


void MainWindow::zmove(){
     rs->move(0,2,1000);
}

void MainWindow::zmove_()
{
      rs->move(0,2,-1000);
}

void MainWindow::umove()
{
      rs->move(0,3,1000);
}

void MainWindow::umove_()
{
      rs->move(0,3,-1000);
}



void MainWindow::on_pushButton_7_pressed()
{

    if(rs->state){
        tz_->start();
    }

}


void MainWindow::on_pushButton_7_released()
{
    if(rs->state){
     tz_->stop();
    }

}


void MainWindow::on_pushButton_8_pressed()
{

    if(rs->state){
    tu->start();
    }
}


void MainWindow::on_pushButton_8_released()
{
    if(rs->state){
        tu->stop();
    }
}


void MainWindow::on_pushButton_9_pressed()
{

    if(rs->state){
        tu_->start();
    }
}


void MainWindow::on_pushButton_9_released()
{
    if(rs->state){
        tu_->stop();
    }
}


void MainWindow::on_pushButton_11_clicked()
{
    rs->close();
}

void MainWindow::onRoiSelected(const QRectF &roi)
{

    qDebug() << "ROI Selected - Position:" << roi.topLeft()
             << "Size:" << roi.size();
    topLeft = roi.topLeft().x();
    topRight = roi.topRight().y();
    selectWidth=roi.width();
    selectHeight=roi.height();
    isFirstFrame=true;
    ui->graphicsView->state=true;

}


void MainWindow::on_pushButton_2_clicked()
{
    double ALon =ui->lineEdit_9->text().toDouble();
    double ALan = ui->lineEdit_10->text().toDouble();
    double AAlt =ui->lineEdit_11->text().toDouble();

    double AHeading = ui->lineEdit->text().toDouble();
    double APitch = ui->lineEdit_2->text().toDouble();
    double ARoll = ui->lineEdit_3->text().toDouble();

    // if(ui->lineEdit_4->text().isEmpty()||ui->lineEdit_5->text().isEmpty()||ui->lineEdit_6->text().isEmpty()){
    //     QMessageBox::critical(this,"Error","B Longtitude/Lantitude/Altitude is empty.");
    //     return ;
    // }
    double BLon = ui->lineEdit_4->text().toDouble();
    double BLan = ui->lineEdit_5->text().toDouble();
    double BAlt= ui->lineEdit_6->text().toDouble();



    // gcal->setPositionA(ALon,ALan,AAlt);
    // gcal->setPositionB(BLon,BLan,BAlt);
    // gcal->setRealTimeAttitude(AHeading,APitch,ARoll);




    // // // A机位置
    // ALon = 120.30071541667;  // A机经度，deg
    // ALan = 30.2623007866115;   // A机纬度，deg
    // AAlt = 60.0/1000;                       // A机高度，km

    // // // B机位置
    // BLon = 120.300913900136; // B机经度，deg
    // BLan = 30.2622503984337;  // B机纬度，deg
    // BAlt = 60.0/1000 ;


    // // // 根据POS设备实时姿态角解算B机在A机载体坐标系中的位置
    // AHeading = 65.516;
    // APitch = -0.112;
    // ARoll = 1.648;

    qDebug()<<"A Longtitude is "<<ALon<<"°, Lantitude is "<<ALan<<"°,Altitude is "<<AAlt<<"km.";
    qDebug()<<"B Longtitude is "<<BLon<<"°, Lantitude is "<<BLan<<"°,Altitude is "<<BAlt<<"km.";
    qDebug()<<"A Heading is "<<AHeading<<"°, Pitch is "<<APitch<<"°,Roll is "<<ARoll<<"°.";


    QVector<double> res = calRes(ALon,ALan,AAlt,BLon,BLan,BAlt,AHeading,APitch,ARoll); //返回指向的方位角和俯仰角度数 E-->Z, N-->U,DEGEE

    rs->move(0,2,res.at(0)*67108864/360);
    rs->move(0,3,res.at(1)*67108864/360);

    qDebug() <<"Single Close Loop track done";

}


void MainWindow::on_pushButton_12_clicked()
{
    rs->stop();
    loopState= false;
    // task->join();
    qDebug() <<"Open Loop track";

}


void MainWindow::on_pushButton_13_clicked()
{
    double zDegree;
    if(ui->lineEdit_12->text().isEmpty()){
        zDegree = 0.0;
    }else{
        if(ui->lineEdit_12->text().toDouble()<0 ||ui->lineEdit_12->text().toDouble()>360 ){
            QMessageBox::critical(this,"Error","Z Degree < 0 or Z Degree > 360");
            return;
        }
        zDegree = ui->lineEdit_12->text().toDouble();
    }

    double uDegree;
    if(ui->lineEdit_13->text().isEmpty()){
        uDegree = 0.0;
    }else{
        if(ui->lineEdit_13->text().toDouble()<0 ||ui->lineEdit_13->text().toDouble()>360 ){
            QMessageBox::critical(this,"Error","U Degree < 0 or U Degree > 360");
            return ;
        }
        uDegree = ui->lineEdit_13->text().toDouble();
    }



    rs->move(1,2,zDegree*67108864/360);
    rs->move(1,3,uDegree*67108864/360);

}


void MainWindow::on_pushButton_14_clicked()
{

    loopState =true ;
    qDebug() <<"Start RT Close Loop track";
    // task = std::thread(&MainWindow::closeloop,this);
    task = new std::thread(&MainWindow::closeloop,this);
}