// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2013/08/23
//  filename:   Gps.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    objet integrant le recepteur gps mb800
//
//
/*********************************************************************/

#include "Gps.h"
#include "geodesie.h"
#include <Euler.h>
#include <cvmatrix.h>
#include <DataPlot1D.h>
#include <Tab.h>
#include <TabWidget.h>
#include <GridLayout.h>
#include <GroupBox.h>
#include <PushButton.h>
#include <FrameworkManager.h>
#include <Map.h>
#include <GeoCoordinate.h>
#include <Vector3D.h>
#include <Label.h>
#include <string.h>

using std::string;
using namespace Geodesie;
using namespace flair::core;
using namespace flair::gui;

namespace flair
{
namespace sensor
{

Gps::Gps(const FrameworkManager* parent,string name,NMEAFlags_t NMEAFlags) : IODevice(parent,name)
{
    this->NMEAFlags=NMEAFlags;

    nmea_zero_INFO(&info);
    nmea_parser_init(&parser);
    alt_ref=0;


    if((NMEAFlags&GGA)==0)
    {
        Err("Enable at least GGA sentence\n");
    }

    int index=0;
    if((NMEAFlags&GGA)!=0)
    {
        index+=3;
    }
    if((NMEAFlags&VTG)!=0)
    {
        index+=2;
    }
    if((NMEAFlags&GST)!=0)
    {
        index+=3;
    }

    cvmatrix_descriptor* desc=new cvmatrix_descriptor(index,1);
    index=0;
    if((NMEAFlags&GGA)!=0)
    {
        desc->SetElementName(0,0,"e");
        desc->SetElementName(1,0,"n");
        desc->SetElementName(2,0,"u");
        index+=3;
    }
    if((NMEAFlags&VTG)!=0)
    {
        desc->SetElementName(index,0,"ve");
        desc->SetElementName(index+1,0,"vn");
        index+=2;
    }
    if((NMEAFlags&GST)!=0)
    {
        desc->SetElementName(index,0,"dev_lat");
        desc->SetElementName(index+1,0,"dev_lon");
        desc->SetElementName(index+2,0,"dev_elv");
        index+=3;
    }
    output=new cvmatrix((IODevice*)this,desc,floatType);
    AddDataToLog(output);

    //station sol
    main_tab=new Tab(parent->GetTabWidget(),name);
    tab=new TabWidget(main_tab->NewRow(),name);
	sensor_tab=new Tab(tab,"Reglages");
	GroupBox* reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
        button_ref=new PushButton(reglages_groupbox->NewRow(),"set ref");
        nb_sat_label=new Label(reglages_groupbox->NewRow(),"nb_sat");
        fix_label=new Label(reglages_groupbox->LastRowLastCol(),"fix");

    position=new GeoCoordinate((IODevice*)this,"position",0,0,0);

    fix=FixQuality_t::Invalid;
    nb_sat=0;
    take_ref=false;
    nb_sat_label->SetText("nb_sat: %i",nb_sat);
    fix_label->SetText("fix: %i",fix);
}

Gps::~Gps()
{
    nmea_parser_destroy(&parser);
    delete main_tab;
}

void Gps::UseDefaultPlot(void)
{
    int index=0;
    plot_tab=new Tab(tab,"Mesures");

    if((NMEAFlags&GGA)!=0)
    {
        e_plot=new DataPlot1D(plot_tab->NewRow(),"e",-10,10);
            e_plot->AddCurve(output->Element(index));
        n_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"n",-10,10);
            n_plot->AddCurve(output->Element(index+1));
        u_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"u",-10,10);
            u_plot->AddCurve(output->Element(index+2));
        index+=3;
    }
    if((NMEAFlags&VTG)!=0)
    {
        ve_plot=new DataPlot1D(plot_tab->NewRow(),"ve",-10,10);
            ve_plot->AddCurve(output->Element(index));
        vn_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"vn",-10,10);
            vn_plot->AddCurve(output->Element(index+1));
        index+=2;
    }

    Tab* map_tab=new Tab(tab,"carte");
        map=new Map(map_tab->NewRow(),"map");
            map->AddPoint(position,"drone");
}

DataPlot1D* Gps::EPlot(void) const
{
    if((NMEAFlags&GGA)!=0)
    {
        return e_plot;
    }
    else
    {
        Err("GGA sentence not requested\n");
        return NULL;
    }
}

DataPlot1D* Gps::NPlot(void) const
{
    if((NMEAFlags&GGA)!=0)
    {
        return n_plot;
    }
    else
    {
        Err("GGA sentence not requested\n");
        return NULL;
    }
}

