// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2021/03/03
//  filename:   IpcImu.cpp
//
//  author:     Sébastien Ambroziak
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class for an ipc imu
//
//
/*********************************************************************/

#include "IpcImu.h"
#include "ipc_receive.h"
#include <FrameworkManager.h>
#include <ImuData.h>
#include <SpinBox.h>
#include <GroupBox.h>
#include <Matrix.h>
#include <AhrsData.h>

using namespace flair::core;
using namespace flair::gui;

namespace flair {
namespace sensor {

IpcImu::IpcImu(std::string name, uint8_t priority, const char* ipc_name, int ipc_channel, bool AhrsOnly)
    : Imu(name,false), Thread(getFrameworkManager(), name, priority) {
  dataRate = new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 200);
  ahrsData = new AhrsData((Imu *)this);
  _AhrsOnly = AhrsOnly;
  if (AhrsOnly){
    //AhrsReceiver = new IpcReceiver<ipc::type::ahrs>(ipc_name,ipc_channel);
    orientation_quat = new IpcReceiver<ipc::type::vector4D_32>("ipc_quat",65);
    angular_vel = new IpcReceiver<ipc::type::vector3D_32>("ipc_angl",65);
  }
  else{
    ImuReceiver = new IpcReceiver<ipc::type::imu>(ipc_name,ipc_channel);
  }
  
  SetIsReady(true);
}

IpcImu::~IpcImu() {
  SafeStop();
  Join();
}


void IpcImu::Run(void) {
  imu_states_t state;
  ImuData *imuData;
  ipc::type::imu imu_msg;
  ipc::type::ahrs ahrs_msg;
  ipc::type::vector4D_32 quat_msg;
  ipc::type::vector3D_32 angl_msg;
  GetDatas(&imuData);

  Quaternion quaternion;
  Vector3Df angRate;
  Vector3Df rawAcc;
  Vector3Df rawMag;
  Vector3Df rawGyr;
  SetPeriodUS((uint32_t)(1000000. / dataRate->Value()));

  while (!ToBeStopped()) {

    angRate.x = 0;
    angRate.y = 0;
    angRate.z = 0;

    rawAcc.x = 0;
    rawAcc.y = 0;
    rawAcc.z = 0;

    rawMag.x = 0;
    rawMag.y = 0;
    rawMag.z = 0;

    rawGyr.x = 0;
    rawGyr.y = 0;
    rawGyr.z = 0;

    if (_AhrsOnly){
      //AhrsReceiver->receive();
      //ahrs_msg = AhrsReceiver->getValue();
      
      orientation_quat->receive();
      angular_vel->receive();
      quat_msg = orientation_quat->getValue();
      angl_msg = angular_vel->getValue();

      quaternion.q0 = quat_msg.x;
      quaternion.q1 = quat_msg.y;
      quaternion.q2 = quat_msg.z;
      quaternion.q3 = quat_msg.w;

      angRate.x = angl_msg.x;
      angRate.y = angl_msg.y;
      angRate.z = angl_msg.z;

      imuData->SetDataTime(AhrsReceiver->getTimestamp().toNanosec());
      ahrsData->SetDataTime(ImuReceiver->getTimestamp().toNanosec());
    }
    else {
      ImuReceiver->receive();

      imu_msg = ImuReceiver->getValue();

      quaternion.q0 = imu_msg.orientation.w;
      quaternion.q1 = imu_msg.orientation.x;
      quaternion.q2 = imu_msg.orientation.y;
      quaternion.q3 = imu_msg.orientation.z;

      rawAcc.x = imu_msg.linear_acceleration.x;
      rawAcc.y = imu_msg.linear_acceleration.y;
      rawAcc.z = imu_msg.linear_acceleration.z;

      /*rawGyr.x = imu_msg.angular_velocity.x;
      rawGyr.y = imu_msg.angular_velocity.y;
      rawGyr.z = imu_msg.angular_velocity.z;*/

      angRate.x = imu_msg.angular_velocity.x;
      angRate.y = imu_msg.angular_velocity.y;
      angRate.z = imu_msg.angular_velocity.z;

      imuData->SetDataTime(ImuReceiver->getTimestamp().toNanosec());
      ahrsData->SetDataTime(ImuReceiver->getTimestamp().toNanosec());
    }

    
    
    //we do not need rotation in simulation
    /*
    ApplyRotation(angRate);
    ApplyRotation(quaternion);
    ApplyRotation(rawAcc);
    ApplyRotation(rawMag);
    ApplyRotation(rawGyr);*/
    ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
    imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
    
    //ahrsData->SetDataTime(GetTime());
    ProcessUpdate(ahrsData);
  }
}

} // end namespace sensor
} // end namespace flair