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

#include "AhrsData.h"
#include "Euler.h"
#include <math.h>
#include <string.h>
#include <stdlib.h>

using std::string;

namespace flair {
namespace core {

/*! \class AhrsDataElement
    */
class AhrsDataElement : public IODataElement {
public:
  AhrsDataElement(const AhrsData *inAhrsData, string name,
                  AhrsData::PlotableData_t inPlotableData)
      : IODataElement(inAhrsData, name), ahrsData(inAhrsData),
        plotableData(inPlotableData) {

    size = sizeof(float);
  }

  ~AhrsDataElement() {}

  void CopyData(char *destination) const {
    float data;
    Vector3Df angularRates;
    Euler eulerAngles;
    Quaternion quaternion;
    ahrsData->GetQuaternionAndAngularRates(quaternion, angularRates);
    quaternion.ToEuler(eulerAngles);
    switch (plotableData) {
    case AhrsData::Roll:
      data = eulerAngles.roll;
      break;
    case AhrsData::Pitch:
      data = eulerAngles.pitch;
      break;
    case AhrsData::Yaw:
      data = eulerAngles.yaw;
      break;
    case AhrsData::RollDeg:
      data = Euler::ToDegree(eulerAngles.roll);
      break;
    case AhrsData::PitchDeg:
      data = Euler::ToDegree(eulerAngles.pitch);
      break;
    case AhrsData::YawDeg:
      data = Euler::ToDegree(eulerAngles.yaw);
      break;
    case AhrsData::Q0:
      data = quaternion.q0;
      break;
    case AhrsData::Q1:
      data = quaternion.q1;
      break;
    case AhrsData::Q2:
      data = quaternion.q2;
      break;
    case AhrsData::Q3:
      data = quaternion.q3;
      break;
    case AhrsData::Wx:
      data = angularRates.x;
      break;
    case AhrsData::Wy:
      data = angularRates.y;
      break;
    case AhrsData::Wz:
      data = angularRates.z;
      break;
    case AhrsData::WxDeg:
      data = Euler::ToDegree(angularRates.x);
      break;
    case AhrsData::WyDeg:
      data = Euler::ToDegree(angularRates.y);
      break;
    case AhrsData::WzDeg:
      data = Euler::ToDegree(angularRates.z);
      break;
    default:
      ahrsData->Err("data type unavailable.\n");
      data = 0;
      break;
    }
    memcpy(destination, &data, sizeof(float));
  }

  const FloatType &GetDataType(void) const { return dataType; }

private:
  const AhrsData *ahrsData;
  AhrsData::PlotableData_t plotableData;
  FloatType dataType;
};

AhrsData::AhrsData(const Object *parent, std::string name, int n)
    : io_data(parent, name, n), dataType(floatType) {
  if (n > 1)
    Warn("n>1 not supported\n");

  AppendLogDescription("q0", floatType);
  AppendLogDescription("q1", floatType);
  AppendLogDescription("q2", floatType);
  AppendLogDescription("q3", floatType);

  AppendLogDescription("wx", floatType);
  AppendLogDescription("wy", floatType);
  AppendLogDescription("wz", floatType);
}

AhrsData::~AhrsData() {}

Quaternion AhrsData::GetQuaternion(void) const {
  Quaternion out;
  GetMutex();
  out = quaternion;
  ReleaseMutex();
  return out;
}

Vector3Df AhrsData::GetAngularRates(void) const {
  Vector3Df out;
  GetMutex();
  out = angularRates;
  ReleaseMutex();
  return out;
}

void AhrsData::GetQuaternionAndAngularRates(Quaternion &outQuaternion,
                                            Vector3Df &outAngularRates) const {
  GetMutex();
  outQuaternion = quaternion;
  outAngularRates = angularRates;
  ReleaseMutex();
}

void AhrsData::SetQuaternionAndAngularRates(const Quaternion &inQuaternion,
                                            const Vector3Df &inAngularRates) {
  GetMutex();
  quaternion = inQuaternion;
  angularRates = inAngularRates;
  ReleaseMutex();
}

void AhrsData::SetQuaternion(const Quaternion &inQuaternion) {
  GetMutex();
  quaternion = inQuaternion;
  ReleaseMutex();
}

void AhrsData::SetAngularRates(const Vector3Df &inAngularRates) {
  GetMutex();
  angularRates = inAngularRates;
  ReleaseMutex();
}

IODataElement *AhrsData::Element(PlotableData_t plotableData) const {
  string name;
  switch (plotableData) {
  case AhrsData::Roll:
    name = "Roll";
    break;
  case AhrsData::Pitch:
    name = "Pitch";
    break;
  case AhrsData::Yaw:
    name = "Yaw";
    break;
  case AhrsData::RollDeg:
    name = "Roll degree";
    break;
  case AhrsData::PitchDeg:
    name = "Pitch degree";
    break;
  case AhrsData::YawDeg:
    name = "Yaw degree";
    break;
  case AhrsData::Q0:
    name = "Q0";
    break;
  case AhrsData::Q1:
    name = "Q1";
    break;
  case AhrsData::Q2:
    name = "Q2";
    break;
  case AhrsData::Q3:
    name = "Q3";
    break;
  case AhrsData::Wx:
    name = "Wx";
    break;
  case AhrsData::Wy:
    name = "Wy";
    break;
  case AhrsData::Wz:
    name = "Wz";
    break;
  case AhrsData::WxDeg:
    name = "Wx degree";
    break;
  case AhrsData::WyDeg:
    name = "Wy degree";
    break;
  case AhrsData::WzDeg:
    name = "Wz degree";
    break;
  default:
    Err("data type unavailable.\n");
  }

  return new AhrsDataElement(this, name, plotableData);
}

void AhrsData::RawRead(char *dst) const {
  GetMutex();
  Queue(&dst, &quaternion.q0, sizeof(quaternion.q0));
  Queue(&dst, &quaternion.q1, sizeof(quaternion.q1));
  Queue(&dst, &quaternion.q2, sizeof(quaternion.q2));
  Queue(&dst, &quaternion.q3, sizeof(quaternion.q3));

  Queue(&dst, &angularRates.x, sizeof(angularRates.x));
  Queue(&dst, &angularRates.y, sizeof(angularRates.y));
  Queue(&dst, &angularRates.z, sizeof(angularRates.z));
  ReleaseMutex();
}

void AhrsData::Queue(char **dst, const void *src, size_t size) const {
  memcpy(*dst, src, size);
  *dst += size;
}

} // end namespace core
} // end namespace flair