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

#                    R D I _ C O O R D S . P L 

#                    doc: Sun Jan 19 17:57:53 2003

#                    (c) 2003 A.M. Thurnherr

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

# RDI Workhorse Coordinate Transformations

# HISTORY:

#	Jan 19, 2003: - written

#	Jan 21, 2003: - made it obey HEADING_BIAS (magnetic declination)

#	Jan 22, 3003: - corrected magnetic declination

#	Feb 16, 2003: - use pitch correction from RDI manual

#	Oct 11, 2003: - BUG: return value of atan() had been interpreted

#					     as degrees instead of radians

#	Feb 27, 2004: - added velApplyHdgBias()

#				  - changed non-zero HEADING_ALIGNMENT from error to warning

#	Sep 16, 2005: - added deg() for [mkprofile]

#	Aug 26, 2006: - BUG: incorrect transformation for uplookers

#	Nov 30, 2007: - optimized &velInstrumentToEarth(), velBeamToInstrument()

#				  - added support for 3-beam solutions

#	Feb 12, 2008: - added threeBeamFlag

#	Mar 18, 2009: - added &gimbal_pitch(), &angle_from_vertical()

#	May 19, 2009: - added &velBeamToVertical()

#	May 23, 2009: - debugged & renamed to &velBeamToBPEarth

#	May 23, 2010: - changed prototypes of rad() & deg() to conform to ANTS

#	Dec 20, 2010: - cosmetics

#	Dec 23, 2010: - added &velBeamToBPInstrument

#	Jan 22, 2011: - made velApplyHdgBias calculate sin/cos every time to allow

#				    per-ensemble corrections

#	Jan 15, 2012: - replaced defined(@...) by (@...) to get rid of warning

#	Aug  7, 2013: - BUG: &velBeamToBPInstrument did not return any val unless

#						 all beam velocities are defined

#	Nov 27, 2013: - added &RDI_pitch(), &tilt_azimuth()

#	Mar  4, 2014: - added support for ensembles with missing PITCH/ROLL/HEADING

#	May 29, 2014: - BUG: vertical velocity can be calculated even without

#						 heading

#				  - removed some old debug statements

#				  - removed unused code from &velBeamToBPInstrument

#	Jan  5, 2016: - added &velEarthToInstrument(@), &velInstrumentToBeam(@)

#	Jan  9, 2016: - added &velEarthToBeam(), &velBeamToEarth()

#	Feb 29, 2016: - debugged & verified velEarthToInstrument(), velInstrumentToBeam()

#				  - added velBeamToEarth()

#	May 19, 2016: - begin implemeting bin interpolation

#	May 25, 2016: - continued

#	May 26, 2016: - made it work

#	May 30, 2016: - begin implementing 2nd order attitude transformations

#	Jun  6, 2016: - toEarth transformation in beamToBPEarth was but crude approximation;

#					updated with transformation taken from Lohrman et al. (JAOT 1990)

#				  - BUG: v34 sign was inconsistent with RDI coord manual

#	Jun  8, 2016: - added $ens as arg to velInstrumentToBeam() for consistency

#	Jul  7, 2016: - added velEarthToBPw() with algorithm debugged and verified

#					by Paul Wanis from TRDI

#	Oct 12, 2017: - documentation

use strict;

use POSIX;

my($PI) = 3.14159265358979;

sub rad(@) { return $_[0]/180 * $PI; }

sub deg(@) { return $_[0]/$PI * 180; }

#----------------------------------------------------------------------

# Tweakables

#----------------------------------------------------------------------

$RDI_Coords::minValidVels = 3;				# 3-beam solutions ok (velBeamToInstrument)

$RDI_Coords::binMapping = 'linterp';		# 'linterp' or 'none' (earthVels, BPearthVels)

$RDI_Coords::beamTransformation = 'LHR90';	# set to 'RDI' to use 1st order transformations from RDI manual

#----------------------------------------------------------------------

# beam to earth transformation 

#----------------------------------------------------------------------

$RDI_Coords::threeBeam_1 = 0;			# stats from velBeamToInstrument

$RDI_Coords::threeBeam_2 = 0;

$RDI_Coords::threeBeam_3 = 0;

$RDI_Coords::threeBeam_4 = 0;

$RDI_Coords::fourBeam    = 0;

$RDI_Coords::threeBeamFlag = 0;			# flag last transformation

