/*********************************************************************
//  created:    2008/07/18 - 15:33
//  filename:   SeptentrioComponent.cpp
//
//  author:     Gerald Dherbomez & Clement Fouque
//              Copyright Heudiasyc UMR UTC/CNRS 6599
//
//  version:    $Id: $
//
//  purpose:
//
*********************************************************************/


#include <iostream>
#include <string>

#include <QTcpSocket>
#include <QApplication>

#include "SeptentrioComponent.h"
#include "SeptentrioSocket.h"
#include "kernel/ComponentFactory.h"

// Construct the factory
static ComponentFactory<SeptentrioComponent>* factory = new ComponentFactory<SeptentrioComponent>("SeptentrioComponent");




//////////////////////////////////////////////////////////////////////////
// Constructor
/////////////////////////////////////////////////////////////////////////
SeptentrioComponent::SeptentrioComponent(QString name) : ComponentBase(name)
{
    socketIf_ = new SeptentrioSocket();
    recording = false;
    computeGPSTk = false ;
    // default values
    host_ = "127.0.0.1";
    port_ = 80;
    frame_ = "" ;
    
    ui_ = 0 ;

    currentRoadtime = 0 ;
    currentTimerange = 0 ;

    // Create message handler:
    pHandleStream = new SerialCOM_Handle_Stream( false );

    // Define protocol to be handled
    pProtocolSBF = new Plrx::SerialCOM_Protocol_SBF();

    // Add message to decode to protocol
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_MeasEpoch() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ShortMeasEpoch() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GenMeasEpoch() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_EndOfMeas() );

    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSNav() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSAlm() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSIon() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSUtc() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSRaw() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_CNAVRaw() );

    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOMT00() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOPRNMask() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOFastCorr() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOIntegrity() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOFastCorrDegr() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEONav() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEODegrFactors() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEONetworkTime() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOAlm() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOIGPMask() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOLongTermCorr() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOIonoDelay() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOServiceLevel() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOClockEphCovMatrix() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEORaw() );

    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PVTCartesian() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PVTGeodetic() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PosCovCartesian() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PosCovGeodetic() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_VelCovCartesian() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_VelCovGeodetic() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_DOP() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PVTResiduals() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_RAIMStatistics() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOCorrections() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_BaseLine() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_EndOfPVT() );

    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ReceiverTime() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_xPPSOffset() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ExtEvent() );

    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_TrackingStatus() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ReceiverStatus() );
    pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ReceiverSetup() );

    // Add protocol to handler:
    pHandleStream->addProtocol( pProtocolSBF );

    /*
    recPos = gpstk::Position( 49.3994929 , 2.7988750 , 87.071 , gpstk::Position::Geodetic ) ;
    curPos = gpstk::Position( 49.3994929 , 2.7988750 , 87.071 , gpstk::Position::Geodetic ) ;
    curVel = Vector<double>(3) ;
    curTime = Vector<double>(2) ;
    
    pSolver = new polarxGPSTKsolver() ;
    */
    prevRoadTime = 0 ;

    // Creating Shared Memory
    pShMem = new QSharedMemory( "polarx_shmem" , this );
    if ( pShMem->create(16384) )
        qDebug() << "Shared Memory Segment created" ;
    else
    {
        qDebug() << pShMem->errorString() ;
        if ( pShMem->error() == 4 )
        {
            qDebug() << "Trying to attach ...." ;
            if ( pShMem->attach() )
                qDebug() << "done" ;
        }
    }
    
    // Initialize the shared memory
    pShMem->lock();
    memset(pShMem->data(), 0, pShMem->size());
    pShMem->unlock();

    // Creating System Sempahore
    pShmSem = new QSystemSemaphore( "polarx_sem" , 0 , QSystemSemaphore::Create );

}





//////////////////////////////////////////////////////////////////////////
// Destructor
//////////////////////////////////////////////////////////////////////////
SeptentrioComponent::~SeptentrioComponent()
{
    delete socketIf_;

    // Delete StreamHandler to free memory
    pProtocolSBF->clear() ;
    delete pProtocolSBF ;
    delete pHandleStream ;
    delete ui_ ;
//     delete pShMem ;

}






