/*****************************************************************************
 * Copyright (c) 2002-2020 MBARI
 * Monterey Bay Aquarium Research Institute, all rights reserved.
 *****************************************************************************
 * @file    testIdt.cc
 * @authors r. henthorn
 * @date    07/21/2020
 * @brief   Test program for Idt83pParser utility
 *
 * Project: LRAUV Obstacle avoidance
 * Summary: See @brief
 *****************************************************************************/
/*****************************************************************************
 * 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 <stdio.h>
#include <string.h>

#include "Idt83pParser.h"
#include "macrologger_main.h"

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

int main(int argc, char const *argv[])
{
   /* code */
   openMLOutstream("testlog");
   setMLLevel(DEBUG_LEVEL);

   // fail due to pointers
   int8_t status = 0;
   if ( (status = parseIdt83p(NULL, NULL, 0)) <= 0)
   {
      LOG_ERROR("Failed to parse packet: %d", status);
   }

   Idt83pData data;
   if ( (status = parseIdt83p(&data, NULL, 0)) <= 0)
   {
      LOG_ERROR("Failed to parse packet: %d", status);
   }

   // fail due to bad packet size
   char packet[256+4*IDT_MAX_BEAMS];
   memset(packet, 0, sizeof(packet));
   if ( (status = parseIdt83p(&data, packet, 0)) <= 0)
   {
      LOG_ERROR("Failed to parse packet: %d", status);
   }

   // fail due to bad packet type
   if ( (status = parseIdt83p(&data, packet, IDT_HDR_SIZE)) <= 0)
   {
      LOG_ERROR("Failed to parse packet: %d", status);
   }

   // Bad packet type
   sprintf(packet, "83Z");
   if ( (status = parseIdt83p(&data, packet, IDT_HDR_SIZE)) <= 0)
   {
      LOG_ERROR("Failed to parse packet: %d", status);
   }

   // Good packet type
   sprintf(packet+IDT_LOC_TYPE, "83P");
   sprintf(packet+IDT_LOC_DATE, "22-JUL-2020");
   sprintf(packet+IDT_LOC_TIME, "01:00:01");
   sprintf(packet+IDT_LOC_MSEC, ".777");
   int intense = packet[IDT_LOC_INTENSE] = 1;
   int nbeams = packet[IDT_LOC_NBEAMS+1] = 10;
   uint16_t size = IDT_HDR_SIZE + nbeams*IDT_B_BYTES + nbeams*intense*IDT_I_BYTES;
   uint16_t roff = IDT_HDR_SIZE;
   uint16_t ioff = roff + nbeams*IDT_B_BYTES;
   for (int i = 0; i < nbeams; i++)
   {
      packet[roff+i*IDT_B_BYTES+1] = i;
      packet[ioff+i*IDT_I_BYTES+1] = i*10;
   }
   if ( (status = parseIdt83p(&data, packet, size)) > 0)
   {
      LOG_DEBUG("Good packet: %d, timestamp %.3f",
         status, data.timestamp);

      for (int b = 0; b < data.nbeams; b++)
      {
         LOG_DEBUG("Range  [%d] = %.2f", b, data.range[b]);
         LOG_DEBUG("Intense[%d] = %d",   b, data.intense[b]);
      }
   }
   else
   {
      LOG_ERROR("Failed to parse packet: %d", status);
   }

   // get a known good packet and test

   return 0;
}