#include "polarxmainwindow.hpp"

using namespace std ;

PolarxMainWindow::PolarxMainWindow() : QWidget()
{

    this->setFixedSize(800,600);

    QTabWidget *tab = new QTabWidget( this );
    tab->resize( this->size() );

    QWidget *recPage = new QWidget ;
    initStatusBlock( recPage );
    tab->addTab( recPage , "Receiver Status" ) ;

    QWidget *sbfPage = new QWidget ;
    initSbfBlock(sbfPage);
    tab->addTab( sbfPage , "SBF Block" ) ;

    QWidget *setupPage = new QWidget ;
    initSetupBlock(setupPage);
    tab->addTab( setupPage , "Setup" ) ;

    show() ;

    connect( upFrame , SIGNAL( clicked() ) , this , SLOT( sendNewFrame() ) );
    connect( upSetup , SIGNAL( clicked() ) , this , SLOT( sendNewSetup() ) ) ;
    ipID = 0 ;
    elevMask_set = 100 ;
}

void PolarxMainWindow::setData( unsigned char * data , int buf_size )
{

    // $PolaRx:
    if ( data[0] == '$' & data[1] == 'P' )
    {
        QString cmd ;
        for ( int i = 0 ; i < buf_size ; i++ )
            cmd.append( data[i] );
        commandManager(cmd.split( "#EoL" ));
    }

}

void PolarxMainWindow::pvtError( QString err )
{
    PVT_lat->setText( err );
    PVT_lon->setText( QString::null );
    PVT_elv->setText( QString::null );
}

void PolarxMainWindow::setSocket( SeptentrioSocket * socket )
{
    socketIf_ = socket ;
    socketIf_->sendToServer("GetSBFOutput curr" ) ;
    socketIf_->sendToServer("GetPVTInterval" ) ;
    socketIf_->sendToServer("GetPVTMode" ) ;
    socketIf_->sendToServer("GetElevationMask" ) ;
    socketIf_->sendToServer("GetMeasInterval"  );
    socketIf_->sendToServer("GetTrackingMode" );
}

void PolarxMainWindow::getAvailableFrame( QString cmd )
{
    //    $PolaRx: SetSBFOutput IP10, PVTgeo #EoL
    cmd.remove( 0 , 27 ) ;
    avFrame = cmd.simplified().split("+");

    for ( QMap< QString , QCheckBox* >::iterator I = sbfBlock_disp.begin() ; I != sbfBlock_disp.end() ; I++ )
    {
        if ( avFrame.contains( I.key() ) )     I.value()->setChecked( true ) ;
        else                                   I.value()->setChecked( false );
    }

    if ( !avFrame.contains("Status") | !avFrame.contains("PVTgeo") )
    {
        avFrame << "Status" << "PVTgeo" ;
        requestFrame();
    }

}

void PolarxMainWindow::requestFrame()
{
    QString cmd = QString("SetSBFOutput IP%1 %2").arg(ipID).arg(avFrame.join("+") ) ;
    socketIf_->sendToServer( cmd );
}