//////////////////////////////////////////////////////////////////////////
// Called by the ComponentManager to start the component
//////////////////////////////////////////////////////////////////////////
void SeptentrioComponent::startActivity()
{
    socketIf_->connectToServer(host_, port_);
    QString tosend("sso curr pvtgeo+status");
    if ( !frame_.isEmpty() )     tosend.append("+").append(frame_) ;
    socketIf_->sendToServer( tosend );
    if (ui_)
        ui_->setSocket( socketIf_ ) ;
    THREAD_ALIVE = true;
    start();
}



//////////////////////////////////////////////////////////////////////////
// Called by the ComponentManager to stop the component
//////////////////////////////////////////////////////////////////////////
void SeptentrioComponent::stopActivity()
{
    THREAD_ALIVE = false;
    socketIf_->closeSocket();
    if (!wait(1500))
    {
        terminate();
        qDebug() << "The SeptentrioComponent thread(" << componentName << ")blocks anormally, it has been killed !!";
    }
}



//////////////////////////////////////////////////////////////////////////
// Called by the ComponentManager to pass the XML parameters to the
// component
//////////////////////////////////////////////////////////////////////////
ComponentBase::COMPONENT_CONFIGURATION SeptentrioComponent::configureComponent(XmlComponentConfig config)
{
    if (param.getProperty("recording") != QString::null)
        recording = ( param.getProperty("recording") == "true" ? true : false );
    if (param.getProperty("receiverIP") != QString::null)
        host_ = param.getProperty("receiverIP");
    if (param.getProperty("receiverPort") != QString::null)
        port_ = param.getProperty("receiverPort").toInt();

    if ( param.getProperty("sbfFrame") != QString::null )
        frame_ = param.getProperty("sbfFrame") ;
    if (param.getProperty("computeGPSTk") != QString::null)
       computeGPSTk = ( param.getProperty("computeGPSTk") == "true" ? true : false );
    if (param.getProperty("showUI") == "true")
       ui_ = new PolarxMainWindow();


    return ComponentBase::CONFIGURED_OK;
}




