/**
 * @file pcaptoradar.c
 * @author Eric Martin
 * @brief
 * @version 0.1
 * @date 2020-11-23
 *
 * @copyright MBARI (c) 2020
 *
 */
// Includes
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <pcap.h>
#include <netinet/in.h>
#include <net/ethernet.h>
#include <arpa/inet.h>

#include <string.h>

#include "radar_4g.h"
#include "radar_pcap.h"
#include "radar_hdf5.h"
#include "lcm_ahrs_pcap.h"

// Function Prototypes
void packet_handler(u_char *args, const struct pcap_pkthdr *header, const u_char *packet);
void print_radar_report_packet(const struct pcap_pkthdr *header, const u_char *packet);
void process_radar_report_packet_a(const struct pcap_pkthdr *header, const u_char *packet);
void process_radar_report_packet_b(const struct pcap_pkthdr *header, const u_char *packet);
void process_radar_data_packet_a(const struct pcap_pkthdr *header, const u_char *packet);
void process_radar_data_packet_b(const struct pcap_pkthdr *header, const u_char *packet);
void process_ahrs_packet(const struct pcap_pkthdr *header, const u_char *packet);
void get_packet_headers(const u_char *packet, u_char **ip_header, u_char **payload);
void print_usage(char *program_name);
int open_pcap_file(char *fname);
void close_pcap_file();
void reset_packet_counts();
void print_radar_report_header();
void print_radar_scan_header();

//* variables
pcap_t *handle;
uint8_t *a_frame;
uint8_t *b_frame;
uint8_t f_channel = 0x00;
uint8_t quiet = 0;
uint8_t save_h5 = 0;
uint16_t current_id_a = 0;
uint16_t current_id_b = 0;
uint64_t n_frames_a = 0;
uint64_t n_frames_b = 0;
uint64_t n_reports_a = 0;
uint64_t n_reports_b = 0;
uint64_t n_ahrs = 0;
int line_out_limit = 0;
uint64_t n_lines_out = 0;

