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

#                    O L D 

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

#                    dlm: Mon Jul  1 15:15:19 2019

#                    (c) 2017 A.M. Thurnherr

#                    uE-Info: 698 42 NIL 0 0 70 2 2 4 NIL ofnI

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

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

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

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

}

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_milapsed = $min_elapsed unless defined($plot_milapsed);

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

	while (&antsIn()) {

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

		last if ($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;

			}

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

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

		pl_mag_calib_plot($valid,$magX,$magY)

			if defined($P{profile_id}) &&

				($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

#	- 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) = @_;

	my($RDI_max_tilt) = 29; 

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

	for (my($r)=0; $r<@ants_; $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) = @_;

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

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

}

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

# 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) = 1.05 * $dhist[$HDG_offset];

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

	}

	GMT_psbasemap('-Bg45a90f15:"IMU Heading Offset [\260]":/ga100f10:"Frequency":WeSn');

	GMT_unitcoords();

	GMT_pstext('-F+f14,Helvetica,CornFlowerBlue+jTR -N');

	printf(GMT "0.99 1.06 %g \260 offset (%d%% agreement)\n",angle($HDG_offset),100*$modefrac);

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

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

    GMT_end();

}

sub calc_hdg_offset($)

{

	my($verbose) = @_;

	print(STDERR "\n\tRe-calculating IMU pitch/roll anomalies") if $verbose;

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

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

		my($r) = int(($LADCP{ENSEMBLE}[$ens]->{ELAPSED_TIME} + $IMP{TIME_LAG} - $ants_[0][$elapsedF]) / $IMP{DT});

		if ($r < 0 && $ens == $LADCP_begin) {

			$r = int(($LADCP{ENSEMBLE}[++$ens]->{ELAPSED_TIME} + $IMP{TIME_LAG} - $ants_[0][$elapsedF]) / $IMP{DT})

				while ($r < 0);

			printf(STDERR "\n\tIMU data begin with instrument already in water => skipping %ds of LADCP data",

				$LADCP{ENSEMBLE}[$ens]->{ELAPSED_TIME}-$LADCP{ENSEMBLE}[$LADCP_begin]->{ELAPSED_TIME})

					if ($verbose);

			$LADCP_begin = $ens;

		}

		if ($r > $#ants_) {

			printf(STDERR "\n\tIMU data end while instrument is still in water => truncating %ds of LADCP data",

				$LADCP{ENSEMBLE}[$LADCP_end]->{ELAPSED_TIME}-$LADCP{ENSEMBLE}[$ens]->{ELAPSED_TIME})

					if ($verbose);

			$LADCP_end = $ens - 1;

			last;

		}

		next unless numberp($IMP{TILT_AZIMUTH}[$r]);

		$nPR++;

		$IMP_pitch_mean += $ants_[$r][$pitchF];

		$IMP_roll_mean	+= $ants_[$r][$rollF];

	}

	$IMP_pitch_mean /= $nPR;

	$IMP_roll_mean /= $nPR;

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

		my($r) = int(($LADCP{ENSEMBLE}[$ens]->{ELAPSED_TIME} + $IMP{TIME_LAG} - $ants_[0][$elapsedF]) / $IMP{DT});

		next unless numberp($IMP{TILT_AZIMUTH}[$r]);

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

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

												  $ants_[$r][$rollF] -$IMP_roll_mean);

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

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

													  $ants_[$r][$rollF] -$IMP_roll_mean);

	}

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

			if $verbose;

	if (defined($opt_s)) {

		$HDG_offset = $opt_s;

		printf(STDERR "\n\tHEADING_OFFSET = %.1f (set by -s)\n",$HDG_offset) if $verbose;

	} else {

		my($dhist_binsize,$dhist_min_pirom,$dhist_min_mfrac) = split(/,/,$opt_e);

		croak("$0: cannot decode -e $opt_e\n")

			unless ($dhist_binsize > 0 && $dhist_min_pirom > 0 && $dhist_min_mfrac >= 0);

		my(@dhist); my($nhist) = my($modeFreq) = 0;

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

			my($r) = int(($LADCP{ENSEMBLE}[$ens]->{ELAPSED_TIME} + $IMP{TIME_LAG} - $ants_[0][$elapsedF]) / $IMP{DT});

			next unless numberp($IMP{TILT_AZIMUTH}[$r]);

			next unless (abs($ants_[$r][$pitchF]-$IMP_pitch_mean) >= $dhist_min_pirom &&

						 abs($ants_[$r][$rollF] -$IMP_roll_mean) >= $dhist_min_pirom);

			$dhist[int(angle_pos($LADCP{ENSEMBLE}[$ens]->{TILT_AZIMUTH}-$IMP{TILT_AZIMUTH}[$r])/$dhist_binsize+0.5)]++;

			$nhist++;

		}

		croak("$0: empty histogram\n")

			unless ($nhist);

		$HDG_offset = 0;

		for (my($i)=1; $i<@dhist-1; $i++) { 							# make sure mode is not on edge

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

		}

		my($modefrac) = ($dhist[$HDG_offset]+$dhist[$HDG_offset-1]+$dhist[$HDG_offset+1]) / $nhist;

		$HDG_offset *= $dhist_binsize;

		pl_hdg_offset($dhist_binsize,$modefrac,@dhist);

		if ($opt_f) {

			printf(STDERR "\n\nIGNORED WARNING (-f): Cannot determine reliable heading offset; $HDG_offset+/-$dhist_binsize deg accounts for only %f%% of total\n",$modefrac*100)

				if ($modefrac < $dhist_min_mfrac);

		} else {

			croak(sprintf("\n$0: Cannot determine reliable heading offset; $HDG_offset+/-$dhist_binsize deg accounts for only %f%% of total\n",$modefrac*100))

				if ($modefrac < $dhist_min_mfrac);

		}

		printf(STDERR "\n\t") if $verbose;

		printf(STDERR "IMU heading offset = %g deg (%d%% agreement)\n",angle($HDG_offset),100*$modefrac) if $verbose;

	}

	return ($HDG_offset,$IMP_pitch_mean,$IMP_roll_mean);

}

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

