/*********************************************************************
//  created:    2007/11/13 - 16:49
//  filename:   DbtPlyCPTComponent.cpp
//
//  author:     Sergio Rodriguez
//              Copyright Heudiasyc UMR UTC/CNRS 6599
//
//  version:    $Id: DbtPlyCPTComponent.cpp 863 2011-07-06 15:10:17Z srodrigu $
//
//  purpose:
*********************************************************************/

#include "DbtPlyCPTComponent.h"

#include "kernel/DbiteFileTypes.h"
#include "kernel/Log.h"
#include "PacpusTools/ShMem.h"
#include "kernel/InputOutputInterface.h"

#include <iostream>
#include <QMetaType>

#define _USE_MATH_DEFINES
#include <math.h>

using namespace pacpus;
using namespace std;

DECLARE_STATIC_LOGGER("pacpus.base.DbtPlyCPTComponent");

static const size_t AllFramesSize = sizeof(TimestampedBestgpsposaFrame)
        + sizeof(TimestampedRawimusaFrame)
        + sizeof(TimestampedInspvaaFrame)
        + sizeof(TimestampedInscovFrame)
        ;

//////////////////////////////////////////////////////////////////////////
/// Construction de la fabrique de composant DbtPlySerialS8Logger
static ComponentFactory<DbtPlyCPTComponent> sFactory("DbtPlyCPTComponent");

//////////////////////////////////////////////////////////////////////////
/// Constructor
DbtPlyCPTComponent::DbtPlyCPTComponent(QString name)
    : DbtPlyFileManager(name)
{
    LOG_INFO("sizeof(TimestampedBestgpsposaFrame)=" << sizeof(TimestampedBestgpsposaFrame));
    LOG_INFO("sizeof(TimestampedRawimusaFrame)=" << sizeof(TimestampedRawimusaFrame));
    LOG_INFO("sizeof(TimestampedInspvaaFrame)=" << sizeof(TimestampedInspvaaFrame));
    LOG_INFO("sizeof(TimestampedInscovFrame)=" << sizeof(TimestampedInscovFrame));
    LOG_INFO("sizeof(trame_bestgpsposa)=" << sizeof(trame_bestgpsposa));
    LOG_INFO("sizeof(trame_rawimusa)=" << sizeof(trame_rawimusa));
    LOG_INFO("sizeof(trame_inspvaa)=" << sizeof(trame_inspvaa));
    LOG_INFO("sizeof(trame_inscov)=" << sizeof(trame_inscov));

    mShMem = new ShMem("SPAN_FRAMES", AllFramesSize);
    allFramesBuffer = new char[AllFramesSize];
}

//////////////////////////////////////////////////////////////////////////
/// Destructor
DbtPlyCPTComponent::~DbtPlyCPTComponent()
{
    delete[] allFramesBuffer;
    delete mShMem;
}

//////////////////////////////////////////////////////////////////////////
/// Configure the component
ComponentBase::COMPONENT_CONFIGURATION DbtPlyCPTComponent::configureComponent(XmlComponentConfig config)
{
    return DbtPlyFileManager::configureComponent(config);
}

void DbtPlyCPTComponent::addInputs()
{
    // empty: no inputs
}

void DbtPlyCPTComponent::addOutputs()
{
    addOutput<TimestampedBestgpsposaFrame, DbtPlyCPTComponent>("bestposegpsposa");
    addOutput<TimestampedRawimusaFrame, DbtPlyCPTComponent>("rawimusa");
    addOutput<TimestampedInspvaaFrame, DbtPlyCPTComponent>("inspvaaa");
    addOutput<TimestampedInscovFrame, DbtPlyCPTComponent>("inscov");
    addOutput<Pose3D, DbtPlyCPTComponent>("genericpose");
}

//////////////////////////////////////////////////////////////////////////
/// Start the component
void DbtPlyCPTComponent::startActivity()
{
    DbtPlyFileManager::startActivity();

    mDataFilename = mEngine->getDataDir() + mDataFilename;

    // user interface
    //if (mShowGui) {
        displayUI();
    //}
}