int main(int argc, char *argv[])
{
	//* options
	int opt, report_header_flag, scan_header_flag, ahrs_header_flag;

	scan_header_flag = 0;
	ahrs_header_flag = 0;
	report_header_flag = 0;

	//* action flags
	uint8_t print_stats = 0;

	//* stored config items
	char *hdf_fname;

	while ((opt = getopt(argc, argv, ":qaibshn:AIBHo:")) != -1)
	{
		switch (opt)
		{
		case 'q':
			quiet = 1;
			break;
		case 'A': // scans from A
			f_channel |= 0x01;
			break;
		case 'B': // scans from B
			f_channel |= 0x02;
			break;
		case 'a': // channel a reports only
			f_channel |= 0x4;
			break;
		case 'b': // channel b reports only
			f_channel |= 0x8;
			break;
		case 'i': // lcm ahrs data
			f_channel |= 0x10;
			break;
		case 'I': // ahrs header added
			ahrs_header_flag = 1;
			break;
		case 'h': // reports header added
			report_header_flag = 1;
			break;
		case 'H': // header for scans
			scan_header_flag = 1;
			break;
		case 's': // print stats after processing
			print_stats = 1;
			break;
		case 'n': // limit line output
			line_out_limit = atoi(optarg);
			fprintf(stderr, "Line output limited to %i lines.\n", line_out_limit);
			break;
		case 'o':
			hdf_fname = optarg;
			save_h5 = 1;
			break;

		case ':':
		default:
			fprintf(stderr, "Unknown or misused option.\n\n");
			print_usage(argv[0]);
			return (-1);
		}
	}

	//* check if there is a filename
	if (argv[optind] == NULL)
	{
		print_usage(argv[0]);
		return (-2);
	}

	char *fname = argv[optind];

	//* memory allocation for 2 channels
	a_frame = calloc(1, sizeof(struct radar_frame_pkt));
	b_frame = calloc(1, sizeof(struct radar_frame_pkt));

	//* collect file stats by looping through in quiet mode
	uint8_t _quiet = quiet;
	uint8_t _save_h5 = save_h5;
	save_h5 = 0;
	quiet = 1;
	fprintf(stderr, "Opening file: %s\n", fname);
	if (open_pcap_file(fname) != 0) return -3;
	pcap_loop(handle, 0, packet_handler, NULL);
	pcap_close(handle);
	quiet = _quiet;
	save_h5 = _save_h5;

	//* output statistics
	if (print_stats)
	{
		fprintf(stderr, "----------------\r\n");
		fprintf(stderr, "REPORTS_A: %llu\r\n", n_reports_a);
		fprintf(stderr, "REPORTS_B: %llu\r\n", n_reports_b);
		fprintf(stderr, "FRAMES_A:  %llu\r\n", n_frames_a);
		fprintf(stderr, "  SCANS:     %llu\r\n", n_frames_a * SCANS_PER_FRAME);
		fprintf(stderr, "  REVS_A:    %llu\r\n", n_frames_a * SCANS_PER_FRAME / SPOKES);
		fprintf(stderr, "AHRS:      %llu\r\n", n_ahrs);
		fprintf(stderr, "----------------\r\n");
	}

	//* Print CSV Headers, if requested
	if (report_header_flag == 1)
		print_radar_report_header();
	if (scan_header_flag == 1)
		print_radar_scan_header();
	if (ahrs_header_flag == 1)
		print_ahrs_data_csv_header();

	//* Setup HDF5 File, if output flag set
	if (save_h5)
	{
		//* Allocate space to store full datasets before looping again
		//  Frames
		h5_malloc_scans_a(n_frames_a * SCANS_PER_FRAME);
		h5_malloc_scans_b(n_frames_b * SCANS_PER_FRAME);

		//  Reports
		h5_malloc_reports_a(n_reports_a);
		h5_malloc_reports_b(n_reports_b);
		//  AHRS
		h5_malloc_ahrs(n_ahrs);
	}

	//* loop over the file
	reset_packet_counts();
	open_pcap_file(fname);
	pcap_loop(handle, 0, packet_handler, NULL);
	pcap_close(handle);

	//* Data should be in memory, copy to file
	if (save_h5)
	{
		fprintf(stderr, "Saving Data to: %s\n", hdf_fname);
		//* open file
		h5_open(hdf_fname);
		//* Create datasets
		// TODO move the below to the final data write.
		h5_write_scans_a(n_frames_a*SCANS_PER_FRAME);
		h5_write_scans_b(n_frames_b*SCANS_PER_FRAME);
		h5_write_reports_a(n_reports_a);
		h5_write_reports_b(n_reports_b);
		h5_write_ahrs();
		h5_close();
	}

	//* Memory Cleanup
	h5_free_all();
	return 0;
}

/**
 * @brief Print a header for csv processing
 *
 */
void print_radar_report_header()
{
	if (!quiet)
	{
		n_lines_out++;
		printf("time,channel,range,gain,sea_auto,sea,rain,");
		printf("interference_rejection,target_expansion,target_boost\n");
	}
}

/**
 * @brief Print a header for csv processing
 *
 */
void print_radar_scan_header()
{
	if (!quiet)
	{
		n_lines_out++;
		printf("time,channel,scan_line,heading,angle,angle_d,range,");
		for (int i = 0; i < SPOKELEN; i++)
			printf("r%04i,", i);
		printf("\r\n");
	}
}

/**
 * @brief pcap loop handler function as the file is read in, packets are processed here.
 */
