// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2014/01/14
//  filename:   Ahrs_impl.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Virtual class for Ahrs_impl
//
//
/*********************************************************************/

#include "Ahrs_impl.h"
#include "Ahrs.h"
#include <Imu.h>
#include <Tab.h>
#include <TabWidget.h>
#include <DataPlot1D.h>
#include <AhrsData.h>

using std::string;
using namespace flair::core;
using namespace flair::gui;
using namespace flair::filter;
using namespace flair::sensor;

Ahrs_impl::Ahrs_impl(Ahrs *inSelf) : self(inSelf) {
  rollPlot = NULL;
  pitchPlot = NULL;
  yawPlot = NULL;
  wXPlot = NULL;
  wYPlot = NULL;
  wZPlot = NULL;
  q0Plot = NULL;
  q1Plot = NULL;
  q2Plot = NULL;
  q3Plot = NULL;

  eulerTab = NULL;
  quaternionTab = NULL;

  ahrsData = new AhrsData(self);
}

Ahrs_impl::~Ahrs_impl() {}

void Ahrs_impl::UseDefaultPlot(void) {

  eulerTab = new Tab(((Imu *)(self->Parent()))->tab, "AHRS");
  rollPlot = new DataPlot1D(eulerTab->NewRow(), "roll", -30, 30);
  rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg));
  pitchPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "pitch", -30, 30);
  pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg));
  yawPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "yaw", -180, 180);
  yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg));

  wXPlot = new DataPlot1D(eulerTab->NewRow(), "w_x", -200, 200);
  wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg));
  wYPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "w_y", -200, 200);
  wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg));
  wZPlot = new DataPlot1D(eulerTab->LastRowLastCol(), "w_z", -200, 200);
  wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg));

  quaternionTab = new Tab(((Imu *)(self->Parent()))->tab, "Quaternion");
  q0Plot = new DataPlot1D(quaternionTab->NewRow(), "q0", -1, 1);
  q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0));
  q1Plot = new DataPlot1D(quaternionTab->NewRow(), "q1", -1, 1);
  q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1));
  q2Plot = new DataPlot1D(quaternionTab->LastRowLastCol(), "q2", -1, 1);
  q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2));
  q3Plot = new DataPlot1D(quaternionTab->LastRowLastCol(), "q3", -1, 1);
  q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3));
}

void Ahrs_impl::AddPlot(const AhrsData *ahrsData, DataPlot::Color_t color) {
  if (rollPlot != NULL) {
    rollPlot->AddCurve(ahrsData->Element(AhrsData::RollDeg), color);
    pitchPlot->AddCurve(ahrsData->Element(AhrsData::PitchDeg), color);
    yawPlot->AddCurve(ahrsData->Element(AhrsData::YawDeg), color);
  }
  if (wXPlot != NULL) {
    wXPlot->AddCurve(ahrsData->Element(AhrsData::WxDeg), color);
    wYPlot->AddCurve(ahrsData->Element(AhrsData::WyDeg), color);
    wZPlot->AddCurve(ahrsData->Element(AhrsData::WzDeg), color);
  }
  if (quaternionTab != NULL) {
    q0Plot->AddCurve(ahrsData->Element(AhrsData::Q0), color);
    q1Plot->AddCurve(ahrsData->Element(AhrsData::Q1), color);
    q2Plot->AddCurve(ahrsData->Element(AhrsData::Q2), color);
    q3Plot->AddCurve(ahrsData->Element(AhrsData::Q3), color);
  }
}