#include "frmmapgps.h"
#include "quihelper.h"
#include "webview.h"
#include "maphelper.h"
#include "mapbaidu.h"
#include "QSound"
#include "math.h"
#include "ui_frmmapgps.h"
#include <QStringBuilder>

#define DEVICE_SIZE 40

frmMapGps::frmMapGps(QString mapKey,QWidget *parent) :
    QWidget(parent),
    bInited(false),
    bLoaded(false),
    ui(new Ui::frmMapGps)
{
    ListGPSPoints.clear();
    ListGPSPoints.reserve(1000000);
    ui->setupUi(this);
    this->MapKey = mapKey;
    this->initForm();
}

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

void frmMapGps::showEvent(QShowEvent *)
{
}
//调试状态
bool frmMapGps::CheckBeingDebugged()
{
    return  IsDebuggerPresent();
}

//--------------------------------------------------------
#define DefineRow 30//HMI界面刷新的总行数
//----------------------
void frmMapGps::AddPoint(GPS *pGPS)
{
    if(!bLoaded)
        return;

    double longitude = pGPS->BD09Longitude;
    double Latitude = pGPS->BD09Latitude;
    long count = ListGPSPoints.count();
    if(count==0)
    {
        ListGPSPoints.append(GPSPoint(longitude,Latitude));
        //qDebug() <<"longtude = "<<  longitude <<"latitude  = "<< Latitude<<" Counts =" << count << "\r\n";
        //ListGPSPoints.append(GPSPoint(longitude+0.003,Latitude+0.003));
        AddDeviceMark();
        moveMarker();
        //显示的第一条
        ui->tableWidgetSource->setRowCount(1);
        addItem2(ui->tableWidgetSource,0, ListGPSPoints[0].toString());
    }
    else
    {
       //求和上一点的距离,如果经纬度偏差不超过(1m),不予添加
       GPSPoint lastpoint = ListGPSPoints[count-1];
       double  latitudeError  = fabs(lastpoint.latitude-Latitude);
       double  longtudeError  =  fabs(lastpoint.longtude-longitude);
       double error_torrent = 0.00001f;
       double error_max =0.1f;//11.1km
       //if((latitudeError>error_torrent)||(longtudeError>error_torrent)||CheckBeingDebugged())
       if( ((latitudeError>error_torrent)||(longtudeError>error_torrent)) &&
           ((latitudeError< error_max)||(longtudeError< error_max)) )
        {
           //----------------------------------------------------
           // 调试的时候增加随机值
           //if(CheckBeingDebugged())
           //{
           //    srand((unsigned int)time(0));//初始化种子为随机值
           //    int randomvalue = rand()%15;
           //    longitude+=randomvalue*0.001f;
           //    randomvalue = rand()%5;
           //    Latitude+=randomvalue*0.001f;
           // }
           //----------------------------------------------------
           ListGPSPoints.append(GPSPoint(longitude,Latitude));
           moveMarker();

           //----------------------------------------
           long count1 = ListGPSPoints.count();
           //qDebug() <<"ListGPSPoints.count = " <<count1;
           //注释掉 Table 刷新,压力测试,还是存在严重卡顿,不是TAB刷新问题
           if(count1<DefineRow+1){ ui->tableWidgetSource->setRowCount(count1); }
           else{ ui->tableWidgetSource->setRowCount(DefineRow+1); }

           addItem2(ui->tableWidgetSource, count1-1, ListGPSPoints[count1-1].toString());
           //滚动条滚到最下面,
           //ui->tableWidgetSource->scrollToBottom();
        }       
    }
}

