/**
 * @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
 * 
 */

#include "lcm_ahrs_pcap.h"


int is_ahrs_packet(const u_char * packet) {
    int retval = strcmp((char*)packet+LCM_CHANNEL_PKT_OFFSET,"AHRS");
    return (retval == 0)?1:0;
}
/**
 * @brief pcap loop handler function as the file is read in, packets are processed here.
 */
void ahrs_packet_handler(const struct pcap_pkthdr * header, const u_char *packet){

    // check the channel name is correct other channels will be available. 
    if (strcmp((char*)packet+LCM_CHANNEL_PKT_OFFSET,"AHRS") != 0)
        return;

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

/**
 * @brief prints the header for the ahrs data in csv form
 */
void print_ahrs_data_csv_header() {
    printf("pcap_time,lcm_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, const struct pcap_pkthdr * packet_header) {
    printf("%li.%06i,",packet_header->ts.tv_sec,packet_header->ts.tv_usec);
    printf("%.6f,", message->timestamp);
    printf("%.15f,", message->latitude);
    printf("%.15f,", 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");
}