#!/usr/bin/perl

#======================================================================

#                    P A T C H P D 0 

#                    doc: Tue Aug 23 20:00:15 2016

#                    dlm: Mon Nov 20 12:27:15 2017

#                    (c) 2010 A.M. Thurnherr

#                    uE-Info: 32 16 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

# 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 = 6.7;

$ADCP_tools_minVersion  = 1.8;

require "$ANTS/ants.pl";

require "$ANTS/libvec.pl";

require "$ANTS/libstats.pl";

require "$ADCP_TOOLS/ADCP_tools_lib.pl";

$antsParseHeader = 0;

&antsUsage('dhko:pr',2,

	'[patch -p)itch] [-r)oll] [-h)eading] (none patches all)',

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

$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)) {

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

#	print(STDERR "assertion succeeded [$ants_[0][$ensF] == $LADCP{ENSEMBLE}[$ens]->{NUMBER} --- 1-$LADCP{ENSEMBLE}[0]->{NUMBER} + $P{RECNO} + $d]\n");

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