void PolarxMainWindow::initSbfBlock(QWidget *  parent)
{
    QMap<QString,QStringList> bname ;

    bname["Raw Data"] << "Meas" << "ShortMeas" << "GenMeas" << "Nav" << "Alm" << "IonUtc" << "MeoRaw"<< "Rinex" ;
    bname["PVT"] << "PVTcar" << "PVTgeo" << "CovCar" << "CovGeo" << "DOP" << "PVTRes" << "RAIM" ;
    bname["SBAS"] << "GEOcorr" << "GEOInfo" ;
    bname["Attitude"] << "AttEuler" << "CovEuler" << "AttQuat" << "CovQuat" << "AuxPos" ;
    bname["Monitoring"] << "PPS" << "Commands" << "Off" << "RxInfo" << "Status" << "EndOfEpoch" << "DiffCorr" << "BaseInfo" << "BaseLine" << "ExtEvent" ;

    QVBoxLayout * layout_ = new QVBoxLayout ;
    for ( QMap<QString,QStringList>::iterator Il = bname.begin() ; Il != bname.end() ; Il++ )
    {
        QGroupBox * box = new QGroupBox( Il.key() , parent );
        QGridLayout * layout = new QGridLayout ;
        box->setLayout( layout );
        int l = 0 ;
        int c = 0 ;
        for ( QStringList::iterator Iv = Il.value().begin() ; Iv != Il.value().end() ; Iv++ )
        {
            sbfBlock_disp[*Iv] = new QCheckBox(*Iv,box);
            layout->addWidget(sbfBlock_disp[*Iv],l,c);
            if (c++ == 4 )
            {
                c = 0 ;
                l++ ;
            } ;
        }
        layout_->addWidget( box );
    }

    upFrame = new QPushButton("Send",parent);
    layout_->addWidget( upFrame );
    parent->setLayout( layout_ );

}

void PolarxMainWindow::sendNewFrame()
{
    avFrame.clear() ;
    for ( QMap< QString , QCheckBox* >::iterator I = sbfBlock_disp.begin() ; I != sbfBlock_disp.end() ; I++ )
        if ( I.value()->isChecked() )
            avFrame << I.key() ;
    requestFrame() ;
}

void PolarxMainWindow::initStatusBlock( QWidget * parent )
{
    //! Receiver status page
    TOW = new QLabel( parent );
    WNC = new QLabel( parent );
    RXLOAD = new QLabel( parent );
    UPTIME = new QLabel( parent );
    CHAN_USED = new QLabel( parent );
    TRACKINGSTATUS = new QStandardItemModel( 1 , 1 );
    PVT_lat = new QLabel( parent );
    PVT_lon = new QLabel( parent );
    PVT_elv = new QLabel( parent );
    SYSTEM = new QLabel( parent );
    SYSTEM2 = new QLabel( parent );
    SYSTEM3 = new QLabel( parent );
    MODE = new QLabel( parent );
    PVT_USED = new QLabel( parent );

    QVBoxLayout * rmain_layout = new QVBoxLayout ;
    QGroupBox * gpstime = new QGroupBox( "GPS Time" , parent );
    QGroupBox * tracking = new QGroupBox( "Tracking" , parent );
    QGroupBox * status = new QGroupBox( "Status" , parent );
    rmain_layout->addWidget( gpstime );
    rmain_layout->addWidget( status );
    rmain_layout->addWidget( tracking );

    QGridLayout * ssub_layout = new QGridLayout ;
    ssub_layout->addWidget( new QLabel("Mode : ") ,0,0 );
    ssub_layout->addWidget( MODE ,0,1 );
    ssub_layout->addWidget( CHAN_USED ,0,2 );
    ssub_layout->addWidget( PVT_USED ,0,3 );
    ssub_layout->addWidget( new QLabel("System : ") ,2,0 );
    ssub_layout->addWidget( SYSTEM ,2,1 );
    ssub_layout->addWidget( SYSTEM2 ,2,2 );
    ssub_layout->addWidget( SYSTEM3 ,2,3 );
    ssub_layout->addWidget( new QLabel("PVT : ") ,3,0 );
    ssub_layout->addWidget( PVT_lat ,3,1 );
    ssub_layout->addWidget( PVT_lon ,3,2 );
    ssub_layout->addWidget( PVT_elv ,3,3 );
    status->setLayout( ssub_layout );


    QHBoxLayout * gp_layout = new QHBoxLayout;
    gp_layout->addWidget(WNC);
    gp_layout->addWidget(TOW);
    gp_layout->addWidget( UPTIME );
    gp_layout->addWidget( RXLOAD );
    gpstime->setLayout( gp_layout ) ;

    QTableView * trackingView = new QTableView ;

    trackingView->setModel( TRACKINGSTATUS );
    QHBoxLayout * t_layout =  new QHBoxLayout ;
    tracking->setLayout( t_layout );
    t_layout->addWidget( trackingView );

    parent->setLayout(rmain_layout);
}

