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

#                    L I B I M P . P L 

#                    doc: Tue Nov 26 21:59:40 2013

#                    (c) 2017 A.M. Thurnherr

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

# HISTORY:

#	Nov 26, 2013: - created

#	Dec  1, 2013: - removed HDG_ROT

#				  - added support for IMP data gaps

#	Mar  4, 2014: - added support for DATA_SOURCE_ID

#	May 22, 2014: - use +/-2deg to assess quality of heading offset

#	May 23, 2014: - added $dhist_binsize, $dhist_min_pirom

#	Jul 27, 2016: - updated variable names for consistency

#	Jul 28, 2016: - major re-write if merging routines

#	Jul 29, 2016: - cosmetics

#				  - increased heading-offset resolution from 2 to 1 degrees

#				  - BUG: inconsistent heading definition used (from old IMP with

#						 confused coordinates)

#				  - modified initial timelag guess (there was a bug and it is 

#				    likely more robust based on end time rather than start time)

#	Aug  5, 2016: - BUG: weird statement accessing LADCP_begin-1

#				  - BUG: DSID of first ensemble was not left original

#	Aug 22, 2016: - major changes to timelagging

#	Aug 23, 2016: - changed semantics for removing ensembles with bad attitudes:

#					instead of setting attitude to undef (or large pitch/roll),

#					clearEns() is used

#	Aug 24, 2016: - overhauled time-lagging

#	Aug 25, 2016: - significant code cleanup

#	Aug 26, 2016: - added _hdg_err.ps output plot

#	Oct 13, 2016: - made hdg nan for invalid records (BUG with current versions of IMP+LADCP, IMPatchPD0)

#	Nov 22, 2016: - added heading-offset plot

#				  - added sensor info to plots

#	Nov 29, 2016: - added stats to compass error plot

#	Dec 29, 2016: - improved histogram plot

#	Nov 16, 2017: - adapted rot_vecs() to KVM coordinates

#				  - made sensor information optional in

#	Nov 20, 2017: - major code cleanup

#	Nov 22, 2017: - replaced "IMP" output in routines used by KVH by "IMU"

#	Dec  8, 2017: - replaced remaing "IMP" output (e.g. in plot) by "IMU"

#	May 22, 2018: - added data trimming to rot_vec

#	May 23, 2018: - added horizontal field strength to mag calib plot

#				  - added support for S/N 8 board inside UL WH300 (neg_piro == 2)

#	May 24, 2018: - continued working (coord_trans == 2)

#	May 25, 2018: - continued working

#	May 30, 2018: - BUG: trimming did not re-calculate elapsed time

#	Jun  5, 2018: - BUG: on -c main magnetometer and accelerometer vectors were

#						 modified twice

#	Jun  7, 2018: - relaxed definition of -e

#	Jun  9, 2018: - BUG: some "edge" data (beyond the valid profile) were bogus (does

#						 not matter in practice)

#	Jun 30, 2019: - renamed -s in IMP+LADCP to -o (option not used in KVH+LADCP)

#				  - changed semantics so that offset histogram is plotted even on -o

#				  - made create_merge_plots return errors

#	Jul  1, 2019: - BUG: pre-deployment output cleared IMU pitch/roll fields (1 record

#						 affected)

#				  - modified output of pre-/post-cast records to include all LADCP

#				    but no IMU data

#				  - BUG: GIMBAL_PITCH had not only been defined for in-water records

#	Jun  9, 2018: - BUG: some "edge" data (beyond the valid profile) were bogus (does not matter in practice)

#	Apr 12, 2020: - modified rot_vecs to pass all records with non-numerical elapsed 

#	Apr 13, 2020: - added prep_piro_ADCP

#				  - BUG: dhist_binsize != 1 did not work

#				  - BUG: dhist agreement % was not rounded

#	Apr 14, 2020: - cosmetics

#	Jul 12, 2021: - improvements to histogram plot

#	Jul 24, 2021: - updated docu

#	Jul  6, 2022: - added support for $suppress_rot_acc_output, @copyFields

#	Jul 14, 2022: - BUG: unshift used instead of push

#				  - BUG: acc was rotated regardless of $suppress_rot_acc_output

#	Sep 12, 2022: - improved error message formatting on verbose

# HISTORY END

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

# gRef() library

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

sub pl_mag_calib_begin($$$)															# initialize mag_calib plot