DataPlot1D* Gps::UPlot(void) const
{
    if((NMEAFlags&GGA)!=0)
    {
        return u_plot;
    }
    else
    {
        Err("GGA sentence not requested\n");
        return NULL;
    }
}

DataPlot1D* Gps::VEPlot(void) const
{
    if((NMEAFlags&VTG)!=0)
    {
        return ve_plot;
    }
    else
    {
        Err("GGA sentence not requested\n");
        return NULL;
    }
}

DataPlot1D* Gps::VNPlot(void) const
{
    if((NMEAFlags&VTG)!=0)
    {
        return vn_plot;
    }
    else
    {
        Err("GGA sentence not requested\n");
        return NULL;
    }
}

Layout* Gps::GetLayout(void) const
{
    return sensor_tab;
}

Tab* Gps::GetPlotTab(void) const
{
    return plot_tab;
}

TabWidget* Gps::GetTab(void) const
{
    return tab;
}

uint16_t Gps::NbSat(void) const
{
    output->GetMutex();
    uint16_t result=nb_sat;
    output->ReleaseMutex();
    return result;
}

Gps::FixQuality_t Gps::FixQuality(void) const
{
    output->GetMutex();
    FixQuality_t result=fix;
    output->ReleaseMutex();
    return result;
}

void Gps::SetRef(void)
{
    take_ref=true;
}

void Gps::GetENUPosition(Vector3D *point)
{
    output->GetMutex();
    point->x=output->ValueNoMutex(0,0);
    point->y=output->ValueNoMutex(1,0);
    point->z=output->ValueNoMutex(2,0);
    output->ReleaseMutex();
}

void Gps::parseFrame(const char *frame, int frame_size){

    int result;

    result=nmea_parse(&parser, frame, frame_size, &info);
    if(result!=1)
    {
        Warn("unrecognized nmea sentence\n");
        Warn("%s\n",frame);
    }

    result=nmea_parse_GPGGA(frame, frame_size, &pack);

    if(result==1)
    {
        //Printf("%s\n",frame);
        //Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);
        //Printf("lat:%f long:%f\n",pos.lat,pos.lon);
        output->GetMutex();//on utilise le mutex de l'output pour fix et nb_sat
        if((int)fix!=pack.sig)
        {
            fix=(FixQuality_t)pack.sig;
            fix_label->SetText("fix: %i",fix);
        }
        if(nb_sat!=pack.satinuse)
        {
            nb_sat=pack.satinuse;
            nb_sat_label->SetText("nb_sat: %i",nb_sat);
        }
        output->ReleaseMutex();


        nmea_info2pos(&info,&pos);
        position->SetCoordinates(Euler::ToDegree(pos.lat),Euler::ToDegree(pos.lon),info.elv);

        if((info.sig==2 && alt_ref==0) || button_ref->Clicked()==true || take_ref==true)
        {
            Printf("prise pos ref\n");
            lat_ref=pos.lat;
            long_ref=pos.lon;
            alt_ref=info.elv;
            take_ref=false;
        }
        //if(alt_ref!=0)
        {
            double x,y,z;
            double e,n,u;
            Geographique_2_ECEF(pos.lon,pos.lat,info.elv,x,y,z);
            ECEF_2_ENU(x,y,z,e, n, u,long_ref,lat_ref,alt_ref);
            //Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);

            //on prend une fois pour toute le mutex et on fait des accès directs
            output->GetMutex();
            output->SetValueNoMutex( 0, 0,e);
            output->SetValueNoMutex( 1, 0,n);
            output->SetValueNoMutex( 2, 0,u);

            int index=3;
            if((NMEAFlags&VTG)!=0)
            {
                output->SetValueNoMutex( index, 0,info.speed*1000./3600.*sin(Euler::ToRadian(info.direction)));
                output->SetValueNoMutex( index+1, 0,info.speed*1000./3600.*cos(Euler::ToRadian(info.direction)));
                index+=2;
            }
            if((NMEAFlags&GST)!=0)
            {
                //Thread::Printf("dev_lon:%f dev_lat:%f dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
                output->SetValueNoMutex( index, 0,info.dev_lat);
                output->SetValueNoMutex( index+1, 0,info.dev_lon);
                output->SetValueNoMutex( index+2, 0,info.dev_elv);
                index+=3;
            }
            output->ReleaseMutex();

            output->SetDataTime(GetTime());
            ProcessUpdate(output);
        }
    }
}

} // end namespace sensor
} // end namespace framewor