//////////////////////////////////////////////////////////////////////////
/// Stops the component
void DbtPlyCPTComponent::stopActivity()
{
    /*   if (mShowGui) {
        mPoseViewer->close();
        mPoseViewer.reset();
    }*/

    DbtPlyFileManager::stopActivity();
}

//////////////////////////////////////////////////////////////////////////
/// processData
void DbtPlyCPTComponent::processData(road_time_t t, road_timerange_t tr , void * buf)
{
    if (buf) {
        // look at the dbt index in file manager and get the identifier of dbt structure
        hdfile_header_t::DataTypeT id = dbt_[dbtIndex_].pfile->getType();
        LOG_TRACE("DBT structure id = " << id);

        switch (id) {
        case TRAME_BESTGPSPOSA:
            memcpy(&(bestgpsposaFrame_.frame), buf, sizeof(trame_bestgpsposa));
            bestgpsposaFrame_.time = t;
            bestgpsposaFrame_.timerange = tr;

            LOG_TRACE("[BESTGPSPOSE] frame replayed at " << bestgpsposaFrame_.time);
            LOG_DEBUG("[BESTGPSPOSE]" << "\t"
                      << "STATUS=" << ((bestgpsposaFrame_.frame.Status==SOL_COMPUTED) ? "SOL_COMPUTED" : "NO SOLUTION AVAILABLE"));
            LOG_DEBUG("[BESTGPSPOSE]" << "\t"
                      << "Lat=" << bestgpsposaFrame_.frame.Lat << "\t"
                      << "Lon=" << bestgpsposaFrame_.frame.Lon << "\t"
                      << "Alt=" << bestgpsposaFrame_.frame.Hgt);

            memcpy(allFramesBuffer, &bestgpsposaFrame_, sizeof(TimestampedBestgpsposaFrame));
            //ShMem refresh
            mShMem->write(allFramesBuffer, AllFramesSize);

            {
                OutputInterface<TimestampedBestgpsposaFrame, DbtPlyCPTComponent> * bestposegpsposaOutput = getTypedOutput<TimestampedBestgpsposaFrame, DbtPlyCPTComponent>("bestposegpsposa");
                if (bestposegpsposaOutput && bestposegpsposaOutput->hasConnection()) {
                    bestposegpsposaOutput->send(bestgpsposaFrame_);
                }
            }
            break;

        case TRAME_RAWIMUSA:
            memcpy(&(rawimuFrame_.frame), buf, sizeof(trame_rawimusa));
            rawimuFrame_.time = t;
            rawimuFrame_.timerange = tr;
            LOG_TRACE("[RAWIMUSA] frame replayed at " << rawimuFrame_.time);

            memcpy(allFramesBuffer
                   + sizeof(TimestampedBestgpsposaFrame),
                   &rawimuFrame_, sizeof(TimestampedRawimusaFrame));
            mShMem->write(allFramesBuffer, AllFramesSize);

            {
                OutputInterface<TimestampedRawimusaFrame, DbtPlyCPTComponent> * rawimusaOutput = getTypedOutput<TimestampedRawimusaFrame, DbtPlyCPTComponent>("rawimusa");
                if (rawimusaOutput && rawimusaOutput->hasConnection()) {
                    rawimusaOutput->send(rawimuFrame_);
                }
            }

            break;

        case TRAME_INSPVAA:
            memcpy(&(inspvaFrame_.frame), buf, sizeof(trame_inspvaa));
            inspvaFrame_.time = t;
            inspvaFrame_.timerange = tr;
            LOG_TRACE("[INSPVAA] frame replayed at " << inspvaFrame_.time);

            LOG_DEBUG("[INSPVAA]" << "\t"
                      << "STATUS=" << ((inspvaFrame_.frame.Status == INS_SOLUTION_GOOD) ? "INS_SOLUTION_GOOD" : "INS Unreliable"));
            LOG_DEBUG("[INSPVAA]" << "\t"
                      << "Lat=" << inspvaFrame_.frame.Lat << "\t"
                      << "Lon=" << inspvaFrame_.frame.Lon << "\t"
                      << "Alt=" << inspvaFrame_.frame.Hgt << "\t"
                      << "E=" << inspvaFrame_.frame.e << "\t"
                      << "N=" << inspvaFrame_.frame.n << "\t"
                      << "U=" << inspvaFrame_.frame.u << "\t"
                      << "Azimu=" << inspvaFrame_.frame.Azimuth << "\t"
                      << "Roll =" << inspvaFrame_.frame.Roll << "\t"
                      << "Pitch=" << inspvaFrame_.frame.Pitch);

            memcpy(allFramesBuffer
                   + sizeof(TimestampedBestgpsposaFrame) +sizeof(TimestampedRawimusaFrame),
                   &inspvaFrame_, sizeof(TimestampedInspvaaFrame));
            mShMem->write(allFramesBuffer, AllFramesSize);

            {
                OutputInterface<TimestampedInspvaaFrame, DbtPlyCPTComponent> * inspvaaaOutput = getTypedOutput<TimestampedInspvaaFrame, DbtPlyCPTComponent>("inspvaaa");
                if (inspvaaaOutput && inspvaaaOutput->hasConnection()) {
                    inspvaaaOutput->send(inspvaFrame_);
                }
            }

            genericEnuPose_.time = t;
            genericEnuPose_.timerange = tr;
            genericEnuPose_.position = QVector3D(inspvaFrame_.frame.e, inspvaFrame_.frame.n, inspvaFrame_.frame.u);
            // FIXME: use Geodesie: DegToRad / RadToDeg
            genericEnuPose_.angle = QVector3D((inspvaFrame_.frame.Azimuth+90.0)*(M_PI/180.0),inspvaFrame_.frame.Pitch*(M_PI/180.0),inspvaFrame_.frame.Roll*(M_PI/180.0));

            // FIXME: it was: send(genericEnuPose_, road_time(), 100000) -- was it OK?
            {
                OutputInterface<Pose3D, DbtPlyCPTComponent> * poseOutput = getTypedOutput<Pose3D, DbtPlyCPTComponent>("genericpose");
                if (poseOutput && poseOutput->hasConnection()) {
                    poseOutput->send(genericEnuPose_, genericEnuPose_.time, genericEnuPose_.timerange);
                }
            }
//         for (int i=0;i<3;i++) {
//             for(int j=0;j<3;j++) {
//                 pose.posconv[i][j] = d.frame.PosCov[i][j];
//                 pose.angcov[i][j] = d.frame.AttCov[i][j];
//             }
//         }

            break;

        case TRAME_INSCOV:
            memcpy(&(inscovFrame_.frame), buf, sizeof(trame_inscov));
            inscovFrame_.time = t;
            inscovFrame_.timerange = tr;
            LOG_TRACE("[INSCOV] frame replayed at " << inscovFrame_.time);

            LOG_DEBUG("[INSCOV]" << "\t"
                      << "Week=" << inscovFrame_.frame.Week << "\t"
                      << "Secs=" << inscovFrame_.frame.Seconds << "\t"
                      << "AttCov=" << inscovFrame_.frame.AttCov[0][0] << "\t"
                      << "PosCov=" << inscovFrame_.frame.PosCov[0][0] << "\t"
                      << "VelCov=" << inscovFrame_.frame.VelCov[0][0]);

            memcpy(allFramesBuffer
                   + sizeof(TimestampedBestgpsposaFrame) + sizeof(TimestampedRawimusaFrame) + sizeof(TimestampedInspvaaFrame),
                   &inscovFrame_, sizeof(TimestampedInscovFrame));
            mShMem->write(allFramesBuffer, AllFramesSize);

            {
                OutputInterface<TimestampedInscovFrame, DbtPlyCPTComponent> * inscovOutput = getTypedOutput<TimestampedInscovFrame, DbtPlyCPTComponent>("inscov");
                if (inscovOutput && inscovOutput->hasConnection()) {
                    inscovOutput->send(inscovFrame_);
                }
            }
            break;

        default:
            LOG_WARN("unknown frame type: " << id);
    }

}
}

//////////////////////////////////////////////////////////////////////////
/// Display GUI
void DbtPlyCPTComponent::displayUI()
{

}