{

	my($pfn,$plotsize,$axlim) = @_;

	my($xmin,$xmax) = (-$axlim,$axlim);

	my($ymin,$ymax) = (-$axlim,$axlim);

	GMT_begin($pfn,"-JX${plotsize}","-R$xmin/$xmax/$ymin/$ymax",'-X6 -Y4 -P');

	GMT_psxy('-Sc0.05');

}

sub pl_mag_calib_plot($$$)															# plot data point

{

	my($valid,$magX,$magY) = @_;

	if ($valid)	{ print(GMT "> -Wred -Gred\n$magX $magY\n"); }

	else 		{ print(GMT "> -Wgreen -Ggreen\n$magX $magY\n"); }

}

sub pl_mag_calib_end($$)															# finish mag_calib plot

{

	my($axlim,$HF_mag,$sensor_info) = @_;

	GMT_psxy('-Sc0.1 -Gblue');														# calibration circle

	for (my($a)=0; $a<2*$pi; $a+=0.075) {

		printf(GMT "%f %f\n",$HF_mag*sin($a),$HF_mag*cos($a));

	}

	if ($axlim <= 0.1) {															# axes labels

		GMT_psbasemap('-Bg1a.04f.001:"X Magnetic Field [Gauss]":/g1a0.02f0.001:"Y Magnetic Field [Gauss]":WeSn');

	} else {

		GMT_psbasemap('-Bg1a.1f.01:"X Magnetic Field [Gauss]":/g1a0.1f0.01:"Y Magnetic Field [Gauss]":WeSn');

	}

    GMT_unitcoords();																# horizontal field strength

    GMT_pstext('-F+f12,Helvetica,blue+jTR -N');

   	printf(GMT "0.98 0.98 HF = %.2f Gauss\n",$HF_mag);

   	printf(GMT "0.98 0.94 $sensor_info\n")											# sensor info

		if ($sensor_info ne '');

    GMT_pstext('-F+f14,Helvetica,blue+jTL -N');										# profile id

    printf(GMT "0.01 1.06 $P{profile_id}\n");

	GMT_end();

}

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

# global variables used:

#	@vecs						field number triplets of vector data

#	@piro						[chip id, pitch field, roll field] 

#	@bias						bias triples for vector data

#	$suppress_rot_acc_output	set to true to suppress output of rotated accelerometer data

#	@copyFields					field numbers to copy from input to output

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

sub rot_vecs(@) 																	# rotate & output IMU vector data 