//==============================================================
void frmMapGps::UpdateGPSInfo(GPS *pGPS)
{
    //longitude 经度
    //Latitude  纬度
    if( ( pGPS->gga_data.quality==1)||(pGPS->gga_data.quality==2))//   GGA =1 定位有效
    //if((pGPS->rmc_data.pos_status !='N')&&(pGPS->rmc_data.mode_ind !='N'))//   RMC = N 定位无效
    {
        // 压力测试 跑到 20k行
        // pGPS->BD09Longitude = 116.405395;//+qrand();
        // pGPS->BD09Latitude = 39.899867;//+qrand();
        //  for(int i =0; i<100;i++)//压力测试
        KalMan_main(pGPS);
        AddPoint(pGPS);
    }
}
//-----------------------------------------------------------------------
void frmMapGps::initForm()
{

    //设置右侧固定宽度
    ui->widgetRight->setFixedWidth(AppData::RightWidth);
    //选项卡居中
    //ui->tabWidget->setStyleSheet("QTabWidget::tab-bar{alignment:center;}");
    ui->tabWidget->setStyleSheet("QTabWidget::tab-bar{alignment:left;}");
    //实例化百度地图类
    baidu = new MapBaiDu(this);
    //读取商用AK 并设置
    baidu->setMapVersionKey(MapKey);
    //实例化通用浏览器控件
    web = new WebView(this);
    //加入到布局
    web->setLayout(ui->gridLayout);
    //关联浏览器控件信号
    connect(web, SIGNAL(receiveDataFromJs(QString, QVariant)),
            this, SLOT(receiveDataFromJs(QString, QVariant)));

    connect(web, SIGNAL(loadFinished(bool)),
            this, SLOT(loadFinished(bool)));

    //定时器模拟轨迹
    //timer = new QTimer(this);
    //connect(timer, SIGNAL(timeout()), this, SLOT(moveMarker()));

    QMetaObject::invokeMethod(this, "loadMap", Qt::QueuedConnection);
    //--------------卡尔曼滤波初始化------------------------------------
     //kalm_init(0.4);//采样周期与GPS模块配置速率相同



}
//TODO:
//1.根据GPS坐标下载离线地图
//2.根据坐标显示地图
//
void frmMapGps::loadMap()
{

    QString fileName = QString("%1/map_web.html").arg(ConfigPath);
    QString url = "file:///" + fileName;
    //设置缩放级别
    baidu->setMapZoom(15);
    //设置单击获取经纬度
    baidu->setEnableClickPoint(false);
    //设置默认的中心点坐标
   // baidu->setMapCenterPoint("117.171043,36.198196");

    ListGPSPoints.clear();

    //设置离线地图
    baidu->setMapLocal(true);//true 离线
    baidu->setShowNavigationControl(true);//显示放大工具

    baidu->setSaveFile(SaveFile);
    baidu->setFileName(fileName);

    QString html = baidu->newMap();

    //写入文件【重点】
    /*
    QFile file;
    file.setFileName("config/map_web.html");
    if(file.open(QIODevice::WriteOnly |QIODevice::Text)){
        QTextStream stream(&file);
        stream.setCodec("utf-8");
        stream<<html<<"\n";
        file.close();
    }
    */

    //两种方式加载,一种是传入html文件,一种是html内容
    if (baidu->getSaveFile()) {
        web->load(url);
    } else {
        QString baseUrl = QString("%1/").arg(ConfigPath);
        web->load("", html, baseUrl);
    }

}

void frmMapGps::setInfo(int angle, int index, int count)
{
    QString info = QString("角度 %1°/第 %2 个/共 %3 个").arg(angle).arg(index).arg(count);
//    ui->labTip->setText(info);
}
//----------------------------------------------------------------------------------------
void frmMapGps::initTable()
{
    initTable(ui->tableWidgetSource);
}

void frmMapGps::initTable(QTableWidget *tableWidget)
{
    //初始化表格控件
    QUIHelper::initTableView(tableWidget, AppData::RowHeight);
    tableWidget->setColumnCount(2);
    tableWidget->setHorizontalHeaderLabels(QStringList() << "序 号" << "轨迹经纬度");
    tableWidget->setColumnWidth(0, 100);
    tableWidget->setColumnWidth(1, 170);
    //tableWidget->setRowCount(1);
}

void frmMapGps::addItem(QTableWidget *tableWidget, long index, const QString &point,const QString  ppm)
{
    //编号
    QTableWidgetItem *itemId = new QTableWidgetItem;
    itemId->setText(QString::number(index + 1));
    itemId->setTextAlignment(Qt::AlignCenter);
    tableWidget->setItem(index, 0, itemId);

    QTableWidgetItem *itemPoint = new QTableWidgetItem;
    //重新过滤小数点,对齐更好看
    itemPoint->setText(MapHelper::getLngLat2(point));
    //设置原始数据更准确
    itemPoint->setData(Qt::UserRole, point);
    tableWidget->setItem(index, 1, itemPoint);

    if(ppm != "" )
    {
        QTableWidgetItem *itemppm = new QTableWidgetItem;
        itemppm->setText(ppm);
        itemppm->setTextAlignment(Qt::AlignCenter);
        tableWidget->setItem(index, 2, itemppm);

    }
}
//----------------------------------------------