//////////////////////////////////////////////////////////////////////////
// Main loop of the thread
//////////////////////////////////////////////////////////////////////////
void SeptentrioComponent::run()
{

    hdfile_t * DBTHeader;
    // Create DBT File for recording SBF frame
    if ( recording )
    {
        DBTHeader = inithdFile((char *)(componentName + "_SbfFrame.dbt").toLatin1().data() , DBT_SBF_FRAMERAW , sizeof(SbfFrame) );
    }

    unsigned char *buffer = NULL;

    while (THREAD_ALIVE)
    {
        // Get the Frame
        socketIf_->sem.acquire();
        SbfFrame frame;
        frame = socketIf_->sbfFrames.front();
        socketIf_->sbfFrames.pop();

        if (ui_)
            ui_->setData( frame.data , frame.size );

        currentRoadtime = frame.time ;
        currentTimerange = frame.timerange ;

        // Record SbfFrame:
        if (recording)
            write_hdfile( DBTHeader , &frame , currentRoadtime , currentTimerange , sizeof(SbfFrame) );

        // Add Buffer to the Handle Stream:
        int iStartBuffer = 0; //new data, so start to read the buffer at 0
        int nbBytesRead = 0;
        int bufferLength = frame.size ;
        bool flag ;
        double TOW;
        int nbMeasReceived = 0 ;
        do
        {
            if ( pHandleStream->addData( frame.data , frame.size , iStartBuffer , &nbBytesRead , (long double)currentRoadtime ) )
            {

                flag = false ;
                SerialCOM_Msg *pCurMsg = pHandleStream->getCurMsgPointer();
                if ( pCurMsg != NULL )
                {


                    if ( pCurMsg->getID() == 5913 )
                    {
                        if (ui_)
                            ui_->setReceiverStatus( static_cast<Plrx::SerialCOM_Msg_ReceiverStatus *>(pCurMsg) );
                    }
                    if ( pCurMsg->getID() == 5912 )
                    {
                        if (ui_)
                            ui_->setTrackingStatus( static_cast<Plrx::SerialCOM_Msg_TrackingStatus *>(pCurMsg) );
                    }

                    // GPS ECEF Pos:
                    /*
                    if ( pCurMsg->getID() == 5904 )
                    {
                        const double pi = 3.1415926535898;
                        const double r2d = 180. / pi; // M_PI 
                        Plrx::SerialCOM_Msg_PVTGeodetic * RecPosData = static_cast<Plrx::SerialCOM_Msg_PVTGeodetic *>(pCurMsg) ;
                        if (ui_)
                            ui_->setPVTGeodetic( RecPosData );
                        try {
                            recPos.setGeodetic( r2d * RecPosData->lat , r2d * RecPosData->lon , RecPosData->h );
                        }
                        catch (gpstk::GeometryException &ge)
                        {
                            // We don't have a valid receiver position
                            recPos.setECEF(-1.e-99, -1.e-99, -1.e-99);
                        }
                    }
                     */

                    // GPS Nav frame:
                    if ( pCurMsg->getID() == 5895 )
                    {

                        Plrx::SerialCOM_Msg_GPSRaw * NavData = static_cast<Plrx::SerialCOM_Msg_GPSRaw *>(pCurMsg) ;
                        //if ( computeGPSTk)      pSolver->addNavigationData( (SbfDataGPSRaw *)NavData );
                        sendToShmem( 5895 , (SbfDataGPSRaw *)NavData, currentRoadtime, currentTimerange );
                        if ( abs(pCurMsg->getMsgTime() - prevRoadTime) < 5000 )
                        {
                            currentRoadtime = prevRoadTime + 5000 ;
                            prevRoadTime = currentRoadtime ;
                        }
                        else
                            prevRoadTime = pCurMsg->getMsgTime() ;
                    }

                    // GPS Meas frame:
                    if ( pCurMsg->getID() == 5889 )
                    {
                        Plrx::SerialCOM_Msg_MeasEpoch * MeasData = static_cast<Plrx::SerialCOM_Msg_MeasEpoch *>(pCurMsg) ;
                        sendToShmem( 5889 , (SbfDataMeasEpoch *)MeasData, currentRoadtime, currentTimerange );

                        TOW = MeasData->TOW ;
                        nbMeasReceived = MeasData->Nsb ;
                        
//                         qDebug() << "Raw C1 measurements" ;
//                         for ( int k = 0 ; k < MeasData->Nsb ; k++ )
//                         {
//                            qDebug("%i\t%7.3f" , MeasData->ChannelData[k].SVID , MeasData->ChannelData[k].CACode ) ;
//                         }
                        
                        //if ( computeGPSTk)      pSolver->addObservationData( (SbfDataMeasEpoch *)MeasData );
                        flag = true ;
                    }

                    if ( pCurMsg->getID() == 5890 )
                    {
                        Plrx::SerialCOM_Msg_ShortMeasEpoch * MeasData = static_cast<Plrx::SerialCOM_Msg_ShortMeasEpoch *>(pCurMsg) ;
                        TOW = MeasData->TOW ;
                        nbMeasReceived = MeasData->N ;
                        //if ( computeGPSTk)      pSolver->addObservationData( MeasData );
                        flag = true ;
                    }
                    
                    if ( pCurMsg->getID() == 5944 )
                    {
                       Plrx::SerialCOM_Msg_GenMeasEpoch * MeasData = static_cast<Plrx::SerialCOM_Msg_GenMeasEpoch *>(pCurMsg) ;
                       TOW = MeasData->TOW ;
                       nbMeasReceived = MeasData->N ;
                       //if ( computeGPSTk)      pSolver->addObservationData( MeasData );
                       flag = true ;
                    }

                    //Compute PolarX position using GPSTK;
                    /*if (computeGPSTk & flag)
                    {
                        if ( !pSolver->computeNavigationSolution() )
                        {
                           curPos = pSolver->getReceiverPosition() ;
                            curVel = pSolver->getReceiverVelocity() ;
                            curTime = pSolver->getReceiverClock();
                            double dx = curPos.X() - recPos.X() ;
                            double dy = curPos.Y() - recPos.Y() ;
                            double dz = curPos.Z() - recPos.Z() ;
                            double d = sqrt( pow(dx,2)+pow(dy,2)+pow(dz,2) );

                            qDebug("[ %6.2fs ] : Available meas. : %i (%i)" , TOW , pSolver->NbMeas() , nbMeasReceived  ) ;
                            qDebug("             Nb Eph stored   : %i" , pSolver->NbEph() ) ;
                            pSolver->EphDump() ;
                            qDebug("*------------------------------------------------------*");
                            qDebug("                 GPSTK SOLUTION                         ");
                            qDebug("X = %7.2f\t Y = %7.2f\t Z = %7.2f" , curPos.X() , curPos.Y() , curPos.Z() );
                            qDebug("Vx = %3.2f\t Vy = %3.2f\t Vz = %3.2f",curVel[0],curVel[1],curVel[2] );
                            qDebug("dt = %3.2f\t ddt = %3.2f\t",curTime[0],curTime[1]);
                            qDebug("dX = %7.2f\t dY = %7.2f\t dZ = %7.2f" , dx , dy , dz  );
                            qDebug("|E| = %7.2f" , d );
                            qDebug("*------------------------------------------------------*\n");
                        }
                        else
                           qDebug("[ %6.2fs ] : Unable to compute : %i %i (%i)" , TOW , pSolver->NbEph() , pSolver->NbMeas() , nbMeasReceived ) ;
                    }
                    */
                    // GEO Corrections frame
                    if ( pCurMsg->getID() == 5935 )
                    {
                       qDebug() << "GeoCorr Reiceved" ;
                    }


                    // DeDBytage des donnees
                    if ( (recording) && (pCurMsg->recording) )
                    {
                        if (!pCurMsg->DByteFileHeader)
                            pCurMsg->DByteFileHeader = inithdFile((char *)(componentName + "_" + QString(pCurMsg->getName().c_str()) +".dbt").toLatin1().data(),
                                                                  pCurMsg->getDbtType(),
                                                                  pCurMsg->getSizeOfStruct());//sizeof(*pCurMsg));
//                         if ( pCurMsg->writeDByte( (road_time_t)pCurMsg->getMsgTime() , currentTimerange ) == 0)
//                             qWarning("Failed to record this data ...\n");
                        if ( pCurMsg->writeDByte( currentRoadtime , currentTimerange ) == 0)
                            qWarning("Failed to record this data ...\n");
                    }
                }
            }

            // Increment number of bytes read
            iStartBuffer = iStartBuffer + nbBytesRead;
        }
        while ( (nbBytesRead>0) && (iStartBuffer<bufferLength)  );

    }

}

