#!/usr/bin/perl
#======================================================================
#                    P A T C H P D 0 
#                    doc: Tue Aug 23 20:00:15 2016
#                    dlm: Tue Apr 14 21:28:49 2020
#                    (c) 2010 A.M. Thurnherr
#                    uE-Info: 103 26 NIL 0 0 72 2 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
#	Jun 13, 2017: - added pitch and roll to -o
#				  - BUG: ??? does -o handle pitch and roll ANOMALIES correctly?
#	Jun 30, 2019: - -o did not work with single argument
#	Apr 14, 2020: - adapted to use for moored ADCP data as well

# PATCH-FILE REQUIREMENTS (ANTS format)
#	- %[L]ADCP_pitch.mu %[L]ADCP_roll.mu	mean ADCP pitch and roll
#	- %IM[UP]_hdg_offset					heading offset of external IMU
#	- [L]ADCP_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 values]',
	'[-o) <[pitch,roll,]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;

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

#----------------------------------------------------------------------
# Step 1: Read ADCP Data
#----------------------------------------------------------------------

readData($ADCP_file,\%ADCP);												# TRDI PD0 file

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

&antsIn();																	# load first IMP record

my($ensF) = &fnrNoErr('ADCP_ens');
$ensF = &fnr('LADCP_ens') unless defined($ensF);

my($pitchF) = &fnr('pitch');
my($rollF)	= &fnr('roll');
my($hdgF)	= &fnr('hdg');

my($ADCP_pitch_mean) = $P{'ADCP_pitch.mu'};
$ADCP_pitch_mean = &antsRequireParam('LADCP_pitch.mu')
	unless numberp($ADCP_pitch_mean);

my($ADCP_roll_mean) = $P{'ADCP_roll.mu'};
$ADCP_roll_mean = &antsRequireParam('LADCP_roll.mu')
	unless numberp($ADCP_roll_mean);

my($pofs,$rofs) = (0,0);													# apply externally supplied offset(s)
my($rho,$crho,$srho);												
if (defined($opt_o)) {
	my($pofs,$rofs,$hofs) = split(/,/,$opt_o);

	if (defined($rofs)) {													# pitch and roll offsets supplied
		croak("$0: cannot decode -o $opt_o\n")
			unless numbersp($pofs,$rofs,$hofs);
	} else {																# no pitch and roll, only heading
		$hofs = $pofs; 
		$pofs = undef;
    }
	croak("$0: cannot decode -o $opt_o\n")									
		unless numberp($hofs);
																			# set up heading correction
	&antsAddParams('IMU_hdg_offset',$P{IMP_hdg_offset})						# backward compatibility
		if defined($P{IMP_hdg_offset});
	$rho  = $hofs - &antsRequireParam('IMU_hdg_offset');					# calculate correction relative to already applied one 
	$crho = cos(rad($rho));
	$srho = sin(rad($rho));

	if (defined($pofs)) {													# rotate IMP pitch and roll offsets into new ADCP frame
		my($IMP_pitch_mean) =  &antsRequireParam('IMP_pitch.mu') * $crho
									+ &antsRequireParam('IMP_roll.mu') * $srho;
		my($IMP_roll_mean)  = -&antsRequireParam('IMP_pitch.mu') * $srho
									+ &antsRequireParam('IMP_roll.mu') * $crho;
		$ADCP_pitch_mean = $IMP_pitch_mean - $pofs;						# apply externally supplied offsets
		$ADCP_roll_mean  = $IMP_roll_mean  - $rofs;
	}
}

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

do {
	my($ADCP_deployed);
	my($ens) = $P{RECNO};
	die("assertion failed [$ants_[0][$ensF] != $ADCP{ENSEMBLE}[$ens]->{NUMBER} --- 1-$ADCP{ENSEMBLE}[0]->{NUMBER} + $P{RECNO} + $d]")
		unless ($ants_[0][$ensF] == $ADCP{ENSEMBLE}[$ens]->{NUMBER});
	$ADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} = 0xA0;
	
	if (numbersp($ants_[0][$pitchF],$ants_[0][$rollF])) {					# valid IMP data -> patch ADCP ensemble
		$ADCP_deployed = 1;
		$missing_pr_block_len = 0;
		if (defined($opt_o)) {												# -o set: rotate pitch and roll into correct coordinates
			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) {														# patch pitch
			$ADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_p<<2);
			$ADCP{ENSEMBLE}[$ens]->{PITCH} = RDI_pitch($ADCP_pitch_mean + $ants_[0][$pitchF],
														$ADCP_roll_mean  + $ants_[0][$rollF]);
		}
		if ($opt_r) {														# patch roll
			$ADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_r<<1);
			$ADCP{ENSEMBLE}[$ens]->{ROLL} = $ADCP_roll_mean + $ants_[0][$rollF];
		}
    } else {																# no valid IMP pitch and roll => invalidate ADCP data
    	$pr_missing++ if $ADCP_deployed;									# don't count missing before deployment
    	$missing_pr_block_len++;
		unless ($opt_k)  {
	    	clearEns(\%ADCP,$ens);
	    	$ADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0;
	    }
    }
	    
    if (numberp($ants_[0][$hdgF])) {										# valid IMP heading
    	$ADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= $opt_h;
		$missing_hdg_block_len = 0;
   		if (defined($opt_o)) {												# apply offset on -o; otherwise, data are correctly rotated
	    	$ants_[0][$hdgF] -= $rho;
	    	$ants_[0][$hdgF] += 360 if ($ants_[0][$hdgF] < 0);
	    }
		$ADCP{ENSEMBLE}[$ens]->{HEADING} = $ants_[0][$hdgF]				# patch heading
			if $opt_h;
	} else {																# no valid IMP heading => invalidate ADCP data
		$hdg_missing++ if $ADCP_deployed;
		$missing_hdg_block_len++;
		unless ($opt_k)  {
	    	clearEns(\%ADCP,$ens);
	    	$ADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0;
	    }
	}
} while (&antsIn());
$pr_missing  -= missing_pr_block_len;										# don't count final block (post recovery)
$hdg_missing -= missing_hdg_block_len;

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

writeData($outPD0,\%ADCP);													# write new PD0

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);