//-------------------
void frmMapGps::addItem2(QTableWidget *tableWidget, long index, const QString &point)
{

    int DisplayRow =DefineRow;

    if(index < DisplayRow)
    {
        //编号
        QTableWidgetItem *itemId = new QTableWidgetItem;
        itemId->setText(QString::number(index + 1));
        itemId->setTextAlignment(Qt::AlignCenter);
        tableWidget->setItem(index, 0, itemId);

        QTableWidgetItem *itemPoint = new QTableWidgetItem;
        //重新过滤小数点,对齐更好看
        itemPoint->setText(MapHelper::getLngLat2(point));
        //设置原始数据更准确
        itemPoint->setData(Qt::UserRole, point);
        tableWidget->setItem(index, 1, itemPoint);
    }
    else //从第 DisplayRow = 29行开始执行
    {
        tableWidget->removeRow(0);//删除第一行,第二行变为第一行
        //int CRow =tableWidget->rowCount();
        //编号
        QTableWidgetItem *itemId = new QTableWidgetItem;
        itemId->setText(QString::number(index + 1));
        itemId->setTextAlignment(Qt::AlignCenter);
        tableWidget->setItem(DisplayRow -1, 0, itemId);

        QTableWidgetItem *itemPoint = new QTableWidgetItem;
        //重新过滤小数点,对齐更好看
        itemPoint->setText(MapHelper::getLngLat2(point));
        //设置原始数据更准确
        itemPoint->setData(Qt::UserRole, point);
        tableWidget->setItem( DisplayRow-1 , 1, itemPoint);
        //设置表格行数,调用必须设置行数,这里注释
        //tableWidget->setRowCount(DisplayRow);
     }
}
//----------------------------------------------

void frmMapGps::loadFinished(bool bFinished)
{
    bLoaded = bFinished;
    if(bLoaded&&!bInited)
    {
        initTable();
        bInited = true;
       // qDebug() <<   "frmMapGps Load = " <<bFinished;
    }
    //AddDeviceMark();
}

void frmMapGps::receiveDataFromJs(const QString &type, const QVariant &data)
{
    if (data.isNull()) {
        return;
    }

    //qDebug() << TIMEMS << "frmMapGps" << type << data;
    QString result = data.toString();

}

void frmMapGps::runJs(const QString &js)
{
    web->runJs(js);
}


void frmMapGps::AddDeviceMark()
{
    if(name!="采样位置")
    {
        name =  "采样位置";
        //第一步: 添加一个标记
        //图片文件在可执行文件下的config/device目录
        QString icon = "./device/device_airplane.png";
        int size = 60;
        QString js = QString("addMarker('%1', '', '', '', 60, '%1', 0, 0, '%2', %3)").arg(name).arg(icon).arg(size);
        runJs(js);
    }
}

//void frmMapGps::moveMarker()
//{

//    double angle = 0;
//    int count = ListGPSPoints.count();
//    int row = count-1;
//    QString point = ListGPSPoints[row].toString();

//    //第一个点和最后一个点不用处理
//    if (row > 1 )//&& row < count - 1)
//    {
//        //上一个点坐标
//        QString point2 = ListGPSPoints[row - 1].toString();
//        //计算当前上一个点和当前点的旋转角度
//        angle = MapHelper::getAngle(point2, point);
//    }
//    //-------------------------------------------------
//    //执行移动设备点函数,参数带旋转角度
//    QString js = QString("moveMarker('%1', '%2', %3)").arg(name).arg(point).arg(angle);
//    runJs(js);
//    //重新绘制轨迹点
//    //清空之前的轨迹点
//    js = QString("deleteOverlay('Polyline')");
//    runJs(js);

//    //取出第一个点到当前焦点所在行的点组成已经走过的轨迹点集合重新绘制
//    QStringList points;

//    for (int i = 0; i <= row; ++i)
//    {
//        points <<ListGPSPoints[i].toString();
//    }
//    //绘制路径集合 具体参数含义参见 mapbaidu.cpp 函数对应处
//    QString strPoints = points.join("|");
//    //设置轨迹颜色粗细及不透明度
//    js = QString("addPolyline('%1', '#ff0000', 8, 0.5)").arg(strPoints);
//    runJs(js);
//    //移动中心点到当前点,地图跟随
//    js = QString("setCenter('%1')").arg(point);
//    runJs(js);
//}
void frmMapGps::moveMarker()
{

    double angle = 0;
    int count = ListGPSPoints.count();
    int row = count-1;
    QString point = ListGPSPoints[row].toString();

    //第一个点和最后一个点不用处理
    if (row > 1 )//&& row < count - 1)
    {
        //上一个点坐标
        QString point2 = ListGPSPoints[row - 1].toString();
        //计算当前上一个点和当前点的旋转角度
        angle = MapHelper::getAngle(point2, point);
    }
    //-------------------------------------------------
    //执行移动设备点函数,参数带旋转角度
    QString js = QString("moveMarker('%1', '%2', %3)").arg(name).arg(point).arg(angle);
    runJs(js);

    //清空之前的轨迹点
    /*
    //清除划线图层
    js = QString("deleteOverlay('Polyline')");
    runJs(js);

    //取出第一个点到当前焦点所在行的点组成已经走过的轨迹点集合重新绘制
    QStringList points;

    for (int i = 0; i <= row; ++i) {
        points <<ListGPSPoints[i].toString();
    }
    */
    //不删除已有的点,只显示新增的点
    switch(count )
    {
        case 1:
        break;
        case 2:
        {
            QStringList points;
            for (int i = count-1; i <  count; ++i)
            {
                points <<ListGPSPoints[i].toString();
            }
            //绘制路径集合 具体参数含义参见 mapbaidu.cpp 函数对应处
            QString strPoints = points.join("|");
            //设置轨迹颜色粗细及不透明度
           // js = QString("addPolyline('%1', '#ff0000', 5, 0.5)").arg(strPoints);
            js = QString("addPolyline('%1', '#e60000', 5, 0.5)").arg(strPoints);//color:rgb(128,0,32)
            runJs(js);
        }
        break;
        default:
        {
            //设置轨迹颜色粗细及不透明度
           // js = QString("addPolylinePoint('%1', '#ff0000', 5, 0.5)").arg(point);
            js = QString("addPolylinePoint('%1', '#e60000', 5, 0.5)").arg(point);
            runJs(js);
        }
        break;
    }
    //移动中心点到当前点,地图跟随
    js = QString("setCenter('%1')").arg(point);
    runJs(js);
}
//==============================================================================

