#!/usr/bin/perl
#======================================================================
#                    P A T C H P D 0 
#                    doc: Tue Aug 23 20:00:15 2016
#                    dlm: Sat Dec 23 16:12:01 2017
#                    (c) 2010 A.M. Thurnherr
#                    uE-Info: 51 24 NIL 0 0 72 0 2 4 NIL ofnI
#======================================================================

$antsSummary = 'patch TRDI PD0 file with external attitude data';

# History:
#	Aug 23, 2016: - exported from IMP+LADCP
#	Aug 25, 2016: - completed basic structure
#	Nov 20, 2017: - major code cleanup
#				  - added -d) to keep original data source id
#	Dec  9, 2017: - added $antsSuppressCommonOptions = 1;
#	Dec 23, 2017: - added support for -c
#				  - BUG: not backward compatible with old IMP files any more

# PATCH-FILE REQUIREMENTS (ANTS format)
#	- %LADCP_pitch.mu %LADCP_roll.mu		mean LADCP pitch and roll
#	- %IMU_hdg_offset						heading offset of external IMU
#	- LADCP_ens								ADCP ensemble number
#	- pitch, roll							external pitch/roll *anomalies* 
#	- hdg									external heading rotated into ADCP coord system

# PATCHED PD0 FILE:
#
#	- pitch = RDI_pitch(mean_LADCP_gimbal_pitch + rotated_external_pitch_anomaly)
#	- roll  = mean_LADCP_roll + rotated_external_roll_anomaly
#	- hdg	= external_hdg - heading_offset
#
#	- unless -d is used, every patched ensemble has set the DATA_SOURCE_ID as follows; 
#	  PROCESSING SOFTWARE NEEDS TO BE ABLE TO DEAL WITH DSID values != 0x7F
#		0xA0	no values patched
#		0xA1	heading patched
#		0xA2	roll patched
#		0xA3	roll & heading patched
#		0xA4	pitch patched
#		0xA5	pitch & heading patched
#		0xA6	pitch & roll patched
#		0xA7	pitch, roll & heading patched
#
#	- additionally, all velocities from ensembles with missing pitch/roll/heading
#	  values are removed unless -k is set

($ANTS)    = (`which ANTSlib` =~ m{^(.*)/[^/]*$});
($ADCP_TOOLS) = ($0 =~ m{^(.*)/[^/]*$});

$antsMinLibVersion = 7.0;
$ADCP_tools_minVersion  = 2.1;

require "$ANTS/ants.pl";
require "$ANTS/libvec.pl";
require "$ANTS/libstats.pl";
require "$ADCP_TOOLS/ADCP_tools_lib.pl";

$antsParseHeader = 0;
$antsSuppressCommonOptions = 1;
&antsUsage('cdhko:pr',2,
	'[patch -p)itch] [-r)oll] [-h)eading] (none patches all)',
	'[patch -c)lock with pre-Y2K RTC calues]',
	'[-o) <heading-offset>] [-k)eep velocities of unpatched ensembles]',
	'[keep original -d)ata-source id]',
	'<original PD0 file> <patched PD0 file> [external attitude file]');

$opt_p = $opt_r = $opt_h = 1
	unless ($opt_p || $opt_r || $opt_h);

$RDI_PD0_IO::OVERRIDE_Y2K_CLOCK = $opt_c;

$LADCP_file  = &antsFileArg();
$outPD0 = $ARGV[0]; shift;

#----------------------------------------------------------------------
# Step 1: Read LADCP Data
#----------------------------------------------------------------------

readData($LADCP_file,\%LADCP);

#----------------------------------------------------------------------
# Step 2: Process External Attidue Input to Patch PD0 file
#----------------------------------------------------------------------

my($pr_missing,$hdg_missing) = (0,0);

&antsIn();

my($ensF) 	= &fnr('LADCP_ens');
my($pitchF) = &fnr('pitch');
my($rollF)	= &fnr('roll');
my($hdgF)	= &fnr('hdg');
my($LADCP_pitch_mean)	= &antsRequireParam('LADCP_pitch.mu');
my($LADCP_roll_mean)	= &antsRequireParam('LADCP_roll.mu');

my($rho,$crho,$srho);
if (defined($opt_o)) {
	&antsAddParams('IMU_hdg_offset',$P{IMP_hdg_offset})						# backward compatibility
		if defined($P{IMP_hdg_offset});
	$rho  = $opt_o - &antsRequireParam('IMU_hdg_offset');
	$crho = cos(rad($rho));
	$srho = sin(rad($rho));
}

do {
	my($ens) = $P{RECNO};
	die("assertion failed [$ants_[0][$ensF] != $LADCP{ENSEMBLE}[$ens]->{NUMBER} --- 1-$LADCP{ENSEMBLE}[0]->{NUMBER} + $P{RECNO} + $d]")
		unless ($ants_[0][$ensF] == $LADCP{ENSEMBLE}[$ens]->{NUMBER});
	$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} = 0xA0;
	
	if (numbersp($ants_[0][$pitchF],$ants_[0][$rollF])) {
		if (defined($opt_o)) {
			my($rot_p) = ($ants_[$r][$pitchF]  * $crho +
						  $ants_[$r][$rollF]   * $srho);
			my($rot_r) = (-$ants_[$r][$pitchF] * $srho +
						   $ants_[$r][$rollF]  * $crho);
			$ants_[$r][$pitchF] = $rot_p;
			$ants_[$r][$rollF]	= $rot_r;
        } 
		if ($opt_p) {
			$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_p<<2);
			$LADCP{ENSEMBLE}[$ens]->{PITCH} = RDI_pitch($LADCP_pitch_mean + $ants_[0][$pitchF],
														$LADCP_roll_mean  + $ants_[0][$rollF]);
		}
		if ($opt_r) {
			$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_r<<1);
			$LADCP{ENSEMBLE}[$ens]->{ROLL} = $LADCP_roll_mean + $ants_[0][$rollF];
		}
    } else {
    	$pr_missing++;
		unless ($opt_k)  {
	    	clearEns(\%LADCP,$ens);
	    	$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0;
	    }
    }
	    
    if (numberp($ants_[0][$hdgF])) {
    	$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= $opt_h;
   		if (defined($opt_o)) {
	    	$ants_[0][$hdgF] -= $rho;
	    	$ants_[0][$hdgF] += 360 if ($ants_[0][$hdgF] < 0);
	    }
		$LADCP{ENSEMBLE}[$ens]->{HEADING} = $ants_[0][$hdgF]
			if $opt_h;
	} else {
		$hdg_missing++;
		unless ($opt_k)  {
	    	clearEns(\%LADCP,$ens);
	    	$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0;
	    }
	}
} while (&antsIn());

$LADCP{ENSEMBLE}[0]->{DATA_SOURCE_ID} = 0x7F;				# ensure correct DSID (1st ens: orig; 2nd ens: this prog)
$LADCP{ENSEMBLE}[1]->{DATA_SOURCE_ID} = 0xA0
	unless ($LADCP{ENSEMBLE}[1]->{DATA_SOURCE_ID}&0xF0 == 0xA0);

writeData($outPD0,\%LADCP);

my($verb) = $opt_k ? 'retained' : 'cleared';
printf(STDERR "$outPD0: %d pitch/roll & %d heading values $verb\n",$pr_missing,$hdg_missing)
       if ($pr_missing+$hdg_missing);
        
exit(0);