// %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:   IpcUs.cpp
//
//  author:     Sébastien Ambroziak
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Class for an ipc us
//
//
/*********************************************************************/

#include "IpcUs.h"
#include "ipc_receive.h"
#include "shm_receive.h"
#include <FrameworkManager.h>
#include <SpinBox.h>
#include <GroupBox.h>
#include <Matrix.h>

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

namespace flair {
namespace sensor {

IpcUs::IpcUs(string name, uint8_t priority, const char* ipc_name, int ipc_channel)
    : Thread(getFrameworkManager(), name, priority), UsRangeFinder( name) {
  data_rate =
      new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 50);

  //init non blocking ipc receiver
  receiver = new ShmReceiver<float>(ipc_name,ipc_channel,true);

  SetIsReady(true);
}

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


void IpcUs::Run(void) {
  
  SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
  Time stamp; 
  Time lastT = GetTime();
  while (!ToBeStopped()) {
    WaitPeriod();

    receiver->receive();

    stamp = GetTime();

    //SetPeriodUS((receiver->getTimestamp().toNanosec() - Last)/1000);
    
    
    
    if (data_rate->ValueChanged() == true) {
      SetPeriodUS((uint32_t)(1000000. / data_rate->Value()));
    }

    //output->SetDataTime((Time)receiver->getTimestamp().toNanosec());
    /*if (lastT < receiver->getTimestamp().toNanosec()){
      output->SetValue(0, 0, receiver->getValue());
      output->SetDataTime(receiver->getTimestamp().toNanosec());
      //cout << "lastT: " << receiver->getTimestamp().toNanosec()  << " ecart: " << receiver->getTimestamp().toNanosec() - lastT << endl;
      lastT = receiver->getTimestamp().toNanosec();
    }

    else{
      output->SetValue(0, 0, receiver->getValue());
      output->SetDataTime(lastT + GetPeriodUS()*1000);
      //cout << "stamp: " << lastT + GetPeriodUS() << ' ' << " \tecart: " << stamp - lastT << endl;
      lastT = lastT + GetPeriodUS()*1000;
    }*/

    /*if (receiver->getValue() >= 0){
      output->SetValue(0, 0, (receiver->getValue())/100.0);
    }
    else
    {
      output->SetValue(0,0,0);
    }*/
    
    output->SetValue(0, 0, (receiver->getValue())/100.0);
    output->SetDataTime(stamp);
    //output->SetDataTime(receiver->getTimestamp().toNanosec());

    //cout << receiver->getTimestamp().toNanosec() << ' ' << receiver->getValue() << endl;
    ProcessUpdate(output);
    //cout << "GetTime: " <<  stamp << " ipc: " << receiver->getTimestamp().toNanosec() << " écart: " << stamp - receiver->getTimestamp().toNanosec() << endl;
  }
}

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