void frmMapGps::tableSelectionChanged()
{
    int index = ui->tableWidgetSource->currentRow();
    if(index >=0)
    {
        QString point = ListGPSPoints[index].toString();
        QString js = QString("setCenter('%1')").arg(point);
        runJs(js);
    }
}

void frmMapGps::on_tableWidgetSource_itemSelectionChanged()
{
   // tableSelectionChanged();
}

void frmMapGps::on_tableWidgetSource_cellDoubleClicked(int row, int column)
{
    tableSelectionChanged();
}
//////=================================================================================
///
/*
static mat_t F(4, 4, 0.0);
static mat_t H(2, 4, 0.0);
static mat_t P0(4, 4, 0.0);
static mat_t I(4, 4, 0.0);
static mat_t Xkf(4, 1, 0.0);
static mat_t Q(4, 4, 0.0);
static mat_t R(2, 2, 0.0);
static mat_t Z(2, 1, 0.0);
*/

mat_t frmMapGps::kalm_pool(double x, double y)
{
    Z.write(0, 0, x);
    Z.write(1, 0, y);
    mat_t X_pre = F * Xkf;
    mat_t F_trans = F.trans();
    mat_t P_pre = (F * P0 * F_trans) + Q;
    mat_t H_trans = H.trans();
    mat_t K1 = (H * P_pre * H_trans + R);
    mat_t K1_inv = K1.inv();
    mat_t K = P_pre * H_trans * K1_inv;
    mat_t KH = K * H;
    mat_t P0_temp = (I - KH) * P_pre;
    mat_t HX_pre = H * X_pre;
    mat_t temp = Z - HX_pre;
    mat_t Xkf_temp = K * temp + X_pre;
    P0.copy(P0_temp);
    Xkf.copy(Xkf_temp);
    // Xkf.trans().print();
    return Xkf;
}
void frmMapGps::kalm_init(double T)
{
    F.write(0, 0, 1.0);
    F.write(0, 1, T);
    F.write(1, 1, 1.0);
    F.write(2, 2, 1.0);
    F.write(2, 3, T);
    F.write(3, 3, 1.0);
    H.write(0, 0, 1.0);
    H.write(1, 2, 1.0);
    P0.write(0, 0, 1.0);
    P0.write(1, 1, 1.0);
    P0.write(2, 2, 1.0);
    P0.write(3, 3, 1.0);
    I.write(0, 0, 1.0);
    I.write(1, 1, 1.0);
    I.write(2, 2, 1.0);
    I.write(3, 3, 1.0);
    Q.write(0, 0, 0.05);
    Q.write(1, 1, 0.1);
    Q.write(2, 2, 0.05);
    Q.write(3, 3, 0.1);
    R.write(0, 0, 100.0);
    R.write(1, 1, 100.0);
}


void frmMapGps::KalMan_main(GPS *pGPS)
{
    double x= pGPS->BD09Latitude;
    double y=pGPS->BD09Longitude;

    double err=0.0001;

    double errLatitude;
    double errLongitude;

    kalm_pool(x,y);

    x= Xkf.read(0,0);
    y=Xkf.read(2,0);

    errLatitude  = abs(pGPS->BD09Latitude - x );
    errLongitude = abs(pGPS->BD09Longitude - y);
    //----------过滤掉前期滤波器未稳定时的数据------------
    if( (errLatitude < err ) && (errLongitude <err) )
    {
       pGPS->BD09Longitude=y;
       pGPS->BD09Latitude =x;
    }
    else
    {
      qDebug("x = %f  ",x);
      qDebug("y = %f\r\n",y);
    }

}