#======================================================================
#                    L I B A L E C . P L 
#                    doc: Wed Jun  1 20:38:19 2011
#                    dlm: Mon Dec 18 10:29:08 2017
#                    (c) 2011 A.M. Thurnherr
#                    uE-Info: 71 31 NIL 0 0 70 2 2 4 NIL ofnI
#======================================================================

# HISTORY:
#	Nov 15, 2017: - created
#	Dec 18, 2017: - turned vel output into m/s

require "$ANTS/libvec.pl";
require "$ANTS/libfuns.pl";

#----------------------------------------------------------------------
# User Routines
#	- from manual
#	- also transform velocities to m/s
#----------------------------------------------------------------------

sub ALEC_u($$$$$$$)
{
    my($vx,$vy,$alpha,$compA,$compB,$beta,$magdec) =
		&antsFunUsage(7,'fffffff','<vx[cm/s]> <vy[cm/s]> <alpha> <compA> <compB> <beta>',@_);

	my($c) = ALEC_vel_speed($vx,$vy,$alpha);
	my($d) = ALEC_vel_dir($compA,$compB,$beta,$vx,$vy);
	return vel_u($c,$d-$magdec);
}

sub ALEC_v($$$$$$$)
{
    my($vx,$vy,$alpha,$compA,$compB,$beta,$magdec) =
		&antsFunUsage(7,'fffffff','<vx[cm/s]> <vy[cm/s]> <alpha> <compA> <compB> <beta>',@_);

	my($c) = ALEC_vel_speed($vx,$vy,$alpha);
	my($d) = ALEC_vel_dir($compA,$compB,$beta,$vx,$vy);
	return vel_v($c,$d-$magdec);
}

#------------------------------------------------------------------------------
# Routines mostly as per ALEC manual
#		- weird atan corrections in manual cause data discontinuities
#			=> removed
#		- routines verified with first PITTA time series
# 	ALEC_vel_speed(vx,vy,alpha)
#	ALEC_heading(compA,compB,beta)
# 	ALEC_vel_instrument_dir(vx,vy)
# 	ALEC_vel_dir(compA,compB,beta,vx,vy)
#------------------------------------------------------------------------------

sub ALEC_vel_speed(@)
{
    my($vx,$vy,$alpha) = &antsFunUsage(3,'fff','<vx[cm/s]> <vy[cm/s]> <alpha>',@_);
	my($ssq) = ($vx**2 + $vy**2);
	return inf unless ($ssq > 1e-6);
	return (1 + $alpha*(4*$vx**2*$vy**2)/($ssq**2)) * sqrt($ssq) / 100;
}

sub ALEC_heading(@)
{
    my($compA,$compB,$beta) =
        &antsFunUsage(3,'fff','<compass output A> <compass output B> <beta>',@_);
    return angle_pos(deg(atan2($compA,$compB)) + $beta);
}

sub ALEC_vel_instrument_dir($$)
{
    my($vx,$vy) = &antsFunUsage(2,'ff','<vx[cm/s]> <vy[cm/s]>',@_);
	return deg(atan2($vx,$vy));
}

sub ALEC_vel_dir($$$$$)
{
    my($compA,$compB,$beta,$vx,$vy) =
        &antsFunUsage(5,'fffff','<compass output A> <compass output B> <beta> <vx[cm/s]> <vy[cm/s]>',@_);
    return ALEC_heading($compA,$compB,$beta) + ALEC_vel_instrument_dir($vx,$vy);
}

1;