/*****************************************************************************
 * Copyright (c) 2002-2020 MBARI
 * Monterey Bay Aquarium Research Institute, all rights reserved.
 *****************************************************************************
 * @file    LcmDeltaT.cc
 * @authors r. henthorn
 * @date    11/23/2020
 * @brief   LcmDeltaT client interface module
 *
 * Project: LRAUV Obstacle Avoidance (oahu)
 * Summary: Derived class of DeltaT to publish 83P data using lrauv-lcmtypes.
 *****************************************************************************/
/*****************************************************************************
  * Copyright Information:
  * Copyright 2002-2020 MBARI
  * Monterey Bay Aquarium Research Institute, all rights reserved.
  *
  * Terms of Use:
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License as published by the Free
  * Software Foundation; either version 3 of the License, or (at your option)
  * any later version. You can access the GPLv3 license at
  * http://www.gnu.org/licenses/gpl-3.0.html
  *
  * This program is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
  * more details (http://www.gnu.org/licenses/gpl-3.0.html).
  *
  * MBARI provides the documentation and software code "as is", with no
  * warranty, express or implied, as to the software, title, non-infringement
  * of third party rights, merchantability, or fitness for any particular
  * purpose, the accuracy of the code, or the performance or results which you
  * may obtain from its use. You assume the entire risk associated with use of
  * the code, and you agree to be responsible for the entire cost of repair or
  * servicing of the program with which you are using the code.
  *
  * In no event shall MBARI be liable for any damages,whether general, special,
  * incidental or consequential damages, arising out of your use of the
  * software, including, but not limited to,the loss or corruption of your data
  * or damages of any kind resulting from use of the software, any prohibited
  * use, or your inability to use the software. You agree to defend, indemnify
  * and hold harmless MBARI and its officers, directors, and employees against
  * any claim,loss,liability or expense,including attorneys' fees,resulting from
  * loss of or damage to property or the injury to or death of any person
  * arising out of the use of the software.
  *
  * The MBARI software is provided without obligation on the part of the
  * Monterey Bay Aquarium Research Institute to assist in its use, correction,
  * modification, or enhancement.
  *
  * MBARI assumes no responsibility or liability for any third party and/or
  * commercial software required for the database or applications. Licensee
  * agrees to obtain and maintain valid licenses for any additional third party
  * software required.
  *****************************************************************************/

/******************************
 * Headers
 ******************************/

#include <sys/time.h>
#include <unistd.h>
#include <lcm/lcm-cpp.hpp>
#include "LcmDeltaT.h"
#include "macrologger.h"

/******************************
 * Macros
 ******************************/


/******************************
 * Code
 ******************************/
using namespace lrauv_lcm_tools;

LcmDeltaT::LcmDeltaT(const DeltaTConfig &config)
   : DeltaT(config), _msgWriter(NULL)
{
   // Setup LCM channel and message
   _lcm = new lcm::LCM();
   if(!_lcm->good())
   {
      LOG_ERROR("LcmDeltaT: LCM init failed");
   }

   _msgWriter = new LcmMessageWriter<std::string>();
   init83pState();
}

LcmDeltaT::~LcmDeltaT()
{
   if (_msgWriter) delete _msgWriter;
   if (_lcm) delete _lcm;
}

// attempt to read a packet from the DeltaT source.
// if successful log data and return the populated 83P structure.
// returns NULL if unsuccessful.
// return parsed info in the Idt83pData parameter object.
// otherwise, returns pointer to the parsed data in the
// Idt83pData parameter object passed in.
Idt83pData* LcmDeltaT::getData(Idt83pData *data)
{
   // Use base class to retrieve Delta T data
   // If successful, publish the data on the LCM channel
   // and return the data structure
   LOG_DEBUG("Publishing on LCM");
   if (DeltaT::getData(data))
   {
      if (_lcm->good() && _msgWriter)
      {
         _msgWriter->set("valid",      data->valid);
         _msgWriter->set("timestamp",  data->timestamp);
         _msgWriter->set("pingtime",   data->pingtime);
         _msgWriter->set("pingnumber", (int)data->pingnumber);
         _msgWriter->set("altitude",   data->altitude);
         _msgWriter->set("interval",   data->interval);
         _msgWriter->set("nbeams",     data->nbeams);
         for (int i = 0; i < IDT_MAX_BEAMS; i++)
         {
            _msgWriter->set("range", data->range[i], i);
            _msgWriter->set("intense", data->intense[i], i);
         }
         struct timeval spec;
         gettimeofday(&spec, NULL);
         double ms = (spec.tv_sec * 1000.) + (spec.tv_usec / 1000.);
         _msgWriter->publish(*_lcm, "DeltaT", (int64_t)ms);
      }
      return data;
   }
   else
   {
      return NULL;
   }
}

// Initialize a DataVectors object representing the 83P state.
//
void LcmDeltaT::init83pState()
{
   Dim sdim(0,0);
   // scalar int value valid
   if (!_msgWriter->addArray(Int, "valid", "valid", "", sdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add valid");
   // epoch seconds float value timestamp
   if (!_msgWriter->addArray(Double, "timestamp", "timestamp", "epoch", sdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add timestamp");
   // epoch seconds float value pingtime
   if (!_msgWriter->addArray(Double, "pingtime", "pingtime", "epoch", sdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add pingtime");
   // scalar int value pingnumber
   if (!_msgWriter->addArray(Int, "pingnumber", "pingnumber", "", sdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add pingnumber");
   // epoch seconds float value altitude
   if (!_msgWriter->addArray(Float, "altitude", "altitude", "", sdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add altitude");
   // int value interval
   if (!_msgWriter->addArray(Int, "interval", "interval", "", sdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add interval");
   // scalar int value nbeams
   if (!_msgWriter->addArray(Int, "nbeams", "nbeams", "", sdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add nbeams");
   //  beam ranges
   Dim cdim(1,IDT_MAX_BEAMS);    // 1-dimension
   if (!_msgWriter->addArray(Float, "range", "range", "meter", cdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add range");
   //  beam intensities
   if (!_msgWriter->addArray(Int, "intense", "intense", "percentage", cdim))
      LOG_ERROR("LcmDeltaT::init83pState() - failed to add intense");
}