{

	my($coord_trans,$min_elapsed,$max_elapsed,$plot_milapsed,$plot_malapsed) = @_;		# negate KVH pitch/roll data if first arg set to 1

	$min_elapsed = 0 unless defined($min_elapsed);

	$max_elapsed = 9e99 unless defined($max_elapsed);

	$plot_minlapsed = $min_elapsed unless defined($plot_milapsed);

	$plot_malapsed = $max_elapsed unless defined($plot_malapsed);

	while (&antsIn()) {

		next if numberp($ants_[0][$elapsedF]) && ($ants_[0][$elapsedF] < $min_elapsed);	# trim data

		last if numberp($ants_[0][$elapsedF]) && ($ants_[0][$elapsedF] > $max_elapsed);

		my($cpiro) = -1;															# current pitch/roll accelerometer

		my(@R); 																	# rotation matrix

		for (my($i)=0; $i<@vecs; $i++) {											# rotate vector data

			if ($piro[$i][0] != $cpiro) {											# next sensor chip

				$cpiro = $piro[$i][0];

				my($accX) = $ants_[0][$vecs[$cpiro][0]];

				my($accY) = $ants_[0][$vecs[$cpiro][1]];

				my($accZ) = $ants_[0][$vecs[$cpiro][2]];

				if ($coord_trans == 2) {											# S/N 8 inside UL WH300

					$accY *= -1; $accZ *= -1;

				}

				my($roll)  = atan2($accY,$accZ); 									# eqn 25 from Freescale AN3461

				my($pitch) = atan2($accX,sqrt($accY**2+$accZ**2));     				# eqn 26 from <Freescale AN3461

				if ($coord_trans == 1) {											# KVH

					$pitch *= -1;

					$roll  *= -1;

                }

				$ants_[0][$piro[$i][1]] = deg($pitch);								# add pitch/roll to data

				$ants_[0][$piro[$i][2]] = deg($roll);

				my($sp) = sin($pitch); my($cp) = cos($pitch);						# define rotation matrix

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

				@R = ([ $cp,	 0,   -$sp	  ],

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

					  [ $sp*$cr, $sr,  $cp*$cr]);

			}

			my($xval) = $ants_[0][$vecs[$i][0]];

			my($yval) = $ants_[0][$vecs[$i][1]];

			my($zval) = $ants_[0][$vecs[$i][2]];

			if ($coord_trans == 2) {												# S/N 8 inside UL WH300

				$yval *= -1; $zval *= -1;

			}

			next if ($suppress_rot_acc_output && $i==$cpiro);

			$ants_[0][$vecs[$i][0]] = ($xval-$bias[$i][0]) * $R[0][0] +

									  ($yval-$bias[$i][1]) * $R[0][1] +

									  ($zval-$bias[$i][2]) * $R[0][2];

			$ants_[0][$vecs[$i][1]] = ($xval-$bias[$i][0]) * $R[1][0] +

			  						  ($yval-$bias[$i][1]) * $R[1][1] +

									  ($zval-$bias[$i][2]) * $R[1][2];

			$ants_[0][$vecs[$i][2]] = ($xval-$bias[$i][0]) * $R[2][0] +

									  ($yval-$bias[$i][1]) * $R[2][1] +

									  ($zval-$bias[$i][2]) * $R[2][2];

		}

		my($magX) = $ants_[0][$magXF];

		my($magY) = $ants_[0][$magYF];

		my($magZ) = $ants_[0][$magZF];

		my($accX) = $ants_[0][$accXF];

		my($accY) = $ants_[0][$accYF];

		my($accZ) = $ants_[0][$accZF];

		my($HF)   = sqrt($magX**2+$magY**2);

		my($valid)= ($HF >= $minfac*$HF_mag) && ($HF <= $maxfac*$HF_mag);

		my($hdg)  = $valid ? mag_heading($magX,$magY) : nan;

		@cpFields = ();

		foreach my $fnr (@copyFields) {

			push(@cpFields,$ants_[0][$fnr]);

        }

		if ($suppress_rot_acc_output) {

			&antsOut($ants_[0][$elapsedF]-$min_elapsed,$ants_[0][$tempF],

					 RDI_pitch($ants_[0][$pitchF],$ants_[0][$rollF]),$ants_[0][$rollF],

					 $hdg,$magX,$magY,$magZ,

	                 vel_u($HF,$hdg),vel_v($HF,$hdg),$valid,@cpFields);

        } else {

			&antsOut($ants_[0][$elapsedF]-$min_elapsed,$ants_[0][$tempF],

					 RDI_pitch($ants_[0][$pitchF],$ants_[0][$rollF]),$ants_[0][$rollF],

					 $hdg,$accX,$accY,$accZ,$magX,$magY,$magZ,

	                 vel_u($HF,$hdg),vel_v($HF,$hdg),$valid,@cpFields);

        }

		pl_mag_calib_plot($valid,$magX,$magY)

			if defined($P{profile_id}) &&

				(!numberp($ants_[0][$elapsedF]) ||

					($ants_[0][$elapsedF] >= $plot_milapsed) &&

					($ants_[0][$elapsedF] <= $plot_malapsed));

	}

}

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

# LADCP merging library

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

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

# Instrument Offset Estimation

#

#	1: resolution of histogram in deg

#		1 deg okay for good sensors

#		2 deg sometimes required (2016 P18 003 2nd sensor)

#	2: min tilt anom to consider for offset estimation

#		0.3 deg works even for calm casts

#		increased values improve histogram

#		2.0 deg is too high for quiet casts (2016 P18 003)

#	3: minimum fraction of hist mode required

#		10% default

#		decreasing histogram resolution is better than

#			decreasing this value, I think

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

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

# trim_out_of_water()

#	- attempt to remove out-of-water records from time-series of

#	  horizontal acceleration

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

sub trim_out_of_water($)