void packet_handler(u_char *args, const struct pcap_pkthdr *header, const u_char *packet)
{
	//* Filter unwanted packets
	struct ether_header *eth_header;
	eth_header = (struct ether_header *)packet;
	if (ntohs(eth_header->ether_type) != ETHERTYPE_IP)
		return;

	//* check it we're done doing anything with data
	if (line_out_limit != 0 && n_lines_out >= line_out_limit)
		return;

	//* Filter out TCP, if any
	if (*(packet + 23) != IPPROTO_UDP)
		return;

	//* AHRS Data
	if (is_ahrs_packet(packet) == 1)
	{
		if (save_h5) {
			process_ahrs_packet(header, packet);
		}
		if (f_channel & 0x10 && !quiet)
		{
			ahrs_packet_handler(header, packet);
			n_lines_out++;
		}
		n_ahrs++;
		return;
	}

	//* Decode Channel
	int channel;
	channel = radar_packet_channel(packet);

	// * Unknown Packet
	if (channel == CH_UNKNOWN)
		return;

	//* Channel A Reports
	if (channel == CH_A_REPORT && header->len == 141)
	{
		if (save_h5)
		{
			process_radar_report_packet_a(header, packet);
		}
		if (f_channel & 0x04 && !quiet)
		{
			n_lines_out++;
			print_radar_report_packet(header, packet);
		}
		n_reports_a++;
	}

	//* Channel B Reports
	if (channel == CH_B_REPORT && header->len == 141)
	{
		if (save_h5)
		{
			process_radar_report_packet_b(header, packet);
		}
		if (f_channel & 0x08 && !quiet)
		{
			n_lines_out++;
			print_radar_report_packet(header, packet);
		}
		n_reports_b++;
	}

	//*  Channel A Data
	if (channel == CH_A_DATA)
	{
		process_radar_data_packet_a(header, packet);
	}

	//*  Channel B Data
	if (channel == CH_B_DATA)
	{
		process_radar_data_packet_b(header, packet);
	}

}

/**
 * @brief failover printout for misuse of the program
 */
void print_usage(char *program_name)
{
	printf("radar packet parser, usage %s <pcap filename>\r\n", program_name);
	printf("  flags: -a | -b      : print report data for channels A or B\r\n");
	printf("         -h           : add csv header line for reports, use with -a/-b\r\n");
	printf("         -A | -B      : print scanline data for channels A or B\r\n");
	printf("         -H           : add csv header line for scan data, use with -A/-B\r\n");
	printf("         -i           : print imu data from LCM packets in record\r\n");
	printf("         -I           : add csv header line for imu data\r\n");
	printf("         -o [OUTFILE] : convert all data to HDF5 format with filename [OUTFILE]\r\n");
	printf("         -s           : print statistics for information decodable in the file\r\n");
	printf("         -q           : quiet mode, suppress output\r\n");
}

void process_ahrs_packet(const struct pcap_pkthdr *header, const u_char *packet)
{
	const uint8_t *ip_header;
	const uint8_t *payload;
	const uint8_t *data;
	pcap_packet_headers(packet, &ip_header, &payload);
	double time = (double)(header->ts.tv_sec) + (double)(header->ts.tv_usec)/1e6;

    // 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);
	h5_copy_ahrs(n_ahrs, &time, &msg);
}

void process_radar_report_packet_a(const struct pcap_pkthdr *header, const u_char *packet){
	const uint8_t *ip_header;
	const uint8_t *payload;
	const uint8_t *data;
	pcap_packet_headers(packet, &ip_header, &payload);

	data = payload + 8;

	double time = (double)(header->ts.tv_sec) + (double)(header->ts.tv_usec)/1e6;
	RadarReport_02C4_99_t *report;
	report = (RadarReport_02C4_99_t *)data;

	h5_copy_report_a(n_reports_a, &time, report);
}
void process_radar_report_packet_b(const struct pcap_pkthdr *header, const u_char *packet){
	const uint8_t *ip_header;
	const uint8_t *payload;
	const uint8_t *data;
	pcap_packet_headers(packet, &ip_header, &payload);

	data = payload + 8;

	double time = (double)(header->ts.tv_sec) + (double)(header->ts.tv_usec)/1e6;
	RadarReport_02C4_99_t *report;
	report = (RadarReport_02C4_99_t *)data;

	h5_copy_report_b(n_reports_a, &time, report);
}
void print_radar_report_packet(const struct pcap_pkthdr *header, const u_char *packet)
{
	printf("%li.%06i,", header->ts.tv_sec, header->ts.tv_usec);
	print_radar_report_2C99(packet);
	printf("\r\n");
}