void PolarxMainWindow::initSetupBlock( QWidget * parent )
{
    QVBoxLayout * layout = new QVBoxLayout() ;
    QGridLayout * blayout = new QGridLayout();
    upSetup = new QPushButton("Send",parent);
    layout->addLayout(blayout);
    layout->addWidget(upSetup);
    parent->setLayout(layout);

    QGroupBox * pvt_ = new QGroupBox("PVT",parent);
    blayout->addWidget( pvt_ , 0 , 0 );
    QGroupBox * tracking_ = new QGroupBox("Tracking",parent);
    blayout->addWidget( tracking_ , 1 , 0 );
    QGroupBox * meas_ = new QGroupBox("Measurements",parent);
    blayout->addWidget( meas_ , 0,1,2,1);

    // PVT
    QGridLayout * pvtLayout = new QGridLayout ;
    pvt_->setLayout( pvtLayout );
    pvtLayout->addWidget( new QLabel("Mode :") , 0 , 0 );
    pvtLayout->addWidget( new QLabel("PVT Frequency :") , 3 , 0 );
    pvtLayout->addWidget( new QLabel("ClockSync Thres. :") , 4 , 0 );
    pvtLayout->addWidget( new QLabel("PPS Rate :") , 5 , 0 );

    QStringList temp_ = QStringList() << "standalone" << "SBAS" << "DGPS" << "RTKfloat" << "RTK" ;
    int n = 0 ;
    for ( QStringList::iterator I = temp_.begin() ; I !=temp_.end() ; I++ )
    {
        pvtMode[*I] = new QCheckBox( *I , parent );
        pvtLayout->addWidget( pvtMode[*I] , n++ , 1 );
    }

    pvtInterval = new QComboBox() ;
    pvtInterval->addItems( QStringList() << "0.10" << "0.20" << "0.50" << "1.00" ) ;
    pvtLayout->addWidget( pvtInterval , 3 , 1 ) ;

    // Meas
    QGridLayout * measLayout = new QGridLayout ;
    meas_->setLayout( measLayout );
    measLayout->addWidget( new QLabel("Meas. Interval :") , 0 , 0 );
    measLayout->addWidget( new QLabel("Multipath Mitigation :") , 1 , 0 );
    measLayout->addWidget( new QLabel("Meas. Smoothing :") , 2 , 0 );
    measLayout->addWidget( new QLabel("Meas. Fit :") , 3 , 0 );

    measInterval = new QComboBox() ;
    measInterval->addItems( QStringList() << "0.10" << "0.20" << "0.50" << "1.00" ) ;
    measLayout->addWidget( measInterval , 0 , 1 ) ;

    // Tracking
    QGridLayout * trkLayout = new QGridLayout ;
    tracking_->setLayout( trkLayout );
    trkLayout->addWidget( new QLabel("Tracking Mode :") , 0 , 0 );
    trkLayout->addWidget( new QLabel("Channel Alloc :") , 1 , 0 );
    trkLayout->addWidget( new QLabel("Elevation Mask :") , 2 , 0 );
    trkLayout->addWidget( new QLabel("Tracking Sens. :") , 3 , 0 );

    trkMode = new QComboBox() ;
    trkMode->addItems( QStringList() << "dynamic" << "static" << "timing" );
    trkLayout->addWidget( trkMode , 0 , 1 );

    elevMask = new QSpinBox() ;
    elevMask->setRange(-90,90);
    elevMask->setSingleStep(1) ;
    trkLayout->addWidget(elevMask,2,1);

}