template <typename T> void SeptentrioComponent::sendToShmem( short id , T * msg, road_time_t t, road_timerange_t tr )
{
//    // Old 1-message solution
//    pShMem->lock() ;
//    memcpy( (short *)pShMem->data() , &id , sizeof(short) );
//    memcpy( (T *)pShMem->data()+sizeof(short) , msg , sizeof(T) );
//    pShMem->unlock();
//    pShmSem->release();
   
   QBuffer buffer_in;
   QDataStream in(&buffer_in);
   
   QBuffer buffer_out;
   buffer_out.open(QBuffer::ReadWrite);
   QDataStream out(&buffer_out);
   
   QQueue<QByteArray> msgQueue;

   // Lock shared memory
   pShMem->lock();
   // Retrieve the current message queue
   buffer_in.setData((char*)pShMem->constData(), pShMem->size());
   buffer_in.open(QBuffer::ReadOnly);
   in >> msgQueue;
   // Enqueue the new message
   QByteArray newmsg;
   newmsg.append( (char*) &id, sizeof(short));
   newmsg.append( (char*) msg, sizeof(T));
   newmsg.append( (char*) &t, sizeof(road_time_t));
   newmsg.append( (char*) &tr, sizeof(road_timerange_t));
   
   msgQueue.enqueue(newmsg);

   // Try to update the shared memory message queue
   out << msgQueue;
   int size = buffer_out.size();
   
   //qDebug() << "msgQueue is now " << msgQueue.size() << " elmts (" << buffer_out.size() << " bytes).";
   // New message will be lost if not enough memory
   if (size <= pShMem->size())
   {
      char *to = (char*)pShMem->data();
      const char *from = buffer_out.data().data();
      memcpy(to, from, qMin(pShMem->size(), size));
      
      // Signal the new data
      pShmSem->release();
   }
   else
   {
      qDebug("SeptentrioComponent: Message lost.");
   }
   // Unlock shared memory
   pShMem->unlock();
}