void process_radar_data_packet_a(const struct pcap_pkthdr *header, const u_char *packet)
{
	// get the individual header pointers
	const uint8_t *ip_header;
	const uint8_t *payload;
	double time[SCANS_PER_FRAME], angle[SCANS_PER_FRAME];
	int range[SCANS_PER_FRAME];
	pcap_packet_headers(packet, &ip_header, &payload);
	long header_size = payload - packet;
	size_t payload_nbytes = (size_t)header->len - header_size;

	// collect the id and offset, make sure it hasn't changed.
	uint16_t offset = radar_packet_offset(packet);
	uint16_t id = radar_packet_id(packet);
	//fprintf(stderr,"%x\n",id);
	uint8_t is_fragment = pcap_packet_is_fragment(packet);

	// local storage for decoded frame data
	uint8_t data[SCANS_PER_FRAME][SPOKELEN];

	// check that the id matches our last packet
	if (current_id_a == 0 && is_fragment && offset == 0)
	{
		current_id_a = id;
	}
	else if (current_id_a != id && !quiet)
	{
		fprintf(stderr, "WARN: CHA Fragmented Packet out of place (off: %u frag: %u)(id0: %x id1: %x).\n", offset, is_fragment, current_id_a, id);
		return;
	}

	// copy the data into its offset
	memcpy(a_frame + offset, payload, payload_nbytes);

	// well if its not a fragment, we should assume its done
	if (!is_fragment)
	{
		current_id_a = 0;
		if (save_h5)
		{
			decode_radar_frame((struct radar_frame_pkt *)(a_frame + 8), header->ts, data, time, angle, range);
			h5_copy_frame_a(n_frames_a, data, time, angle, range);
		}
		if (f_channel & 0x01 && !quiet)
		{
			n_lines_out++;
			print_radar_frame((struct radar_frame_pkt *)(a_frame + 8), header->ts, 'A');
		}
		n_frames_a++;
	}
}

void process_radar_data_packet_b(const struct pcap_pkthdr *header, const u_char *packet)
{
	// get the individual header pointers
	const uint8_t *ip_header;
	const uint8_t *payload;
	double time[SCANS_PER_FRAME], angle[SCANS_PER_FRAME];
	int range[SCANS_PER_FRAME];
	pcap_packet_headers(packet, &ip_header, &payload);
	long header_size = payload - packet;
	size_t payload_nbytes = (size_t)header->len - header_size;

	// collect the id and offset, make sure it hasn't changed.
	uint16_t offset = radar_packet_offset(packet);
	uint16_t id = radar_packet_id(packet);
	uint8_t is_fragment = pcap_packet_is_fragment(packet);

	// local storage for decoded frame data
	uint8_t data[SCANS_PER_FRAME][SPOKELEN];

	// check that the id matches our last packet
	if (current_id_b == 0 && is_fragment && offset == 0)
	{
		current_id_b = id;
	}
	else if (current_id_b != id && !quiet)
	{
		fprintf(stderr, "WARN: CHB Fragmented Packet out of place (off: %u frag: %u)(id0: %x id1: %x).\n", offset, is_fragment, current_id_b, id);
		return;
	}

	// copy the data into its offset
	memcpy(b_frame + offset, payload, payload_nbytes);

	// well if its not a fragment, we should assume its done
	if (!is_fragment)
	{
		current_id_b = 0;
		if (save_h5)
		{
			decode_radar_frame((struct radar_frame_pkt *)(a_frame + 8), header->ts, data, time, angle, range);
			h5_copy_frame_b(n_frames_b, data, time, angle, range);
		}
		if (f_channel & 0x01 && !quiet)
		{
			n_lines_out++;
			print_radar_frame((struct radar_frame_pkt *)(b_frame + 8), header->ts, 'B');
		}
		n_frames_b++;
	}
}

void reset_packet_counts()
{
	current_id_a = 0;
	current_id_b = 0;
	n_frames_a = 0;
	n_frames_b = 0;
	n_reports_a = 0;
	n_reports_b = 0;
	n_ahrs = 0;
}

int open_pcap_file(char *fname)
{
	char error_buffer[PCAP_ERRBUF_SIZE];
	handle = pcap_open_offline(fname, error_buffer);
	if (handle == NULL)
	{
		fprintf(stderr, "Error: cannot open file...\n");
		return (-3);
	}

	return 0;
}

void close_pcap_file()
{
	pcap_close(handle);
}