{ # STATIC SCOPE

	my(@B2I);

	sub velBeamToInstrument(@)

	{

		my($ADCP,$ens,$v1,$v2,$v3,$v4) = @_;

		return undef unless (defined($v1) + defined($v2) +

					   		 defined($v3) + defined($v4)

								>= $RDI_Coords::minValidVels);

		unless (@B2I) {

			my($a) = 1 / (2 * sin(rad($ADCP->{BEAM_ANGLE})));

			my($b) = 1 / (4 * cos(rad($ADCP->{BEAM_ANGLE})));

			my($c) = $ADCP->{CONVEX_BEAM_PATTERN} ? 1 : -1;

			my($d) = $a / sqrt(2);

			@B2I = ([$c*$a,	-$c*$a,	0,		0	 ],

				    [0,		0,		-$c*$a,	$c*$a],

				    [$b,	$b,		$b,		$b	 ],

				    [$d,	$d,		-$d,	-$d	 ]);

		}

		if (!defined($v1)) {					# 3-beam solutions

			$RDI_Coords::threeBeamFlag = 1;

			$RDI_Coords::threeBeam_1++;

			$v1 = -($v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][0];

		} elsif (!defined($v2)) {

			$RDI_Coords::threeBeamFlag = 1;

			$RDI_Coords::threeBeam_2++;

			$v2 = -($v1*$B2I[3][0]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][1];

		} elsif (!defined($v3)) {

			$RDI_Coords::threeBeamFlag = 1;

			$RDI_Coords::threeBeam_3++;

			$v3 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v4*$B2I[3][3])/$B2I[3][2];

		} elsif (!defined($v4)) {

			$RDI_Coords::threeBeamFlag = 1;

			$RDI_Coords::threeBeam_4++;

			$v4 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2])/$B2I[3][3];

		} else {

			$RDI_Coords::threeBeamFlag = 0;

			$RDI_Coords::fourBeam++;

		}

		return ($v1*$B2I[0][0]+$v2*$B2I[0][1],

				$v3*$B2I[1][2]+$v4*$B2I[1][3],

				$v1*$B2I[2][0]+$v2*$B2I[2][1]+$v3*$B2I[2][2]+$v4*$B2I[2][3],

				$v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3]);

	}

} # STATIC SCOPE

#----------------------------------------------------------------------

# velInstrumentToEarth(\%ADCP,ens,v1,v2,v3,v4) => (u,v,w,e)

#	- $RDI_Coords::beamTransformation = 'LHR90'

#		- from Lohrmann, Hackett & Roet (J. Tech., 1990)

#		- eq A1 maps to RDI matrix M (sec 5.6) with

#			alpha = roll

#			beta = gimball_pitch

#			psi = calculation_pitch

#			psi = asin{sin(beta) cos(alpha) / sqrt[1- sin(alpha)^2 sin(beta)^2]}

#		- (I only checked for 0 heading, but this is sufficient)

#	- $RDI_Coords::beamTransformation = 'RDI'

#		- default prior to LADCP_w V1.3

#		- from RDI manual

#		- 99% accurate for p/r<8deg

#			=> 1cm/s error for 1m/s winch speed!

#----------------------------------------------------------------------

