/** \file
 *
 *  Contains the Navigator class implementation.
 *
 *  Copyright (c) 2013 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "Navigator.h"

#include "data/ConfigReader.h"
#include "data/Location.h"
#include "data/Matrix3x3.h" // TODO: replace with alternate LA tool
#include "data/Point3D.h" // TODO: replace with alternate LA tool
#include "data/Point6D.h" // TODO: replace with alternate LA tool
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "dockModule/SetNavIF.h"
#include "units/Units.h"


Navigator::Navigator( const Str& name, const Module* module )
    : SyncNavigationComponent( name, module ), // This is intended to be a superclass for all navigators...
      verbosity_( 0 ),
      allowableFailures_( 20 ),
      accuracyPremultiplier_( 1.0 ),
      inputMeasurementStartupAllowance_( 180 ), // TODO make this a config setting?
      startTime_( Timestamp::NOT_SET_TIME ),
      navTimeStamp_( Timestamp::NOT_SET_TIME ),
      orientationReadTime_( Timestamp::NOT_SET_TIME ),
      velocityReadTime_( Timestamp::NOT_SET_TIME ),
      orientationStaleAfter_( 1 ),
      velocityStaleAfter_( 1 ),
      timeFix_( nanf( "" ) ),
      latitudeFix_( nanf( "" ) ),
      longitudeFix_( nanf( "" ) ),
      lastTimeFix_( nanf( "" ) ),
      lastLatitudeFix_( nanf( "" ) ),
      lastLongitudeFix_( nanf( "" ) ),
      horizontalPathLengthSinceLastFix_( -1 ), // initialize at -1 so that you can use 0 to test for fix this cycle
      latitude_( nanf( "" ) ),
      longitude_( nanf( "" ) ),
      depth_( nanf( "" ) ),
      course_( nan( "" ) ),
      courseAccuracy_( nan( "" ) ),
      speed_( nan( "" ) ),
      speedAccuracy_( nan( "" ) ),
      lastLatitude_( nanf( "" ) ),
      lastLongitude_( nanf( "" ) ),
      lastDepth_( nanf( "" ) ),
      latitudeAccuracy_( nanf( "" ) ),
      longitudeAccuracy_( nanf( "" ) ),
      depthAccuracy_( nanf( "" ) ),
      horizontalPathLengthSinceLastFixAccuracy_( nanf( "" ) )
{
    // universal readers
    timeFixReader_ = newUniversalReader( UniversalURI::TIME_FIX );
    latitudeFixReader_ = newUniversalReader( UniversalURI::LATITUDE_FIX );
    longitudeFixReader_ = newUniversalReader( UniversalURI::LONGITUDE_FIX );
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
    vehicleOrientationReader_ = newUniversalBlobReader( UniversalURI::PLATFORM_ORIENTATION_MATRIX );

    setNavTimeFixReader_ = newDataReader( SetNavIF::TIME_FIX );
    setNavLatitudeFixReader_ = newDataReader( SetNavIF::LATITUDE_FIX );
    setNavLongitudeFixReader_ = newDataReader( SetNavIF::LONGITUDE_FIX );

    // universal writers
    latitudeWriter_ = newUniversalWriter( UniversalURI::LATITUDE, Units::DEGREE, nanf( "" ) );
    longitudeWriter_ = newUniversalWriter( UniversalURI::LONGITUDE, Units::DEGREE, nanf( "" ) );
    depthWriter_ = newUniversalWriter( UniversalURI::DEPTH, Units::METER, nanf( "" ) );
    platformSpeedWriter_ = newUniversalWriter( UniversalURI::PLATFORM_SPEED_WRT_GROUND, Units::METER_PER_SECOND, nanf( "" ) );
    platformCourseWriter_ = newUniversalWriter( UniversalURI::PLATFORM_COURSE, Units::RADIAN, nanf( "" ) );
    horizontalPathLengthSinceLastFixWriter_ = newUniversalWriter( UniversalURI::HORIZONTAL_PATH_LENGTH_SINCE_LAST_FIX, Units::METER, nanf( "" ) );
    fixDistanceMadeGoodWriter_ = newUniversalWriter( UniversalURI::FIX_DISTANCE_MADE_GOOD, Units::METER, nanf( "" ) );
    fixHorizontalPathLengthSinceLastFixWriter_ = newUniversalWriter( UniversalURI::FIX_HORIZONTAL_PATH_LENGTH_SINCE_LAST_FIX, Units::METER, nanf( "" ) );
    fixResidualDistanceWriter_ = newUniversalWriter( UniversalURI::FIX_RESIDUAL_DISTANCE, Units::METER, nanf( "" ) );
    fixResidualBearingWriter_ = newUniversalWriter( UniversalURI::FIX_RESIDUAL_BEARING, Units::RADIAN, nanf( "" ) );
    fixResidualPercentDistanceTraveledWriter_ = newUniversalWriter( UniversalURI::FIX_RESIDUAL_PERCENT_DISTANCE_TRAVELED, Units::NONE, nanf( "" ) );
    // Initialize all of these with NaN accuracy, which will not be reset until the vehicle gets a GPS fix.
}


Navigator::~Navigator()
{}

void Navigator::readConfigs( void )
{
    double orientationStaleAfterSeconds( orientationStaleAfter_.asDouble() ), velocityStaleAfterSeconds( velocityStaleAfter_.asDouble() );
    verbosityLevelConfigReader_->read( Units::COUNT, verbosity_ );
    allowableFailuresConfigReader_->read( Units::COUNT, allowableFailures_ );
    orientationStaleAfterConfigReader_->read( Units::SECOND, orientationStaleAfterSeconds );
    velocityStaleAfterConfigReader_->read( Units::SECOND, velocityStaleAfterSeconds );
    if( verbosity_ > 1 ) logger_.syslog( "Will consider orientation measurement stale after " + Str( orientationStaleAfterSeconds, 0 ) + "s.", Syslog::INFO );
    if( verbosity_ > 1 ) logger_.syslog( "Will consider velocity measurement stale after " + Str( velocityStaleAfterSeconds, 0 ) + "s.", Syslog::INFO );
    orientationStaleAfter_ = Timespan( orientationStaleAfterSeconds );
    velocityStaleAfter_ = Timespan( velocityStaleAfterSeconds );
}

void Navigator::readExternalFix( void )
{
    bool gotTimeFix( false ), gotLatitudeFix( false ), gotLongitudeFix( false );
    // check for a fix
    if( timeFixReader_->getTimestamp().elapsed() <= dt_ )
    {
        gotTimeFix = timeFixReader_->read( Units::SECOND, timeFix_ );
        timeAccuracy_ = timeFixReader_->getWrittenAccuracy( Units::SECOND ); // update the accuracy of the estimate
    }
    if( latitudeFixReader_->getTimestamp().elapsed() <= dt_ )
    {
        gotLatitudeFix = latitudeFixReader_->read( Units::RADIAN, latitudeFix_ );
        latitudeAccuracy_ = latitudeFixReader_->getWrittenAccuracy( Units::RADIAN ); // update the accuracy of the estimate
    }
    if( longitudeFixReader_->getTimestamp().elapsed() <= dt_ )
    {
        gotLongitudeFix = longitudeFixReader_->read( Units::RADIAN, longitudeFix_ );
        longitudeAccuracy_ = longitudeFixReader_->getWrittenAccuracy( Units::RADIAN ); // update the accuracy of the estimate
    }
    if( gotTimeFix && gotLatitudeFix && gotLongitudeFix )
    {
        if( verbosity_ > 1 )
        {
            char msg[256];
            sprintf( msg, "Got GPS fix: latitudeAccuracy_ = %g, longitudeAccuracy = %g", latitudeAccuracy_, longitudeAccuracy_ );
            logger_.syslog( msg, Syslog::DEBUG );
        }
        // XXX DO NOT reset this time. It would break the intended functionality of wasTouchedSinceLastRun -- see e.g., [UniversalFixResidualReporter].
        // navTimeStamp_ = timeFix_; // Timestamp::Timestamp has = overloaded as a "set" operator for double epoch seconds.
        if( !isnan( horizontalPathLengthSinceLastFix_ ) && horizontalPathLengthSinceLastFix_ > 500. ) // TODO Change this 500 m to a config parameter.
        {
            if( verbosity_ > 0 )
            {
                char msg[256];
                sprintf( msg, "horizontalPathLengthSinceLastFix_ = %g, recording fix residuals...", horizontalPathLengthSinceLastFix_ );
                logger_.syslog( msg, Syslog::DEBUG );
            }
            evaluateAndRecordFixResidual();
        }
        lastTimeFix_ = timeFix_;
    }
    if( gotLatitudeFix && gotLongitudeFix )
    {
        if( !gotTimeFix && verbosity_ > 1 )
        {
            char msg[256];
            sprintf( msg, "Got non-GPS fix: latitudeAccuracy_ = %g, longitudeAccuracy = %g", latitudeAccuracy_, longitudeAccuracy_ );
            logger_.syslog( msg, Syslog::DEBUG );
        }
        lastLatitudeFix_ = latitudeFix_;
        lastLongitudeFix_ = longitudeFix_;
        horizontalPathLengthSinceLastFix_ = 0.0;
        // TODO: It is not clear whether overwriting these lat/lon is general enough for a Navigator superclass... or if this part actually belongs in the DeadReckoner superclass.
        // overwrite current geographic position
        latitude_ = latitudeFix_;
        longitude_ = longitudeFix_;
        depth_ = 0; // assume you are on the surface -- Other depth sources should write more accurately, but the DR methods should at least zero depth here.
        depthAccuracy_ = 1.0e6; // reset this to something large -- want to rely on Keller & CTD in all but the most dire of circumstances...
        horizontalPathLengthSinceLastFixAccuracy_ = 1.0; // reset this to something reasonable
        latitudeFix_ = nanf( "" );
        longitudeFix_ = nanf( "" );
    }
}

void Navigator::readSetNavFix( void )
{
    bool gotTimeFix( false ), gotLatitudeFix( false ), gotLongitudeFix( false );
    // check for a fix
    if( setNavTimeFixReader_->getTimestamp().elapsed() <= dt_ )
    {
        gotTimeFix = setNavTimeFixReader_->read( Units::SECOND, timeFix_ );
        timeAccuracy_ = 0.01;
    }
    if( setNavLatitudeFixReader_->getTimestamp().elapsed() <= dt_ )
    {
        gotLatitudeFix = setNavLatitudeFixReader_->read( Units::RADIAN, latitudeFix_ );
        latitudeAccuracy_ = 0.001; // update the accuracy of the estimate
    }
    if( setNavLongitudeFixReader_->getTimestamp().elapsed() <= dt_ )
    {
        gotLongitudeFix = setNavLongitudeFixReader_->read( Units::RADIAN, longitudeFix_ );
        longitudeAccuracy_ = 0.001; // update the accuracy of the estimate
    }
    if( gotTimeFix && gotLatitudeFix && gotLongitudeFix )
    {
        if( verbosity_ > 1 )
        {
            char msg[256];
            sprintf( msg, "Got GPS fix: latitudeAccuracy_ = %g, longitudeAccuracy = %g", latitudeAccuracy_, longitudeAccuracy_ );
            logger_.syslog( msg, Syslog::DEBUG );
        }
        // XXX DO NOT reset this time. It would break the intended functionality of wasTouchedSinceLastRun -- see e.g., [UniversalFixResidualReporter].
        // navTimeStamp_ = timeFix_; // Timestamp::Timestamp has = overloaded as a "set" operator for double epoch seconds.
        if( horizontalPathLengthSinceLastFix_ > 500. ) // TODO Change this 500 m to a config parameter.
        {
            if( verbosity_ > 0 )
            {
                char msg[256];
                sprintf( msg, "horizontalPathLengthSinceLastFix_ = %g, recording fix residuals...", horizontalPathLengthSinceLastFix_ );
                logger_.syslog( msg, Syslog::DEBUG );
            }
            evaluateAndRecordFixResidual();
        }
        lastTimeFix_ = timeFix_;
        lastLatitudeFix_ = latitudeFix_;
        lastLongitudeFix_ = longitudeFix_;
        horizontalPathLengthSinceLastFix_ = 0.0;
        // TODO: It is not clear whether overwriting these lat/lon is general enough for a Navigator superclass... or if this part actually belongs in the DeadReckoner superclass.
        // overwrite current geographic position
        latitude_ = latitudeFix_;
        longitude_ = longitudeFix_;
        depth_ = 0; // assume you are on the surface -- Other depth sources should write more accurately, but the DR methods should at least zero depth here.
        depthAccuracy_ = 1.0e6; // reset this to something large -- want to rely on Keller & CTD in all but the most dire of circumstances...
        horizontalPathLengthSinceLastFixAccuracy_ = 1.0; // reset this to something reasonable
        latitudeFix_ = nanf( "" );
        longitudeFix_ = nanf( "" );
    }
}

void Navigator::readVehicleOrientation( void )
{
    if( vehicleOrientationReader_->read2DClass( Units::NONE, rotationFromVehicleToNavigationFrame_ ) ) // read rotation matrix from FSK to NED
    {
        orientationReadTime_ = Timestamp::Now();
    }
    else if( orientationReadTime_.elapsed() < orientationStaleAfter_ )
    {


        if( verbosity_ > 0 )	logger_.syslog( "Most recent orientation data is " + Str( orientationReadTime_.elapsed().asDouble() ) + " seconds old.", Syslog::DEBUG );

    }
    else if( startTime_.elapsed() < orientationStaleAfter_ )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Starting up and don't have orientation data yet.", Syslog::DEBUG );
    }
    else
    {
        logger_.syslog( "Unable to read the rotation from vehicle frame to navigation frame for more than " + Str( orientationStaleAfter_.asDouble(), 0 ) + " seconds.", Syslog::FAULT );
        this->setFailure( FailureMode::SOFTWARE );
    }
}

void Navigator::writeVehiclePosition( void )
{
    // In the event that a particular navigator has dropped out for a period of time (e.g. speed wrt sea floor due to DVL loss),
    // the latitude accuracy will have been set to nan and won't get set to a valid number again until readExternalFix sets it.
    // This doesn't allow for a given navigator to pick up where another of lesser accuracy has left off. Thus the check below
    // to see if at least someone is writing lat/lon once _this_ navigator has a valid depth/path length accuracy. The idea
    // is to allow a given navigator to come and go while one with less accuracy continues on in the meantime. A simple example
    // of this is DVL speed wrt bottom and speed based on prop turns. The vehicle might as well use the DVL when is comes back
    // and simply start the better navigation based on the last written position. Otherwise, it won't be used until the next
    // external fix is read.

    // Check to see if lat/lon are nan while other members are good.
    if( !isnan( depthAccuracy_ ) && !isnan( horizontalPathLengthSinceLastFixAccuracy_ ) && isnan( latitudeAccuracy_ ) && isnan( longitudeAccuracy_ ) )
    {
        // If that's the case, find lat/lon accuracy if it exists
        if( latitudeReader_->isActive() && longitudeReader_->isActive() )
        {
            if( verbosity_ > 0 ) logger_.syslog( "RESETTING LAT/LON accuracy", Syslog::INFO );
            latitudeAccuracy_ = latitudeReader_->getWrittenAccuracy( Units::RADIAN );
            longitudeAccuracy_ = longitudeReader_->getWrittenAccuracy( Units::RADIAN );
        }

    }

    if( !isnan( latitudeAccuracy_ ) && !isnan( longitudeAccuracy_ ) && !isnan( depthAccuracy_ ) && !isnan( horizontalPathLengthSinceLastFixAccuracy_ ) ) // NOTE: Position will only be written if accuracies are all defined.
    {
        if( isnan( latitude_ ) || isnan( longitude_ ) || isnan( depth_ ) || isnan( horizontalPathLengthSinceLastFix_ ) ) // Belt & suspenders!
        {
            // XXX This condition *should not* happen! XXX

            // set internal accuracies to NaN, as they _should_ already be if everything was working as intended.
            latitudeAccuracy_ = nan( "" );
            longitudeAccuracy_ = nan( "" );
            depthAccuracy_ = nan( "" );
            horizontalPathLengthSinceLastFixAccuracy_ = nan( "" );
            // DO NOT write position out to the slate. Stop it here and prevent the effect from cascading to other components.
            latitudeWriter_->setInvalid( true );
            longitudeWriter_->setInvalid( true );
            depthWriter_->setInvalid( true );
            platformSpeedWriter_->setInvalid( true );
            platformCourseWriter_->setInvalid( true );
            horizontalPathLengthSinceLastFixWriter_->setInvalid( true );
            // Record the event so that we can track the bug further upstream after pulling the full logs. Including horizontalPathLengthSinceLastFix_ in this message should allow us to spot cases where the position _fix_ is being written with NaN.
            char msg[1024]; // TODO: How long does this buffer actually need to be?
            sprintf( msg, "Caught NaN! Will not write estimated position: latitude_ = %g, longitude_ = %g, depth_ = %g, horizontalPathLengthSinceLastFix_ = %g, latitudeAccuracy_ = %g, longitudeAccuracy_ = %g, depthAccuracy_ = %g", latitude_, longitude_, depth_, horizontalPathLengthSinceLastFix_, latitudeAccuracy_, longitudeAccuracy_, depthAccuracy_ );
            logger_.syslog( msg, Syslog::ERROR );
            this->setFailure( FailureMode::SOFTWARE );
        }
        else if( ( velocityReadTime_.elapsed() < velocityStaleAfter_ ) && ( orientationReadTime_.elapsed() < orientationStaleAfter_ ) )
        {
            latitudeWriter_->setInvalid( false );
            longitudeWriter_->setInvalid( false );
            depthWriter_->setInvalid( false );
            horizontalPathLengthSinceLastFixWriter_->setInvalid( false );
            latitudeWriter_->writeWithAccuracy( Units::RADIAN, latitude_, latitudeAccuracy_, navTimeStamp_ );
            longitudeWriter_->writeWithAccuracy( Units::RADIAN, longitude_, longitudeAccuracy_, navTimeStamp_ );
            depthWriter_->writeWithAccuracy( Units::METER, depth_, depthAccuracy_, navTimeStamp_ );
            if( !isnan( speed_ ) && !isnan( speedAccuracy_ ) )
            {
                platformSpeedWriter_->setInvalid( false );
                platformSpeedWriter_->writeWithAccuracy( Units::METER_PER_SECOND, speed_, speedAccuracy_ );
            }
            if( !isnan( course_ ) && !isnan( courseAccuracy_ ) )
            {
                platformCourseWriter_->setInvalid( false );
                platformCourseWriter_->writeWithAccuracy( Units::RADIAN, course_, courseAccuracy_ );
            }
            horizontalPathLengthSinceLastFixWriter_->writeWithAccuracy( Units::METER, horizontalPathLengthSinceLastFix_, horizontalPathLengthSinceLastFixAccuracy_, navTimeStamp_ );
            elapsedSinceOrientationReadWriter_->write( Units::SECOND, orientationReadTime_.elapsed().asDouble(), navTimeStamp_ );
            elapsedSinceVelocityReadWriter_->write( Units::SECOND, velocityReadTime_.elapsed().asDouble(), navTimeStamp_ );
            latitudeAccuracyWriter_->write( Units::RADIAN, longitudeAccuracy_, navTimeStamp_ ); // Log the navigational accuracy
            this->resetFailCount();
        }
        else if( horizontalPathLengthSinceLastFix_ == 0 )  // If there is a GPS fix, but other navigation data is invalid, horizontalPathLengthSinceLastFix_ will be zero -- still write position
        {
            latitudeWriter_->setInvalid( false );
            longitudeWriter_->setInvalid( false );
            depthWriter_->setInvalid( false );
            horizontalPathLengthSinceLastFixWriter_->setInvalid( false );
            latitudeWriter_->writeWithAccuracy( Units::RADIAN, latitude_, latitudeAccuracy_, navTimeStamp_ );
            longitudeWriter_->writeWithAccuracy( Units::RADIAN, longitude_, longitudeAccuracy_, navTimeStamp_ );
            depthWriter_->writeWithAccuracy( Units::METER, depth_, depthAccuracy_, navTimeStamp_ );
            if( !isnan( speed_ ) && !isnan( speedAccuracy_ ) )
            {
                platformSpeedWriter_->setInvalid( false );
                platformSpeedWriter_->writeWithAccuracy( Units::METER_PER_SECOND, speed_, speedAccuracy_ );
            }
            if( !isnan( course_ ) && !isnan( courseAccuracy_ ) )
            {
                platformCourseWriter_->setInvalid( false );
                platformCourseWriter_->writeWithAccuracy( Units::RADIAN, course_, courseAccuracy_ );
            }
            horizontalPathLengthSinceLastFixWriter_->writeWithAccuracy( Units::METER, horizontalPathLengthSinceLastFix_, horizontalPathLengthSinceLastFixAccuracy_, navTimeStamp_ );
            elapsedSinceOrientationReadWriter_->write( Units::SECOND, orientationReadTime_.elapsed().asDouble(), navTimeStamp_ );
            elapsedSinceVelocityReadWriter_->write( Units::SECOND, velocityReadTime_.elapsed().asDouble(), navTimeStamp_ );
            latitudeAccuracyWriter_->write( Units::RADIAN, longitudeAccuracy_, navTimeStamp_ ); // Log the navigational accuracy
            if( verbosity_ > 0 ) logger_.syslog( "Writing estimated position: horizontal path length since last fix is zero, but velocity and/or orientation data is invalid.", Syslog::DEBUG );
            horizontalPathLengthSinceLastFix_ = 0.001; // just enough to fail the else if test next time
            this->resetFailCount();
        }
        else
        {
            elapsedSinceOrientationReadWriter_->write( Units::SECOND, orientationReadTime_.elapsed().asDouble(), navTimeStamp_ );
            elapsedSinceVelocityReadWriter_->write( Units::SECOND, velocityReadTime_.elapsed().asDouble(), navTimeStamp_ );
            if( startTime_.elapsed() > inputMeasurementStartupAllowance_ )
            {
                if( verbosity_ > 1 ) logger_.syslog( "Will not write estimated position: allowance for input measurements to start up has expired, velocity or orientation is invalid, and horizontal path length since last fix is nonzero.", Syslog::ERROR );
                this->setFailure( FailureMode::SOFTWARE );
            }
            else
            {
                if( verbosity_ > 1 ) logger_.syslog( "Will not write estimated position: allowance for input measurements to start up has not expired, but velocity or orientation is invalid, and horizontal path length since last fix is nonzero.", Syslog::DEBUG );
            }
        }
    }
    else
    {
        latitudeWriter_->setInvalid( true );
        longitudeWriter_->setInvalid( true );
        depthWriter_->setInvalid( true );
        platformSpeedWriter_->setInvalid( true );
        platformCourseWriter_->setInvalid( true );
        horizontalPathLengthSinceLastFixWriter_->setInvalid( true );
        if( startTime_.elapsed() > inputMeasurementStartupAllowance_ )
        {
            if( verbosity_ > 0 )logger_.syslog( "Will not write estimated position: latitudeAccuracy_ = " + Str( latitudeAccuracy_ ) + ", longitudeAccuracy_ = " + Str( longitudeAccuracy_ ) + ", depthAccuracy_ = " + Str( depthAccuracy_ ), Syslog::ERROR );
            this->setFailure( FailureMode::SOFTWARE );
        }
        else if( verbosity_ > 1 )
        {
            logger_.syslog( "Will not write estimated position: latitudeAccuracy_ = " + Str( latitudeAccuracy_ ) + ", longitudeAccuracy_ = " + Str( longitudeAccuracy_ ) + ", depthAccuracy_ = " + Str( depthAccuracy_ ), Syslog::DEBUG );
        }
    }
    // TODO: Consider moving the assignments below into the top branch of the if statement above. This would require a different definition of the timeElapsed_ variable in order to produce usable navigation estimates.
    lastLatitude_ = latitude_;
    lastLongitude_ = longitude_;
    lastDepth_ = depth_;
}

void Navigator::evaluateAndRecordFixResidual( void )
{
    double fixResidualDistance, fixResidualBearing, fixDistanceMadeGood, fixResidualPercentDistanceTraveled;

    fixDistanceMadeGood = Location::GetDistanceFast( latitudeFix_, longitudeFix_, lastLatitudeFix_, lastLongitudeFix_ );
    fixResidualDistance = Location::GetDistanceFast( latitudeFix_, longitudeFix_, latitude_, longitude_ );
    fixResidualBearing = Location::GetBearing( latitudeFix_, longitudeFix_, latitude_, longitude_ );
    fixResidualPercentDistanceTraveled = fixResidualDistance / horizontalPathLengthSinceLastFix_ * 100.0;

    // Use horizontalPathLengthSinceLastFixAccuracy_ as the accuracy proxy to drive which component is chosen.
    fixHorizontalPathLengthSinceLastFixWriter_->writeWithAccuracy( Units::METER, horizontalPathLengthSinceLastFix_, horizontalPathLengthSinceLastFixAccuracy_, navTimeStamp_ );
    fixDistanceMadeGoodWriter_->writeWithAccuracy( Units::METER, fixDistanceMadeGood, horizontalPathLengthSinceLastFixAccuracy_, navTimeStamp_ );
    fixResidualDistanceWriter_->writeWithAccuracy( Units::METER, fixResidualDistance, horizontalPathLengthSinceLastFixAccuracy_, navTimeStamp_ );
    fixResidualBearingWriter_->writeWithAccuracy( Units::RADIAN, fixResidualBearing, horizontalPathLengthSinceLastFixAccuracy_, navTimeStamp_ );
    fixResidualPercentDistanceTraveledWriter_->writeWithAccuracy( Units::PERCENT, fixResidualPercentDistanceTraveled, horizontalPathLengthSinceLastFixAccuracy_, navTimeStamp_ );
}