/**
 * @file parse_lcm_ahrs.c
 * @author eric martin (emartin@mbari.org)
 * @brief 
 *      this file demonstrates use of pcap file ingestion
 *      via the use of libpcap to parse packet capture files
 *      of LCM (http://lcm-proj.github.io) traffic of a known 
 *      singular channel of traffic available. 
 * 
 *      when other data types become present, channel name 
 *      based filtering of lcm-packets prior to attempted 
 *      decoding has been implemented, and the channel name 
 *      "AHRS" is hard coded in this example. 
 *  
 * @version 0.1
 * @date 2020-05-18
 * 
 * @copyright Copyright MBARI (c) 2020
 * 
 */

// Includes
#include <stdio.h>
#include <string.h>
#include <pcap.h>
#include <lcm/lcm.h>

// Local Includes
#include "lcmtypes_wec_ahrs_data_l.h"

// Defines
#define CHANNEL_OFFSET 50
#define DATA_OFFSET 55


// Function Prototypes
void print_ahrs_data_csv(lcmtypes_wec_ahrs_data_l * message);
void print_usage(char * program_name);
void print_packet_info(const u_char *packet, struct pcap_pkthdr * packet_header); 
void ahrs_packet_handler(u_char *args, const struct pcap_pkthdr * header, const u_char *packet);
void print_ahrs_data_csv_header();

/**
 * @brief Main function
 * 
 * @return int < 0 on failure, 0 on sucess
 */
int main(int argc, char *argv[]) {
    if (argc < 2) {
        print_usage(argv[0]);
        return -1;
    }

    // load the pcap file
    fprintf(stderr,"Opening file: %s\n", argv[1]);
    char error_buffer[PCAP_ERRBUF_SIZE];
    pcap_t * handle = pcap_open_offline(argv[1], error_buffer);

    print_ahrs_data_csv_header();
    //loop over the file
    pcap_loop(handle, 0, ahrs_packet_handler, NULL);
    return 0;
}

/**
 * @brief failover printout for misuse of the program
 */
void print_usage(char * program_name) {
    printf("ahrs lcm parser, usage %s <pcap filename>\n", program_name);
}

/**
 * @brief pcap loop handler function as the file is read in, packets are processed here.
 */
void ahrs_packet_handler(u_char *args, const struct pcap_pkthdr * header, const u_char *packet){
    // print packet info
    //print_packet_info(packet, *header);
    
    // check the channel name is correct
    if (strcmp((char*)packet+CHANNEL_OFFSET,"AHRS") != 0)
        return;

    // transcode that message
    lcmtypes_wec_ahrs_data_l msg;
    int decode_success = lcmtypes_wec_ahrs_data_l_decode(packet, DATA_OFFSET, 256, &msg);
    
    if (decode_success > 0) {
        print_ahrs_data_csv(&msg);
    }
}

/**
 * @brief prints the pcap packet information
 */
void print_packet_info(const u_char *packet, struct pcap_pkthdr * packet_header) {
    printf("Packet capture length: %d\n", packet_header->caplen);
    printf("Packet total length %d\n", packet_header->len);
}

/**
 * @brief prints the header for the ahrs data in csv form
 */
void print_ahrs_data_csv_header() {
    printf("time,latitude,longitude,yaw_true,pitch,roll,x_accel,y_accel,z_accel,n_velocity,e_velocity,d_velocity,altitude,x_rate,y_rate,z_rate, x_rate_temp,\n");
}

/**
 * @brief prints csv formatted data available in the ahrs lcm packet
 */
void print_ahrs_data_csv(lcmtypes_wec_ahrs_data_l * message) {
    printf("%f,", message->timestamp);
    printf("%f,", message->latitude);
    printf("%f,", message->longitude);
    printf("%f,", message->yawAngleTrue);
    printf("%f,", message->pitchAngle );
    printf("%f,", message->rollAngle );
    printf("%f,", message->xAccel );
    printf("%f,", message->yAccel );
    printf("%f,", message->zAccel );
    printf("%f,", message->nVelocity );
    printf("%f,", message->eVelocity );
    printf("%f,", message->dVelocity );
    printf("%f,", message->altitude );
    printf("%f,", message->xRateCorrected );
    printf("%f,", message->yRateCorrected );
    printf("%f,", message->zRateCorrected );
    printf("%f,", message->xRateTemp );
    printf("\n");
}