{

	my($verbose) = @_;

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

	# first-difference horizontal acceleration at full resolution to pass-filter

	# dangling motion

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

	$IMP{Ah}[0] 	= sqrt($ants_[0][$accXF]**2+$ants_[0][$accYF]**2);

	$IMP{dAhdt}[0]	= nan;

	for (my($r)=1; $r<@ants_; $r++) {

		$IMP{Ah}[$r] = sqrt($ants_[$r][$accXF]**2+$ants_[$r][$accYF]**2);

		$IMP{dAhdt}[$r] = ($IMP{Ah}[$r]-$IMP{Ah}[$r-1]) / ($ants_[$r][$elapsedF]-$ants_[$r-1][$elapsedF]);

	}

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

	# create 10-s binned time series to calculate rms values of this quantity (dAhdt), and

	# scale this with cos(sqrt($$pitch**2+$$roll**2)) to dampen underwater peaks (when the

	# instrument has a large tilt because it is being dragged)

	#	NB: dAhdt, pitch and roll are only set up to last full bin (not so, sum and n)

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

	my(@dAhdt,@pitch,@roll,@dAhdt_rms);

	my(@sum) = my(@sume) = my(@sump) = my(@sumr) = my(@n) = (0);

	my($bin_start) = $ants_[0][$elapsedF];

	for (my($r)=1; $r<@ants_; $r++) {   

		if ($ants_[$r][$elapsedF] - $bin_start <= 10) { 						# within 10-s bin

			$sum[$#sum] += $IMP{dAhdt}[$r]; 									# sums

			$sume[$#sume] += $ants_[$r][$elapsedF];

			$sump[$#sump] += $ants_[$r][$pitchF];

			$sumr[$#sumr] += $ants_[$r][$rollF];

			$n[$#n]++;

			next;

		}

		$dAhdt[$#sum] = $sum[$#sum] / $n[$#n];								# bin done => means

		$elapsed[$#sum] = $sume[$#sume] / $n[$#n];

		$pitch[$#sum] = $sump[$#sump] / $n[$#n];

		$roll[$#sum] = $sumr[$#sumr] / $n[$#n];

		my($sumsq) = 0; 													# sum of squares for rms(accel)

		for (my($rr)=$r-$n[$#n]; $rr<$r; $rr++) {

			$sumsq += ($IMP{dAhdt}[$rr] - $dAhdt[$#sum])**2;

		}

		$dAhdt_rms[$#sum] = sqrt($sumsq / $n[$#n]);

		push(@sum,$IMP{dAhdt}[$r]); 										# begin next bin

		push(@sume,$ants_[$r][$elapsedF]);

		push(@sump,$ants_[$r][$pitchF]);

		push(@sumr,$ants_[$r][$rollF]);

		push(@n,1);

		$bin_start = $ants_[$r][$elapsedF];

	}

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

	# trim beginning/end when IMP is out of water

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

	my($i,$si);

	for ($i=int(@dAhdt_rms/2); $i>0; $i--) {

		last if ($dAhdt_rms[$i] * cos(rad(sqrt($pitch[$i]**2+$roll[$i]**2))) > 1.0);

	}

	if ($dAhdt_rms[$i] * cos(rad(sqrt($pitch[$i]**2+$roll[$i]**2))) > 1.0) {

		for ($si=0; $ants_[$si][$elapsedF]<=$elapsed[$i]; $si++) {}

		splice(@ants_,0,$si);

		printf(STDERR "\n\t\t%5d  leading out-of-water IMU records removed",$si)

			if ($si>0 && $verbose);

	} else {

		print(STDERR "\n\t\tWARNING: no leading out-of-water IMU records detected/removed") if $verbose;

	}

	for ($i=int(@dAhdt_rms/2); $i<@dAhdt_rms; $i++) {

		last if ($dAhdt_rms[$i] * cos(rad(sqrt($pitch[$i]**2+$roll[$i]**2))) > 1.0);

	}

	if ($dAhdt_rms[$i] * cos(rad(sqrt($pitch[$i]**2+$roll[$i]**2))) > 1.0) {

		for ($si=$#ants_; $ants_[$si][$elapsedF]>=$elapsed[$i]; $si--) {}

		my($rem) = @ants_ - $si;

		splice(@ants_,$si);

		printf(STDERR "\n\t\t%5d trailing out-of-water IMU records removed",$rem)

			if ($rem>0 && $verbose);

	} else {

		print(STDERR "\n\t\tWARNING: no trailing out-of-water IMU records detected/removed") if $verbose;

	}

	printf(STDERR "\n\t\tcast duration		  : %.1f min",

		($ants_[$#ants_][$elapsedF] - $ants_[0][$elapsedF]) / 60)

	        if $verbose;

}

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

# prep_piro_IMP()

# 	- calculate pitch/roll offsets & tilt azimuth for IMP

#	- if first_rec and last_rec are provided, the mean calculation will 

#	  be restricted to this range (TILT_AZIM, _ANOM calculated for all recs)

#	- during an attempt to improve time lagging for the 2015 IOPAS data set,

#	  it was noticed that one particular instrument, WHM300#12973 maxes out

#	  pitch at 27.36 degrees, whereas the roll may not be maxed out at 28.76 deg,

#	  the max observed during the cruise.

#	- therefore, IMP{TILT_AZIM} and IMP{TILT_ANOM} are calculated here, first,

#	  with a pitch/roll cutoff value of 29 degrees

#	- after the time lagging, when the LADCP start and end times are known,

#	  the TILT values are re-calculated without the pitch/roll limit, and

#	  using only the correct time range

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

sub prep_piro_IMP($@)

{

	my($verbose,$first_rec,$last_rec) = @_;

	my($RDI_max_tilt) = 29; 

	my($IMP_pitch_mean,$IMP_roll_mean,$nPR) = (0,0,0);

	$first_rec = 0 			unless defined($first_rec);

	$last_rec  = $#ants_	unless defined($last_rec);

	for (my($r)=$first_rec; $r<=$last_rec; $r++) {

		next unless numbersp($ants_[$r][$pitchF],$ants_[$r][$rollF]);

		$nPR++;

		$IMP_pitch_mean += min($ants_[$r][$pitchF],$RDI_max_tilt);

		$IMP_roll_mean	+= min($ants_[$r][$rollF],$RDI_max_tilt);

	}

	$IMP_pitch_mean /= $nPR;

	$IMP_roll_mean /= $nPR;

	printf(STDERR "\n\t\tIMU mean pitch/roll	  : %.1f/%.1f deg",$IMP_pitch_mean,$IMP_roll_mean)

			if $verbose;

	for (my($r)=0; $r<@ants_; $r++) {

		next unless numbersp($ants_[$r][$pitchF],$ants_[$r][$rollF]);

		$IMP{TILT_AZIMUTH}[$r] = tilt_azimuth(min($ants_[$r][$pitchF],$RDI_max_tilt)-$IMP_pitch_mean,

											  min($ants_[$r][$rollF],$RDI_max_tilt) -$IMP_roll_mean);

		$IMP{TILT_ANOM}[$r] = angle_from_vertical(min($ants_[$r][$pitchF],$RDI_max_tilt)-$IMP_pitch_mean,

												  min($ants_[$r][$rollF],$RDI_max_tilt) -$IMP_roll_mean);

	}

	return ($IMP_pitch_mean,$IMP_roll_mean);

}

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

# prep_piro_LADCP()

#	- calculate pitch/roll offsets & tilt azimuth of LADCP

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

sub prep_piro_LADCP($)

{

	my($verbose) = @_;

	$first_ens = 0 			unless defined($first_ens);

	$last_ens  = $#ants_	unless defined($last_ens);

	my($LADCP_pitch_mean,$LADCP_roll_mean) = (0,0);

	for (my($ens)=0; $ens<=$#{$LADCP{ENSEMBLE}}; $ens++) {

		$LADCP{ENSEMBLE}[$ens]->{GIMBAL_PITCH} =

			gimbal_pitch($LADCP{ENSEMBLE}[$ens]->{PITCH},$LADCP{ENSEMBLE}[$ens]->{ROLL});

	}

	for (my($ens)=$LADCP_begin; $ens<=$LADCP_end; $ens++) {

		$LADCP{ENSEMBLE}[$ens]->{GIMBAL_PITCH} =

			gimbal_pitch($LADCP{ENSEMBLE}[$ens]->{PITCH},$LADCP{ENSEMBLE}[$ens]->{ROLL});

		$LADCP_pitch_mean += $LADCP{ENSEMBLE}[$ens]->{GIMBAL_PITCH};

		$LADCP_roll_mean  += $LADCP{ENSEMBLE}[$ens]->{ROLL};

	}

	$LADCP_pitch_mean /= ($LADCP_end-$LADCP_begin+1);

	$LADCP_roll_mean  /= ($LADCP_end-$LADCP_begin+1);

	printf(STDERR "\n\t\tLADCP mean pitch/roll	  : %.1f/%.1f deg",$LADCP_pitch_mean,$LADCP_roll_mean)

			if $verbose;

	for (my($ens)=$LADCP_begin; $ens<=$LADCP_end; $ens++) {

		$LADCP{ENSEMBLE}[$ens]->{TILT_AZIMUTH} =

			tilt_azimuth($LADCP{ENSEMBLE}[$ens]->{GIMBAL_PITCH}-$LADCP_pitch_mean,

						 $LADCP{ENSEMBLE}[$ens]->{ROLL}-$LADCP_roll_mean);

		$LADCP{ENSEMBLE}[$ens]->{TILT_ANOM} =

			angle_from_vertical($LADCP{ENSEMBLE}[$ens]->{GIMBAL_PITCH}-$LADCP_pitch_mean,

								$LADCP{ENSEMBLE}[$ens]->{ROLL}-$LADCP_roll_mean);

	}

	print(STDERR "\n") if $verbose;

	return ($LADCP_pitch_mean,$LADCP_roll_mean);

}

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

# prep_piro_ADCP()

#	- calculate pitch/roll offsets & tilt azimuth of moored ADCP

#	- very similar to prep_piro_LADCP() but using windowing as in

#	  prep_piro_IMP()

#	- window ens are indices, not ensemble numbers

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

sub prep_piro_ADCP($@)

{

	my($verbose,$window_first_ens,$window_last_ens) = @_;

	$window_first_ens = 0 					unless defined($window_first_ens);

	$window_last_ens  = $#{$ADCP{ENSEMBLE}}	unless defined($window_last_ens);

	for (my($ens)=0; $ens<=$#{$ADCP{ENSEMBLE}}; $ens++) {

		$ADCP{ENSEMBLE}[$ens]->{GIMBAL_PITCH} =

			gimbal_pitch($ADCP{ENSEMBLE}[$ens]->{PITCH},$ADCP{ENSEMBLE}[$ens]->{ROLL});

	}

	my($ADCP_pitch_mean,$ADCP_roll_mean) = (0,0);

	for (my($ens)=$window_first_ens; $ens<=$window_last_ens; $ens++) {

		$ADCP_pitch_mean += $ADCP{ENSEMBLE}[$ens]->{GIMBAL_PITCH};

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

	}

	$ADCP_pitch_mean /= ($window_last_ens-$window_first_ens+1);

	$ADCP_roll_mean  /= ($window_last_ens-$window_first_ens+1);

	printf(STDERR "\n\t\tADCP mean pitch/roll       : %.1f/%.1f deg",$ADCP_pitch_mean,$ADCP_roll_mean)

			if $verbose;

	for (my($ens)=0; $ens<=$#{$ADCP{ENSEMBLE}}; $ens++) {

		$ADCP{ENSEMBLE}[$ens]->{TILT_AZIMUTH} =

			tilt_azimuth($ADCP{ENSEMBLE}[$ens]->{GIMBAL_PITCH}-$ADCP_pitch_mean,

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

		$ADCP{ENSEMBLE}[$ens]->{TILT_ANOM} =

			angle_from_vertical($ADCP{ENSEMBLE}[$ens]->{GIMBAL_PITCH}-$ADCP_pitch_mean,

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

	}

	print(STDERR "\n") if $verbose;

	return ($ADCP_pitch_mean,$ADCP_roll_mean);

}

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

# sub calc_hdg_offset()

#	- estimate heading offset from tilt time series

#	- returns heading offset and updated IMP mean tilts

#	- also creates diagnostic plot with pl_hdg_offset()

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

sub pl_hdg_offset($@)

{

	my($dhist_binsize,$modefrac,@dhist) = @_;

	my($plotsize) = '13c';

	my($xmin,$xmax) = (-180.5,180.5);

	my($ymin) = 0;

	my($ymax) = 0;

	for (my($i)=0; $i<@dhist; $i++) {

		$ymax = $dhist[$i] if ($dhist[$i] > $ymax);

    }

	$ymax = 1.05 * $ymax;

	GMT_begin("$P{profile_id}${opt_a}_hdg_offset.ps","-JX${plotsize}","-R$xmin/$xmax/$ymin/$ymax",'-X6 -Y4 -P');

	GMT_psxy("-Sb${dhist_binsize}u -GCornFlowerBlue");

	for (my($i)=0; $i<@dhist; $i++) {

		next unless $dhist[$i];

		printf(GMT "%f $dhist[$i]\n",$i*$dhist_binsize>180 ? $i*$dhist_binsize-360 : $i*$dhist_binsize);

	}