/**
 * @file radar_4g.c
 * @author Eric Martin (emartin@mbari.org)
 * @brief 
 * @version 0.1
 * @date 2020-11-24
 * 
 * @copyright Copyright (c) 2020 MBARI
 * 
 */
#include "radar_4g.h"

/**
 * @brief Determine the range repored for the scan line
 * 
 * @param line radar line data packet
 * @return int range in meters 
 * TODO this differs from the report range.
 */
int line_range(struct radar_line * line) {
	short int large_range = (line->br4g.largerange[1] << 8) | line->br4g.largerange[0];
	short int small_range = (line->br4g.smallrange[1] << 8) | line->br4g.smallrange[0];
	int range_meters = 0;

	if (large_range == 0x80)
	{
		if (small_range == -1)
		{
            range_meters = 0;  // Invalid range received
          } else {
            range_meters = small_range / 4;
          }
	}
	else
	{
		range_meters = large_range * 64;
	}

	return range_meters;
}


/**
 * @brief Determine the heading reported for the scan line
 * 
 * @param line radar line data packet
 * @return int uncalibrated heading value.
 */
short int line_heading(struct radar_line * line) {
	return (line->br4g.heading[1] << 8) | line->br4g.heading[0];
}

/**
 * @brief Determine the reported raw angle for the scan line
 * 
 * @param line radar line data packet
 * @return int uncalibrated angle value.
 */
int line_angle(struct radar_line * line) {
	return (line->br4g.angle[1] << 8) | line->br4g.angle[0];
}

/**
 * @brief Determine the head relative scan angle
 * 
 * @param line radar line data packet
 * @return double angle in degrees
 */
double line_angle_degrees(struct radar_line * line) {
	int raw_angle = line_angle(line);
	return SCALE_RAW_TO_DEGREES(raw_angle);
}