void PolarxMainWindow::sendNewSetup()
{
    QStringList tosend ;
    for ( QMap< QString , QCheckBox * >::iterator I = pvtMode.begin() ; I != pvtMode.end() ; I++ )
    {
        if ( I.value()->isChecked() )
        {
            tosend << I.key();
            socketIf_->sendToServer( QString("SetPVTMode %1").arg(tosend.join("+")) );
        }
    }

    if ( pvtInterval->currentText() != pvtInt_set )
        socketIf_->sendToServer( "SetPVTInterval " + pvtInterval->currentText() );

    if ( elevMask->value() != elevMask_set )
        socketIf_->sendToServer( QString("SetElevationMask %1").arg(elevMask->value())  );

    if ( measInterval->currentText() != measInt_set )
        socketIf_->sendToServer( "SetMeasInterval " + measInterval->currentText() );

    if ( trkMode->currentText() != trkMode_set )
        socketIf_->sendToServer( "SetTRackingmode " + trkMode->currentText() );

}

void PolarxMainWindow::extractIpID( QString cmd )
{
    QString ip_ ;
    ip_.append( cmd.at(24) );
    ip_.append( cmd.at(25) );
    ipID = ip_.toInt() ;
}

void PolarxMainWindow::getPvtInterval( QString cmd )
{
    cmd.remove(0,23);
    int idx = pvtInterval->findText( cmd.simplified() ) ;
    pvtInterval->setCurrentIndex( idx );
    pvtInt_set = cmd.simplified() ;
}

void PolarxMainWindow::getPvtMode( QString cmd )
{
    if ( cmd.at(6) == 'x' )
    {
        cmd.remove(0,20);
        cmd.truncate( cmd.indexOf( "$" ) );
        QStringList avMod = cmd.simplified().split("+");
        for ( QMap< QString , QCheckBox * >::iterator I = pvtMode.begin() ; I != pvtMode.end() ; I++ )
            if ( avMod.contains( I.key() ) )
                I.value()->setChecked( true );
    }
}

void PolarxMainWindow::getElevationMask( QString cmd )
{
//    $PolaRx: SetElevationMask -10 #EoL
    QString tmp_ ;
    for (int k = 26 ; k < cmd.size() ; k++ )
        tmp_.append(cmd.at(k)) ;
//     tmp_.append(cmd.at(26)) ;
//     tmp_.append(cmd.at(27)) ;
//     if ( tmp_.at(1) == '-' ) tmp_.append(cmd.at(28)) ;
    elevMask->setValue( tmp_.toInt() );
    elevMask_set = tmp_.toInt() ;
}

void PolarxMainWindow::getMeasInterval( QString cmd )
{
    cmd.remove(0,24);
    int idx = measInterval->findText( cmd.simplified() ) ;
    measInterval->setCurrentIndex( idx );
    measInt_set = cmd.simplified() ;
}

void PolarxMainWindow::getTrackingMode(QString cmd)
{
//    $PolaRx: SetTRackingmode dynamic
    cmd.remove(0,24);
    int idx = trkMode->findText( cmd.simplified() ) ;
    trkMode->setCurrentIndex( idx );
    trkMode_set = cmd.simplified() ;
}

void PolarxMainWindow::commandManager( QStringList cmdList )
{
    for ( QStringList::iterator cmd = cmdList.begin() ; cmd != cmdList.end() ; cmd++ )
    {
        QString cmd_ = (*cmd).simplified() ;
        qDebug() << cmd_ ;
        bool toto = cmd_.contains( "SetSBFOutput") ;
        if ( (ipID == 0) & toto )                                            extractIpID( cmd_ );
        if ( cmd_.contains( QString("SetSBFOutput IP%1").arg(ipID) ) )     getAvailableFrame( cmd_ ) ;
        if ( cmd_.contains( "SetPVTInterval" ))                            getPvtInterval( cmd_ ) ;
        if ( cmd_.contains( "SetPVTMode" ) )                               getPvtMode( cmd_ );
        if ( cmd_.contains( "SetElevationMask" ) )                         getElevationMask( cmd_ );
        if ( cmd_.contains( "SetMeasInterval" ) )                          getMeasInterval( cmd_ );
        if ( cmd_.contains( "SetTRackingmode" ) )                          getTrackingMode( cmd_ );
    }
}