{ # STATIC SCOPE

	my($hdg,$pitch,$roll,@I2E);

	sub velInstrumentToEarth(@)

	{

		my($ADCP,$ens,$v1,$v2,$v3,$v4) = @_;

		return undef unless (defined($v1) && defined($v2) &&

					   		 defined($v3) && defined($v4) &&

							 defined($ADCP->{ENSEMBLE}[$ens]->{PITCH}) &&

							 defined($ADCP->{ENSEMBLE}[$ens]->{ROLL}));

		unless (@I2E &&

				$pitch == $ADCP->{ENSEMBLE}[$ens]->{PITCH} &&

				$roll  == $ADCP->{ENSEMBLE}[$ens]->{ROLL}) {

			printf(STDERR "$0: warning HEADING_ALIGNMENT == %g ignored\n",

						  $ADCP->{HEADING_ALIGNMENT})

				if ($ADCP->{HEADING_ALIGNMENT});

			$hdg   = $ADCP->{ENSEMBLE}[$ens]->{HEADING} - $ADCP->{HEADING_BIAS}

				if defined($ADCP->{ENSEMBLE}[$ens]->{HEADING});

			$pitch = $ADCP->{ENSEMBLE}[$ens]->{PITCH};

			$roll  = $ADCP->{ENSEMBLE}[$ens]->{ROLL};

			my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll)));

			my($rad_calc_pitch) = ($RDI_Coords::beamTransformation eq 'RDI') ? $rad_gimbal_pitch : 

								  asin(sin($rad_gimbal_pitch)*cos(rad($roll)) /

									   sqrt(1-sin(rad($roll))**2*sin($rad_gimbal_pitch)**2));

			my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg)))

				if defined($hdg);				

			my($sp,$cp) = (sin($rad_calc_pitch),cos($rad_calc_pitch));

			my($sr,$cr) = (sin(rad($roll)),	cos(rad($roll)));

			@I2E = $ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}

				 ? (

					[-$ch*$cr-$sh*$sp*$sr,	$sh*$cp,-$ch*$sr+$sh*$sp*$cr],

					[-$ch*$sp*$sr+$sh*$cr,	$ch*$cp, $sh*$sr+$ch*$sp*$cr],

					[+$cp*$sr,				$sp,	-$cp*$cr,			],

				 ) : (

					[$ch*$cr+$sh*$sp*$sr,	$sh*$cp, $ch*$sr-$sh*$sp*$cr],

					[$ch*$sp*$sr-$sh*$cr,	$ch*$cp,-$sh*$sr-$ch*$sp*$cr],

					[-$cp*$sr,				$sp,	 $cp*$cr,			],

				 );

		}

		return defined($ADCP->{ENSEMBLE}[$ens]->{HEADING})

			   ? ($v1*$I2E[0][0]+$v2*$I2E[0][1]+$v3*$I2E[0][2],

				  $v1*$I2E[1][0]+$v2*$I2E[1][1]+$v3*$I2E[1][2],

				  $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2],

				  $v4)

			   : (undef,undef,

				  $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2],

				  $v4);

	}

} # STATIC SCOPE

sub velBeamToEarth(@)

{

	my($ADCP,$e,@v) = @_;

	return velInstrumentToEarth($ADCP,$e,velBeamToInstrument($ADCP,$e,@v));

}

#----------------------------------------------------------------------

# velEarthToInstrument() transforms earth to instrument coordinates

#	- based on manually inverted rotation matrix M (Sec 5.6 in coord-trans manual)

#		- Paul Wanis from TRDI pointed out that M is orthonormal, which

#		  implies that M^-1 = M' (where M' is the transpose), confirming

#		  the (unnecessary) derivation

#	- code was verified for both down- and uplookers

#	- missing heading data (IMP) causes undef beam velocities

#----------------------------------------------------------------------

{ # STATIC SCOPE

	my($hdg,$pitch,$roll,@E2I);

	sub velEarthToInstrument(@)

	{

		my($ADCP,$ens,$u,$v,$w,$ev) = @_;

		unless (@E2I &&

				$pitch == $ADCP->{ENSEMBLE}[$ens]->{PITCH} &&

				$roll  == $ADCP->{ENSEMBLE}[$ens]->{ROLL}) {

			$hdg = $ADCP->{ENSEMBLE}[$ens]->{HEADING} - $ADCP->{HEADING_BIAS} 

				if defined($ADCP->{ENSEMBLE}[$ens]->{HEADING});

			$pitch = $ADCP->{ENSEMBLE}[$ens]->{PITCH};

			$roll  = $ADCP->{ENSEMBLE}[$ens]->{ROLL};

			my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll)));

			my($useRoll) = ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}) ? $roll+180 : $roll;

			my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg)))

				if defined($hdg);				

			my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch));

			my($sr,$cr) = (sin(rad($useRoll)),	  cos(rad($useRoll)));

			@E2I = ([$ch*$cr+$sh*$sp*$sr,	 $ch*$sp*$sr-$sh*$cr,	-$cp*$sr],		# M^-1 = R^-1 * P^-1 * R^-1

				    [$sh*$cp,				 $ch*$cp,				$sp	],

				    [$ch*$sr-$sh*$sp*$cr,	-$sh*$sr-$ch*$sp*$cr,	$cp*$cr]);

		}

		return defined($ADCP->{ENSEMBLE}[$ens]->{HEADING})

			   ? ($u*$E2I[0][0]+$v*$E2I[0][1]+$w*$E2I[0][2],

				  $u*$E2I[1][0]+$v*$E2I[1][1]+$w*$E2I[1][2],

				  $u*$E2I[2][0]+$v*$E2I[2][1]+$w*$E2I[2][2],

				  $ev)

			   : (undef,undef,undef,undef);

	} # velEarthToIntrument()

} # STATIC SCOPE

