/** \file
 *
 *  Contains the LBLNavigation class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "LBLNavigation.h"
#include "LBLNavigationIF.h"

#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "utils/AuvMath.h"
#include "utils/Timestamp.h"

#include <cerrno>
#include <cstddef>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>

// These are static to simplfy unit testing of this essentially singleton component
LBLNavigation::TimeAndDistance LBLNavigation::PingFilter_[LBLNavigationIF::NUM_PINGS][LBLNavigationIF::MAX_PING_FILTER_SIZE + 1];
int LBLNavigation::PingFilterPos_[LBLNavigationIF::NUM_PINGS];
LBLNavigation::TimeAndPoisition LBLNavigation::FixFilter_[LBLNavigationIF::MAX_FIX_FILTER_SIZE];
int LBLNavigation::FixFilterPos_( -1 );

LBLNavigation::LBLNavigation( const Module* module )
// Note: using SyncComponent rather than SyncNavigationComponent so we can set the CycleOrder to be the last Navigation component
    : SyncComponent( LBLNavigationIF::NAME, module, SyncComponent::CycleOrder( ( int )SyncComponent::CYCLE_NAVIGATION + SyncComponent::CYCLE_INCREMENT - 1 ) ),
      verbosityCfg_( 0 ),
      navigationErrorCfg_( 0.0001 ),
      baselineLockoutCfg_( 0.25 * M_PI / 180.0 ),
      advancePingCfg_( 0 ),
      advancePositionCfg_( 0.25 ),
      maxPositionChangeCfg_( 3 ),
      maxSpeedWrtTransponderCfg_( 1.5 ),
      pingFilterSizeCfg_( 5 ),
      lastPingFilterSizeCfg_( 0 ),
      maxPingAgeInFilterCfg_( 30 ),
      maxPingAgeCfg_( 5 ),
      fixFilterSizeCfg_( 10 ),
      lastFixFilterSizeCfg_( 0 ),
      fixHalfLifeInFilterCfg_( 20 ),
      soundSpeed_( 1500 ),
      lastPingLocation_(),
      lastPingDistance_( nanf( "" ) ),
      lastPingTimestamp_( nanf( "" ) ),
      lastPingChannel_( -1 )
{

    // Config inputs
    verbosityConfigReader_ = newConfigReader( LBLNavigationIF::VERBOSITY );
    soundSpeedConfigReader_ = newConfigReader( LBLNavigationIF::SPEED_OF_SOUND );
    navigationErrorConfigReader_ = newConfigReader( LBLNavigationIF::NAVIGATION_ERROR );
    baselineLockoutConfigReader_ = newConfigReader( LBLNavigationIF::BASELINE_LOCKOUT );
    advancePingConfigReader_ = newConfigReader( LBLNavigationIF::ADVANCE_PING );
    advancePositionConfigReader_ = newConfigReader( LBLNavigationIF::ADVANCE_POSITION );
    maxPositionChangeConfigReader_ = newConfigReader( LBLNavigationIF::MAX_POSITION_CHANGE );
    maxSpeedWrtTransponderConfigReader_ = newConfigReader( LBLNavigationIF::MAX_SPEED_WRT_TRANSPONDER );
    pingFilterSizeConfigReader_ = newConfigReader( LBLNavigationIF::PING_FILTER_SIZE );
    maxPingAgeInFilterConfigReader_ = newConfigReader( LBLNavigationIF::MAX_PING_AGE_IN_FILTER );
    maxPingAgeConfigReader_ = newConfigReader( LBLNavigationIF::MAX_PING_AGE );
    fixFilterSizeConfigReader_ = newConfigReader( LBLNavigationIF::FIX_FILTER_SIZE );
    fixHalfLifeInFilterConfigReader_ = newConfigReader( LBLNavigationIF::FIX_HALF_LIFE_IN_FILTER );

    // Slate input measurements

    /// Current platform latitude
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );

    /// Current platform longitude
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );

    /// Current platform depth
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );

    /// Current platform speed (from DR)
    speedReader_ = newUniversalReader( UniversalURI::PLATFORM_SPEED_WRT_GROUND );

    /// Current platform course (from DR)
    courseReader_ = newUniversalReader( UniversalURI::PLATFORM_COURSE );

    /// Current speed of sound in water
    soundSpeedReader_ = newUniversalReader( UniversalURI::SPEED_OF_SOUND_IN_SEA_WATER );

    for( int iPing = 0; iPing < LBLNavigationIF::NUM_PINGS; ++iPing )
    {
        /// Ping location (origin)
        pingPositionReaders_[iPing] = newBlobReader( *LBLNavigationIF::PING_POSITIONS[iPing] );
        /// Ping distance
        pingTOFReaders_[iPing] = newDataReader( *LBLNavigationIF::PING_TIME_OF_FLIGHTS[iPing] );
        /// Ping location storage
        pingPositions_[iPing] = Point3D();
    }

    ResetPingFilters( verbosityCfg_, logger_ );
    ResetFixFilters( verbosityCfg_, logger_ );

    // Slate output variables
    latitudeWriter_ = newUniversalWriter( UniversalURI::LATITUDE_FIX );
    longitudeWriter_ = newUniversalWriter( UniversalURI::LONGITUDE_FIX );

}

LBLNavigation::~LBLNavigation()
{
}

void LBLNavigation::initialize( void )
{
    run();
}

void LBLNavigation::uninitialize( void )
{
    uninitializeLastPing();
}

void LBLNavigation::run( void )
{
    // Look for recent pings position updates -- if any, deal with them.
    for( int iPing = 0; iPing < LBLNavigationIF::NUM_PINGS; ++iPing )
    {

        if( pingPositionReaders_[iPing]->wasTouchedSinceLastRun( this ) )
        {
            if( !pingPositionReaders_[iPing]->read1DClass( Units::NONE, pingPositions_[iPing] ) )
            {
                logger_.syslog( "Err reading position" + Str( iPing + 1, 10 ), Syslog::ERROR );
            }
            else
            {
                if( verbosityCfg_ > 0 )
                {
                    logger_.syslog( "Set position " + Str( iPing + 1, 10 ) + " to " + pingPositions_[iPing].toString(), Syslog::INFO );
                }
            }
        }
    }

    // Get our current position
    double latitude( nanf( "" ) ), longitude( nanf( "" ) ), depth( nanf( "" ) );
    if( !latitudeReader_->read( Units::RADIAN, latitude ) ||
            !longitudeReader_->read( Units::RADIAN, longitude ) ||
            !depthReader_->read( Units::METER, depth ) )
    {
        // The algorithm assumes we have some guess of where we are
        return;
    }

    // If we can't calculate things, bail now
    if( isnan( latitude ) || isnan( longitude ) || isnan( depth ) )
    {
        return;
    }
    Location currentLocation( Units::RADIAN, latitude, longitude ), location( currentLocation );
    bool wroteLocation = false;

    // Get speed and course from DR
    double speed( nanf( "" ) ), course( nanf( "" ) );
    speedReader_->read( Units::METER_PER_SECOND, speed );
    courseReader_->read( Units::METER_PER_SECOND, course );

    // Read configuation
    verbosityConfigReader_->read( Units::ENUM, verbosityCfg_ );
    if( !( soundSpeedReader_->read( Units::METER_PER_SECOND, soundSpeed_ ) && !isnan( soundSpeed_ ) )
            && !soundSpeedConfigReader_->read( Units::METER_PER_SECOND, soundSpeed_ ) )
    {
        soundSpeed_ = 1500;
    }
    navigationErrorConfigReader_->read( Units::METER, navigationErrorCfg_ );
    baselineLockoutConfigReader_->read( Units::RADIAN, baselineLockoutCfg_ );
    // not used: lookbackCountConfigReader_->read( Units::COUNT, lookbackCountCfg_ );
    // not used: lookbackThresholdConfigReader_->read( Units::RADIAN, lookbackThresholdCfg_ );
    advancePingConfigReader_->read( Units::RATIO, advancePingCfg_ );
    advancePositionConfigReader_->read( Units::RATIO, advancePositionCfg_ );
    maxPositionChangeConfigReader_->read( Units::RATIO, maxPositionChangeCfg_ );
    maxSpeedWrtTransponderConfigReader_->read( Units::METER_PER_SECOND, maxSpeedWrtTransponderCfg_ );
    pingFilterSizeConfigReader_->read( Units::COUNT, pingFilterSizeCfg_ );
    // Apply limits
    pingFilterSizeCfg_ = AuvMath::Limit( pingFilterSizeCfg_, LBLNavigationIF::MIN_PING_FILTER_SIZE, LBLNavigationIF::MAX_PING_FILTER_SIZE );
    // Make odd, rounding up
    pingFilterSizeCfg_ = ( ( pingFilterSizeCfg_ / 2 ) * 2 ) + 1;
    if( pingFilterSizeCfg_ != lastPingFilterSizeCfg_ )
    {
        ResetPingFilters( verbosityCfg_, logger_ );
        lastPingFilterSizeCfg_ = pingFilterSizeCfg_;
    }
    maxPingAgeInFilterConfigReader_->read( Units::SECOND, maxPingAgeInFilterCfg_ );
    maxPingAgeConfigReader_->read( Units::SECOND, maxPingAgeCfg_ );
    fixFilterSizeConfigReader_->read( Units::COUNT, fixFilterSizeCfg_ );
    // Apply limits
    fixFilterSizeCfg_ = AuvMath::Limit( fixFilterSizeCfg_, LBLNavigationIF::MIN_FIX_FILTER_SIZE, LBLNavigationIF::MAX_FIX_FILTER_SIZE );
    // Make odd, rounding up
    fixFilterSizeCfg_ = ( ( fixFilterSizeCfg_ / 2 ) * 2 ) + 1;
    if( fixFilterSizeCfg_ != lastFixFilterSizeCfg_ )
    {
        ResetFixFilters( verbosityCfg_, logger_ );
        lastFixFilterSizeCfg_ = fixFilterSizeCfg_;
    }
    fixHalfLifeInFilterConfigReader_->read( Units::SECOND, fixHalfLifeInFilterCfg_ );

    double dt = this->getTimeOfLastRun().elapsed().asDouble();
    double nowTimestamp = Timestamp::Now().asDouble();

    // Expire last position if it is old
    if( lastPingTimestamp_ == lastPingTimestamp_ && nowTimestamp - lastPingTimestamp_ > maxPingAgeCfg_ )
    {
        uninitializeLastPing();
    }

    // Advance last postion
    if( !lastPingLocation_.isNan() )
    {
        lastPingLocation_.atBearing( course, speed  * advancePingCfg_ * dt );
    }

    // Look for recent pings -- if any, deal with them.
    for( int iPing = 0; iPing < LBLNavigationIF::NUM_PINGS; ++iPing )
    {
        Point3D &pingPosition = pingPositions_[iPing];
        if( pingPosition.isNan() )
        {
            continue;
        }

        /// Ping location (origin)
        if( pingTOFReaders_[iPing]->wasTouchedSinceLastRun( this ) )
        {

            Point3D &pingPosition = pingPositions_[iPing];

            /// Ping TOF/distance
            if( !pingTOFReaders_[iPing]->wasTouchedSinceLastRun( this ) )
            {
                continue;
            }
            double tof;
            if( !pingTOFReaders_[iPing]->read( Units::SECOND, tof ) )
            {
                logger_.syslog( "Err reading tof" + Str( iPing + 1, 10 ), Syslog::ERROR );
                continue;
            }

            double pingDistance = CalcDistanceDistanceFromTOF( tof, pingPosition.getZ(), depth, soundSpeed_ );
            if( verbosityCfg_ > 0 )
            {
                logger_.syslog( "Got ping from #" + Str( iPing + 1, 10 ) + ", tof=" + Str( tof ) + ", distance= " + Str( pingDistance ), Syslog::INFO );
            }

            Location pingLocation = Location( Units::RADIAN, pingPosition.getX(), pingPosition.getY() );

            bool pingFilteredOk = FilterPing( iPing, nowTimestamp, pingDistance,
                                              pingFilterSizeCfg_, maxPingAgeInFilterCfg_, maxSpeedWrtTransponderCfg_,
                                              verbosityCfg_, logger_ );

            if( pingFilteredOk )
            {
                /// Deal with uninitialized stuff
                if( !isnan( lastPingDistance_ ) && !lastPingLocation_.isNan() )
                {
                    if( DoLBL( location, depth,
                               pingLocation, pingDistance, lastPingLocation_, lastPingDistance_,
                               baselineLockoutCfg_, logger_ )
                            && !location.isNan() )
                    {
                        FilterFix( currentLocation, location, nowTimestamp,
                                   fixFilterSizeCfg_, fixHalfLifeInFilterCfg_, advancePositionCfg_, maxPositionChangeCfg_,
                                   verbosityCfg_, logger_ );
                        double lblLatitude = location.getLat( Units::RADIAN );
                        double lblLongitude = location.getLon( Units::RADIAN );
                        //lblLatitude = lblLatitude * advancePositionCfg_ + latitude * ( 1 - advancePositionCfg_ );
                        //lblLongitude = lblLongitude * advancePositionCfg_ + longitude * ( 1 - advancePositionCfg_ );
                        if( !isnan( latitude ) && !isnan( longitude ) )
                        {
                            if( verbosityCfg_ > 0 )
                            {
                                logger_.syslog( Str( "With channels" ) + lastPingChannel_ + "and" + iPing + ", setting location to " + location.toString( Units::DEGREE ), Syslog::INFO );
                            }
                            latitudeWriter_->writeWithAccuracy( Units::RADIAN, lblLatitude, navigationErrorCfg_ );
                            longitudeWriter_->writeWithAccuracy( Units::RADIAN, lblLongitude, navigationErrorCfg_ );
                            wroteLocation = true;
                        }
                    }
                }

                // Finally
                lastPingTimestamp_ = nowTimestamp;
                lastPingDistance_ = pingDistance;
                lastPingLocation_ = pingLocation;
                lastPingChannel_ = iPing;

            }
        }
    }
    if( !wroteLocation )
    {
        latitudeWriter_->setAccuracy( Units::RADIAN, DataElement::NO_ACCURACY );
        longitudeWriter_->setAccuracy( Units::RADIAN, DataElement::NO_ACCURACY );
    }
}

bool LBLNavigation::DoLBL( Location& location, double depth,
                           Location pingLocation, double pingDistance, Location lastPingLocation, double lastPingDistance,
                           double baselineLockoutCfg, LoggerBase& logger )
{

    if( pingLocation.getDistance( lastPingLocation ) < baselineLockoutCfg )
    {
        logger.syslog( "Ignoring LBL ping, too close to previous ping location=", pingLocation.getDistance( lastPingLocation ), Syslog::INFO );
        return false;
    }

    double distanceFromBaseline = location.distanceFromBaseline( pingLocation, lastPingLocation );
    if( distanceFromBaseline < baselineLockoutCfg )
    {
        if( distanceFromBaseline < 0 )
        {
            logger.syslog( "Negative distanceFromBaseline from l0=" + location.toString( Units::DEGREE ) + ",pingAt=" + pingLocation.toString( Units::DEGREE ) + ", lastPing=" + lastPingLocation.toString( Units::DEGREE ), Syslog::ERROR );
        }

        logger.syslog( "Ignoring LBL ping, distance from baseline=", distanceFromBaseline, Syslog::INFO );
        return false;
    }

    /// circles intersect
    Location lblLocationA, lblLocationB;
    bool intersect = Location::IntersectingCircles( pingLocation, pingDistance, lastPingLocation, lastPingDistance, lblLocationA, lblLocationB );
    if( !intersect )
    {
        logger.syslog( "Ignoring LBL ping, circles do not intersect", Syslog::INFO );
        return false;
    }

    // Method 1: nearest distance to intersection
    double distA = location.getDistance( lblLocationA );
    double distB = location.getDistance( lblLocationB );
    Location lblLocationAlt;

    if( distA < distB )
    {
        location = lblLocationA;
    }
    else
    {
        location = lblLocationB;
    }

    return true;
}

double LBLNavigation::CalcDistanceDistanceFromTOF( double tof, double pingDepth, double depth, double soundSpeed )
{
    double distance = tof * soundSpeed;
    if( pingDepth != depth )
    {
        double distance2 = distance * distance + ( pingDepth - depth ) * ( pingDepth - depth );
        distance = distance2 >= 0 ? sqrt( distance2 ) : 0;
    }
    //printf("CalcDistanceDistanceFromTOF( tof=%g, pingDepth=%g, depth=%g, soundSpeed=%g ) = %g\n", tof, pingDepth, depth, soundSpeed, distance);
    return distance;
}

// reset ping filters, either after initialization or after filter size change
void LBLNavigation::ResetPingFilters( int verbosity, LoggerBase& logger )
{
    for( int iPing = 0; iPing < LBLNavigationIF::NUM_PINGS; ++iPing )
    {
        /// Ping filter
        for( int iFilt = 0; iFilt < LBLNavigationIF::MAX_PING_FILTER_SIZE; ++iFilt )
        {
            PingFilter_[iPing][iFilt].distance_ = 0;
            PingFilter_[iPing][iFilt].timestamp_ = 0;
        }
        PingFilterPos_[iPing] = -1;
    }
    if( verbosity > 0 )
    {
        logger.syslog( Str( "Reset ping filters" ), Syslog::INFO );
    }

}

// reset ping filters, either after initialization or after filter size change
void LBLNavigation::ResetFixFilters( int verbosity, LoggerBase& logger )
{
    /// Fix filter
    for( int iFilt = 0; iFilt < LBLNavigationIF::MAX_FIX_FILTER_SIZE; ++iFilt )
    {
        FixFilter_[iFilt].position_ = nanf( "" );
        FixFilter_[iFilt].timestamp_ = 0;
    }
    FixFilterPos_ = -1;
    if( verbosity > 0 )
    {
        logger.syslog( Str( "Reset fix filter" ), Syslog::INFO );
    }
}

// returns true if ping meets filter conditions
int LBLNavigation::FilterPing( const int& iPing, const double& pingTime, const double& pingDistance,
                               int pingFilterSize, double maxPingAgeInFilter, double maxSpeedWrtTransponder,
                               int verbosity, LoggerBase& logger )
{
    // Either eliminate bottom entry
    if( PingFilterPos_[iPing] == pingFilterSize )
    {
        for( int iFilt = 0; iFilt < PingFilterPos_[iPing]; ++iFilt )
        {
            //printf("******* Dumping bottom filter, iFilt=%d, PingFilterPos_[%d]=%d *****\n",iFilt, iPing, PingFilterPos_[iPing]);
            PingFilter_[iPing][iFilt].distance_ = PingFilter_[iPing][iFilt + 1].distance_;
            PingFilter_[iPing][iFilt].timestamp_ = PingFilter_[iPing][iFilt + 1].timestamp_;
        }
        PingFilter_[iPing][PingFilterPos_[iPing]].distance_ = nanf( "" );
        PingFilter_[iPing][PingFilterPos_[iPing]].timestamp_ = nanf( "" );
    }
    // Or add an entry to the top of the filter
    else
    {
        ++PingFilterPos_[iPing];
        //printf("******* Incremented PingFilterPos_[%d]=%d *****\n", iPing, PingFilterPos_[iPing]);
    }

    // Set the top entry
    PingFilter_[iPing][PingFilterPos_[iPing]].distance_ = pingDistance;
    PingFilter_[iPing][PingFilterPos_[iPing]].timestamp_ = pingTime;

    // Remove old entries
    while( PingFilterPos_[iPing] >= 0 && pingTime - PingFilter_[iPing][0].timestamp_ > maxPingAgeInFilter )
    {
        if( verbosity > 0 )
        {
            logger.syslog( Str( "For ping chan " ) + iPing + " filter, dropping ping aged " + ( pingTime - PingFilter_[iPing][0].timestamp_ ), Syslog::INFO );
        }
        for( int iFilt = 0; iFilt < PingFilterPos_[iPing]; ++iFilt )
        {
            //printf("******* Shift old with iFilt=%d, PingFilterPos_[%d]=%d, ts=%lu *****\n",iFilt, iPing, PingFilterPos_[iPing], (unsigned long)PingFilter_[iPing][0].timestamp_);
            PingFilter_[iPing][iFilt].distance_ = PingFilter_[iPing][iFilt + 1].distance_;
            PingFilter_[iPing][iFilt].timestamp_ = PingFilter_[iPing][iFilt + 1].timestamp_;
        }
        PingFilter_[iPing][PingFilterPos_[iPing]].distance_ = nanf( "" );
        PingFilter_[iPing][PingFilterPos_[iPing]].timestamp_ = nanf( "" );
        --PingFilterPos_[iPing];
    }

    // If filter isn't full enough return now.
    if( PingFilterPos_[iPing] < LBLNavigationIF::MIN_PING_FILTER_SIZE - 1 )
    {
        if( verbosity > 0 )
        {
            logger.syslog( Str( "Ping chan " ) + iPing + " filter not full", Syslog::INFO );
        }
        return false;
    }

    // Otherwise do median-like filtering
    int numOk = 0;
    double dDistance = 0, dTime = 0, speedWrtTransponder = 0;
    for( int iFilt = 0; iFilt < PingFilterPos_[iPing]; ++iFilt )
    {
        dDistance = PingFilter_[iPing][PingFilterPos_[iPing]].distance_ - PingFilter_[iPing][iFilt].distance_;
        dTime = PingFilter_[iPing][PingFilterPos_[iPing]].timestamp_ - PingFilter_[iPing][iFilt].timestamp_;
        speedWrtTransponder = abs( dDistance / dTime );
        if( speedWrtTransponder <= maxSpeedWrtTransponder )
        {
            ++numOk;
        }
        //printf("Calculating with PingFilterPos_[%d]=%d,iFilt=%d,dist=%g,speed=%g,numOk=%d\n",iPing,PingFilterPos_[iPing],iFilt,PingFilter_[iPing][iFilt].distance_,speedWrtTransponder,numOk);
    }

    bool ok = numOk > PingFilterPos_[iPing] / 2;
    if( verbosity > 0 )
    {
        logger.syslog( Str( "Ping chan " ) + iPing + " returning " + ( ok ? "true" : "false" ), Syslog::INFO );
    }
    return ok;
}

// updates currrentLocation by past filtered fixes
void LBLNavigation::FilterFix( const Location& currentLocation, Location& fix, const double& fixTime,
                               int fixFilterSize, double fixHalfLifeInFilter, double advancePosition, double maxPositionChange,
                               int verbosity, LoggerBase& logger )
{
    // Either eliminate bottom entry
    if( FixFilterPos_ == fixFilterSize - 1 )
    {
        for( int iFilt = 0; iFilt < FixFilterPos_; ++iFilt )
        {
            FixFilter_[iFilt].position_ = FixFilter_[iFilt + 1].position_;
            FixFilter_[iFilt].timestamp_ = FixFilter_[iFilt + 1].timestamp_;
        }
        FixFilter_[FixFilterPos_].position_ = nanf( "" );
        FixFilter_[FixFilterPos_].timestamp_ = nanf( "" );
    }
    // Or add an entry to the top of the filter
    else
    {
        ++FixFilterPos_;
    }

    // Set the top entry
    FixFilter_[FixFilterPos_].position_ = currentLocation.calcVector( fix );
    FixFilter_[FixFilterPos_].timestamp_ = fixTime;

    // Perform the filtering
    double fixFilterWeights[LBLNavigationIF::MAX_FIX_FILTER_SIZE] = { 1 };
    double fixElapsed;
    double fixFilterWeightSum = 0;
    double fixDistance, fixDistWeight;

    for( int iFilt = 0; iFilt <= FixFilterPos_; ++iFilt )
    {
        fixElapsed = fixTime - FixFilter_[iFilt].timestamp_;
        fixFilterWeights[iFilt] = pow( 2.0, - fixElapsed / fixHalfLifeInFilter );
    }

    for( int iFilt = 0; iFilt <= FixFilterPos_; ++iFilt )
    {
        for( int jFilt = 0; jFilt < iFilt; ++jFilt )
        {
            if( iFilt == jFilt ) continue;
            fixDistance = ( FixFilter_[iFilt].position_ - FixFilter_[jFilt].position_ ).getMagnitude();
            fixDistance = MAX( fixDistance, 1.0 );
            fixDistWeight = 1 / fixDistance;
            fixFilterWeights[iFilt] *= fixDistWeight;
            fixFilterWeights[jFilt] *= fixDistWeight;
        }
    }

    Point2D filteredFix = 0;
    for( int iFilt = 0; iFilt <= FixFilterPos_; ++iFilt )
    {
        filteredFix += FixFilter_[iFilt].position_ * fixFilterWeights[iFilt];
        fixFilterWeightSum +=  fixFilterWeights[iFilt];
    }

    if( fixFilterWeightSum == 0 )
    {
        fixFilterWeightSum = 1;
    }

    filteredFix *= 1 / fixFilterWeightSum;

    filteredFix *= advancePosition;
    double changeMagnitude = filteredFix.getMagnitude();
    if( changeMagnitude > maxPositionChange )
    {
        filteredFix *= maxPositionChange / changeMagnitude;
    }

    // Advance fix filter
    for( int iFilt = 0; iFilt <= FixFilterPos_; ++iFilt )
    {
        FixFilter_[iFilt].position_ -= filteredFix;
    }

    // Move fix towards filtered fix
    fix = currentLocation;
    fix.delta( filteredFix );
}

void LBLNavigation::uninitializeLastPing( void )
{
    lastPingDistance_ = nanf( "" );
    lastPingLocation_ = Location();
    lastPingTimestamp_ = nanf( "" );
    lastPingChannel_ = -1;
}