void PolarxMainWindow::setReceiverStatus( SbfDataReceiverStatus * msg )
{
    RXLOAD->setText( QString("Load: %1%").arg( msg->CPULoad ) ) ;
    UPTIME->setText( QString("Uptime: %1s").arg( msg->UpTime )) ;

    bool CPU_OVERLOAD = ( msg->RxState >> 7 )&1 ;
//     bool CPU_OVERLOAD = ( msg->RxState&64 ) == 64  ;
    if (CPU_OVERLOAD)
        RXLOAD->setStyleSheet("background-color:red");
    else
        RXLOAD->setStyleSheet("");
}

void PolarxMainWindow::setTrackingStatus( SbfDataTrackingStatus * msg )
{
    CHAN_USED->setText( QString("Tracking: %1").arg(msg->N) ) ;
    for ( int k = 0 ; k < msg->N ; k++ )
    {
       TRACKINGSTATUS->setItem( k , 0 , new QStandardItem( QString("%1").arg( msg->ChannelData[k].RxChannel ) ) );
        TRACKINGSTATUS->setItem( k , 1 , new QStandardItem( QString("%1").arg( msg->ChannelData[k].SVID ) ) ) ;

        unsigned char AntFlag = msg->ChannelData[k].main_tracking ;
        if ( AntFlag == 0 )    TRACKINGSTATUS->setItem( k , 3 , new QStandardItem( "-" ) );
        else if ( AntFlag == 1 )     TRACKINGSTATUS->setItem( k , 3 , new QStandardItem( "Searching" ));
        else if ( AntFlag == 2 )     TRACKINGSTATUS->setItem( k , 3 , new QStandardItem( "Sync."  ));
        else if ( AntFlag == 3 )     TRACKINGSTATUS->setItem( k , 3 , new QStandardItem( "C/A" ));
        else if ( AntFlag == 4 )     TRACKINGSTATUS->setItem( k , 3 , new QStandardItem( "C/A+P(y)" ));
        else if ( AntFlag == 5 )     TRACKINGSTATUS->setItem( k , 3 , new QStandardItem( "C/A+L2C" ));
        else if ( AntFlag == 6 )     TRACKINGSTATUS->setItem( k , 3 , new QStandardItem( "C/A+P(y)+L2C" ));

        unsigned char PVTFlag = msg->ChannelData[k].PVT_usage ;
        if ( PVTFlag == 0 )          TRACKINGSTATUS->setItem( k,5 , new QStandardItem( "Not Used" ));
        else if ( PVTFlag == 1 )     TRACKINGSTATUS->setItem( k,5 , new QStandardItem( "Used" ));
        else if ( PVTFlag == 2 )     TRACKINGSTATUS->setItem( k,5 , new QStandardItem( "Rejected" ));

        TRACKINGSTATUS->setItem( k,2 , new QStandardItem( QString("%1 / %2").arg( (int)msg->ChannelData[k].Elevation ).arg( (int)msg->ChannelData[k].Azimuth ) )) ;

        int Health = (int)msg->ChannelData[k].Health;
        if ( Health == 0 )           TRACKINGSTATUS->setItem( k,4 , new QStandardItem( "Healty" )) ;
        else if ( Health == 1)       TRACKINGSTATUS->setItem( k,4 , new QStandardItem( "Unhealty" )) ;
        else if ( Health == 2)       TRACKINGSTATUS->setItem( k,4 , new QStandardItem( "-" )) ;
    }
    if ( TRACKINGSTATUS->rowCount() > msg->N )
       TRACKINGSTATUS->removeRows( TRACKINGSTATUS->rowCount() , TRACKINGSTATUS->rowCount()-msg->N ) ;
}