#----------------------------------------------------------------------

# velInstrumentToBeam() transforms instrument to beam coordinates

#	- based on manually solved eq system in sec 5.3 of coord manual

#	- does not implement bin-remapping

#	- returns undef for 3-beam solutions, as it is not known which

#	  beam was bad

#----------------------------------------------------------------------

{ # STATIC SCOPE

	my($a,$b,$c,$d);

	sub velInstrumentToBeam(@)

	{

		my($ADCP,$ens,$x,$y,$z,$ev) = @_;

		return undef unless (defined($x) + defined($y) +

					   		 defined($z) + defined($ev) == 4);

		unless (defined($a)) {

			$a = 1 / (2 * sin(rad($ADCP->{BEAM_ANGLE})));

			$b = 1 / (4 * cos(rad($ADCP->{BEAM_ANGLE})));

			$c = $ADCP->{CONVEX_BEAM_PATTERN} ? 1 : -1;

			$d = $a / sqrt(2);

		}

		return ( $x/(2*$a*$c) + $z/(4*$b) + $ev/(4*$d),

				-$x/(2*$a*$c) + $z/(4*$b) + $ev/(4*$d),

				-$y/(2*$a*$c) + $z/(4*$b) - $ev/(4*$d),

				 $y/(2*$a*$c) + $z/(4*$b) - $ev/(4*$d));

	}

} # STATIC SCOPE

#----------------------------------------------------------------------

# velEarthToBeam() combines velEarthToInstrument and velInstrumentToBeam

#----------------------------------------------------------------------

sub velEarthToBeam(@)

{

	my($ADCP,$ens,$u,$v,$w,$ev) = @_;

	return velInstrumentToBeam($ADCP,$ens,

				velEarthToInstrument($ADCP,$ens,$u,$v,$w,$ev));

}

#----------------------------------------------------------------------

# velEarthToBPw() returns w12 and w34 for beam-coordinate data

#	- I am grateful for Paul Wanis from TRDI who corrected a

#	  bug in my transformation (fixed in V1.3). [The bug did not

#	  affect the final w profiles significantly, because w12 and w34

#	  are used only as diagnostics.]

#	- algorithm:

#		1) rotate into instrument coordinates

#		2) w12 = w + e*tan(beam_angle)/sqrt(2)

#		   w34 = w - e*tan(beam_angle)/sqrt(2)

#		3) rotate into horizontal coords (earth coords w/o

#		   considering heading, i.e. same as earth coords

#		   in case of w

#	- the commented-out version above is a "brute-force"

#	  implementation which should give the same result

#----------------------------------------------------------------------

#sub velEarthToBPw(@)

#{

#   my(@bpv) = velBeamToBPEarth(&velEarthToBeam(@_));

#   return ($bpv[1],$bpv[3]);

#}

sub velEarthToBPw(@)

{

	my($ADCP,$ens,$u,$v,$w,$ev) = @_;

	my(@iv) = velEarthToInstrument(@_);

	my(@iv12) = my(@iv34) = @iv;

	$iv12[2] += $iv[3] * tan(rad($ADCP->{BEAM_ANGLE}))/sqrt(2);

	$iv34[2] -= $iv[3] * tan(rad($ADCP->{BEAM_ANGLE}))/sqrt(2);

	my(@ev12) = velInstrumentToEarth($ADCP,$ens,@iv12);

	my(@ev34) = velInstrumentToEarth($ADCP,$ens,@iv34);

	return ($ev12[2],$ev34[2]);

}

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

# velBeamToBPEarth(@) calculates the vertical- and horizontal vels

# from the two beam pairs separately. Note that (w1+w2)/2 is 

# identical to the w estimated according to RDI (ignoring 3-beam 

# solutions).

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