# rot_IMP()

# 	- rotate IMP Data Into LADCP Instrument Coords

#	- also replaced pitch/roll by corresponding anomalies!!!

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

sub rot_IMP($)

{

	my($verbose) = @_;

	my($crho) = cos(rad($HDG_offset));

	my($srho) = sin(rad($HDG_offset));

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

		my($r) = int(($LADCP{ENSEMBLE}[$ens]->{ELAPSED_TIME} + $IMP{TIME_LAG} - $ants_[0][$elapsedF]) / $IMP{DT});

		if (numbersp($ants_[$r][$pitchF],$ants_[$r][$rollF])) { 				# pitch/roll

			my($rot_p) = (($ants_[$r][$pitchF]-$IMP_pitch_mean)*$crho +

						  ($ants_[$r][$rollF]-$IMP_roll_mean)*$srho);

			my($rot_r) = (-($ants_[$r][$pitchF]-$IMP_pitch_mean)*$srho +

						   ($ants_[$r][$rollF]-$IMP_roll_mean)*$crho);

			$ants_[$r][$pitchF] = $rot_p;

			$ants_[$r][$rollF]	= $rot_r;

		}

		$ants_[$r][$hdgF] = angle_pos($ants_[$r][$hdgF] - $HDG_offset)

			if numberp($ants_[$r][$hdgF]);

	}

	my($rot_p) =  $IMP_pitch_mean * $crho + $IMP_roll_mean * $srho; 			# mean pitch roll

	my($rot_r) = -$IMP_pitch_mean * $srho + $IMP_roll_mean * $crho;

	$IMP_pitch_mean = $rot_p;

	$IMP_roll_mean	= $rot_r;

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

	return ($IMP_pitch_mean,$IMP_roll_mean);