#!/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);