{ # STATIC SCOPE

	my($TwoCosBAngle,$TwoSinBAngle);

	sub velBeamToBPEarth(@)

	{

		my($ADCP,$ens,$b1,$b2,$b3,$b4) = @_;

		my($v12,$w12,$v34,$w34);

		return (undef,undef,undef,undef) 

			unless (defined($ADCP->{ENSEMBLE}[$ens]->{PITCH}) &&

                    defined($ADCP->{ENSEMBLE}[$ens]->{ROLL}));

		unless (defined($TwoCosBAngle)) {

			$TwoCosBAngle = 2 * cos(rad($ADCP->{BEAM_ANGLE}));

			$TwoSinBAngle = 2 * sin(rad($ADCP->{BEAM_ANGLE}));

		}

		my($rad_roll)  = rad($ADCP->{ENSEMBLE}[$ens]->{ROLL});							

		my($sr) = sin($rad_roll); my($cr) = cos($rad_roll);

		my($rad_gimbal_pitch) = atan(tan(rad($ADCP->{ENSEMBLE}[$ens]->{PITCH})) * $cr);	# gimbal pitch

		my($rad_calc_pitch) = ($RDI_Coords::beamTransformation eq 'RDI') ? $rad_gimbal_pitch :

							  asin(sin($rad_gimbal_pitch)*cos($rad_roll) /

								   sqrt(1-sin($rad_roll)**2*sin($rad_gimbal_pitch)**2));

		my($sp) = sin($rad_calc_pitch); my($cp) = cos($rad_calc_pitch);

		# Sign convention:

		#	- refer to Coord manual Fig. 3

		#	- v12 is horizontal velocity from beam1 to beam2, i.e. westward for upward-looking ADCP

		#	  with beam 3 pointing north (heading = 0)

		if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_DOWN}) {					# beampair Earth coords

		}

		return ($v12,$w12,$v34,$w34);

	}

}

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

# velBeamToBPInstrument(@) calculates the instrument-coordinate vels

# from the two beam pairs separately.

#	- in spite of the function name, the output is in ship

#	  coordinates (instr coords with w up)

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

{ # STATIC SCOPE

	my($TwoCosBAngle,$TwoSinBAngle);

	sub velBeamToBPInstrument(@)

	{

		my($ADCP,$ens,$b1,$b2,$b3,$b4) = @_;

		my($v12,$w12,$v34,$w34);

		return (undef,undef,undef,undef) 

			unless (defined($ADCP->{ENSEMBLE}[$ens]->{PITCH}) &&

                    defined($ADCP->{ENSEMBLE}[$ens]->{ROLL}));

		unless (defined($TwoCosBAngle)) {

			$TwoCosBAngle = 2 * cos(rad($ADCP->{BEAM_ANGLE}));

			$TwoSinBAngle = 2 * sin(rad($ADCP->{BEAM_ANGLE}));

		}

		# Sign convention:

		#	- refer to Coord manual Fig. 3

		#	- v12 is horizontal velocity from beam1 to beam2

		#	- w is +ve upward, regardless of instrument orientation

		if (defined($b1) && defined($b2)) {

			$v12 = ($b1-$b2)/$TwoSinBAngle;

			$w12 = ($b1+$b2)/$TwoCosBAngle;

			$w12 *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP});

		}

		if (defined($b3) && defined($b4)) {

			$v34 = ($b4-$b3)/$TwoSinBAngle;

			$w34 = ($b3+$b4)/$TwoCosBAngle;

			$w34 *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP});

		}

		return ($v12,$w12,$v34,$w34);

	}

}

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

# velApplyHdgBias() applies the heading bias, which is used to correct

# for magnetic declination for data recorded in Earth-coordinates ONLY.

# Bias correction for beam-coordinate data is done in velInstrumentToEarth()

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

sub velApplyHdgBias(@)

{

	my($ADCP,$ens,$v1,$v2,$v3,$v4) = @_;

	return (undef,undef,undef,undef) 

		unless (defined($v1) && defined($v2) &&

				defined($ADCP->{ENSEMBLE}[$ens]->{HEADING}));

	my($sh) = sin(rad(-$ADCP->{HEADING_BIAS}));

	my($ch) = cos(rad(-$ADCP->{HEADING_BIAS}));

	return ( $v1*$ch + $v2*$sh,

			-$v1*$sh + $v2*$ch,

			 $v3			  ,

			 $v4			  );

}

#----------------------------------------------------------------------

# Pitch/Roll Functions

#----------------------------------------------------------------------

sub gimbal_pitch($$)	# RDI coord trans manual

{

	my($RDI_pitch,$RDI_roll) = @_;

	return 'nan' unless defined($RDI_pitch) && defined($RDI_roll);

	return deg(atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll))));

}

sub RDI_pitch($$)

{

	my($gimbal_pitch,$roll) = @_;