void PolarxMainWindow::setPVTGeodetic( SbfDataPVTGeodetic * msg )
{
    TOW->setText( QString("Tow : %1s").arg(msg->TOW) );
    WNC->setText( QString("Week : %1").arg(msg->WNc) );

    PVT_USED->setText( QString("PVT: %1").arg( msg->nrSV ) );
    if ( msg->error == 0 )
    {
        PVT_lat->setText( QString("%1").arg(msg->lat) );
        PVT_lon->setText( QString("%1").arg(msg->lon) );
        PVT_elv->setText( QString("%1").arg(msg->h  ) );
    }
    else if ( msg->error == 1 )  pvtError( "Not Enough Meas" );
    else if ( msg->error == 2 )  pvtError( "Not Enough Eph" );
    else if ( msg->error == 3 )  pvtError(  "DOP too large" );
    else if ( msg->error == 4 )  pvtError(  "SSR too large" );
    else if ( msg->error == 5 )  pvtError(  "No convergence" );
    else if ( msg->error == 6 )  pvtError(  "Not Enough Meas after outlier rejection" );
    else if ( msg->error == 7 )  pvtError(  "Unvalid Data" );
    else if ( msg->error == 8 )  pvtError(  "Not Enough Diff Corr" );
    else if ( msg->error == 9 )  pvtError(  "Base station coor unavailable" );

    if ( msg->mode == 0 )                MODE->setText( "No PVT" );
    else if ( msg->mode == 1 )           MODE->setText( "Standalone PVT" );
    else if ( msg->mode == 2 )           MODE->setText( "Differential PVT" );
    else if ( msg->mode == 3 )           MODE->setText( "Fixed Base" );
    else if ( msg->mode == 4 )           MODE->setText( "RTK fixed" );
    else if ( msg->mode == 5 )           MODE->setText( "RTK float" );
    else if ( msg->mode == 9 )           MODE->setText( "RTK fixed (FKP)" );
    else if ( msg->mode == 10 )          MODE->setText( "RTK float (FKP)" );
    else                            MODE->setText( "-" );

    QString sys ;
    if ( msg->gpsUsed & !msg->glonassUsed )
        sys += "GPS";
    else if ( !msg->gpsUsed & msg->glonassUsed )
        sys += "GLONASS" ;
    else if ( msg->gpsUsed & msg->glonassUsed )
        sys += "GPS+GLONASS" ;

    if ( !msg->PVTFix3D )
        sys += " (2D PVT)";
    else if ( msg->L1SignalUsed & !msg->L2SignalUsed )
        sys += " (L1)" ;
    else if ( !msg->L1SignalUsed & msg->L2SignalUsed )
        sys += " (L2)" ;
    else if ( msg->L1SignalUsed & msg->L2SignalUsed )
        sys += " (L1/L2)" ;
    SYSTEM->setText( sys );

    sys.clear() ;
    if ( msg->sbas == 0 )               sys += "No SBAS" ;
    else if ( msg->sbas == 1 )          sys += "EGNOS" ;
    else if ( msg->sbas == 2 )          sys += "WAAS" ;
    else if ( msg->sbas == 3 )          sys += "MSAS" ;
    else if ( msg->sbas == 15 )         sys += "Other" ;

    if ( msg->sbas )
    {
        sys += " (" ;
        if ( msg->sbasLTC )  sys += "LTC" ;
        if ( msg->sbasFC )  sys += "+FC" ;
        if ( msg->sbasIono )  sys += "+Iono" ;
        sys += ")";
        if ( msg->sbasPA )   sys += " - PA active" ;
        SYSTEM2->setText( sys );

        SYSTEM3->setText( QString("PRN : %1").arg(msg->sbasPrn) );
    }
}