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

#include "UniversalFixResidualReporter.h"
#include "UniversalFixResidualReporterIF.h"

#include "data/Location.h"

#include "data/ConfigReader.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"

UniversalFixResidualReporter::UniversalFixResidualReporter( const Module* module )
    : SyncNavigationComponent( UniversalFixResidualReporterIF::NAME, module ),
      verbosity_( 0 )
{
    verbosityLevelConfigReader_ = newConfigReader( UniversalFixResidualReporterIF::VERBOSITY_CFG );
    // universal readers
    timeFixReader_ = newUniversalReader( UniversalURI::TIME_FIX );
    latitudeFixReader_ = newUniversalReader( UniversalURI::LATITUDE_FIX );
    longitudeFixReader_ = newUniversalReader( UniversalURI::LONGITUDE_FIX );
    fixHorizontalPathLengthSinceLastFixReader_ = newUniversalReader( UniversalURI::FIX_HORIZONTAL_PATH_LENGTH_SINCE_LAST_FIX );
    fixDistanceMadeGoodReader_ = newUniversalReader( UniversalURI::FIX_DISTANCE_MADE_GOOD );
    fixResidualDistanceReader_ = newUniversalReader( UniversalURI::FIX_RESIDUAL_DISTANCE );
    fixResidualBearingReader_ = newUniversalReader( UniversalURI::FIX_RESIDUAL_BEARING );
    fixResidualPercentDistanceTraveledReader_ = newUniversalReader( UniversalURI::FIX_RESIDUAL_PERCENT_DISTANCE_TRAVELED );
}

UniversalFixResidualReporter::~UniversalFixResidualReporter()
{}

void UniversalFixResidualReporter::initialize( void )
{
    logger_.syslog( "Initializing UniversalFixResidualReporter component." );
    // Apparently don't need to request data if just waiting for it to become available at the convenience of the other component.
    verbosityLevelConfigReader_->read( Units::COUNT, verbosity_ );
}

void UniversalFixResidualReporter::run( void )
{
    if( fixHorizontalPathLengthSinceLastFixReader_->wasTouchedSinceLastRun( this ) && fixDistanceMadeGoodReader_->wasTouchedSinceLastRun( this ) && fixResidualDistanceReader_->wasTouchedSinceLastRun( this ) && fixResidualBearingReader_->wasTouchedSinceLastRun( this ) && fixResidualPercentDistanceTraveledReader_->wasTouchedSinceLastRun( this ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Fix parameters were touched since the last run.", Syslog::DEBUG );
        reportFixResidual();
    }
}

void UniversalFixResidualReporter::reportFixResidual( void )
{
    bool gotTimeFix( false ), gotLatitudeFix( false ), gotLongitudeFix( false ), gotFixDistanceMadeGood( false ), gotHorizontalPathLengthSinceLastFix( false ), gotFixResidualDistance( false ), gotFixResidualBearing( false ), gotFixResidualPercentDistanceTraveled( false );
    float timeFix, latitudeFix, longitudeFix, fixDistanceMadeGood, horizontalPathLengthSinceLastFix, fixResidualDistance, fixResidualBearing, fixResidualPercentDistanceTraveled;

    gotTimeFix = timeFixReader_->read( Units::SECOND, timeFix );
    gotLatitudeFix = latitudeFixReader_->read( Units::DEGREE, latitudeFix );
    gotLongitudeFix = longitudeFixReader_->read( Units::DEGREE, longitudeFix );
    gotHorizontalPathLengthSinceLastFix = fixHorizontalPathLengthSinceLastFixReader_->read( Units::METER, horizontalPathLengthSinceLastFix );
    gotFixDistanceMadeGood = fixDistanceMadeGoodReader_->read( Units::METER, fixDistanceMadeGood );
    gotFixResidualDistance = fixResidualDistanceReader_->read( Units::METER, fixResidualDistance );
    gotFixResidualBearing = fixResidualBearingReader_->read( Units::DEGREE, fixResidualBearing );
    gotFixResidualPercentDistanceTraveled = fixResidualPercentDistanceTraveledReader_->read( Units::PERCENT, fixResidualPercentDistanceTraveled );

    if( gotTimeFix && gotLatitudeFix && gotLongitudeFix && gotHorizontalPathLengthSinceLastFix && gotFixDistanceMadeGood && gotFixResidualDistance && gotFixResidualBearing && gotFixResidualPercentDistanceTraveled )
    {
        char msg[1024];
        sprintf( msg, "Fix residual: %.1f %%DT, over the last %.1f m. Residual distance %.1f m at bearing %.1f degrees. Fix at (%.4f, %.4f) with %.1f m made good.",
                 fixResidualPercentDistanceTraveled, horizontalPathLengthSinceLastFix, fixResidualDistance, fixResidualBearing, latitudeFix, longitudeFix, fixDistanceMadeGood );
        logger_.syslog( msg, ( verbosity_ > 0 ) ? Syslog::IMPORTANT : Syslog::INFO );
        // TODO: Add some annotation of which element these fix parameters are coming from (hopefully the same element for all of them).
    }
    else if( gotTimeFix || gotLatitudeFix || gotLongitudeFix || gotHorizontalPathLengthSinceLastFix || gotFixDistanceMadeGood || gotFixResidualDistance || gotFixResidualBearing || gotFixResidualPercentDistanceTraveled )
    {
        logger_.syslog( "Could not read all data about the new fix.", Syslog::ERROR );
        char got[1024];
        sprintf( got, "gotTimeFix %d gotLatitudeFix %d gotLongitudeFix %d gotHorizontalPathLengthSinceLastFix %d gotFixDistanceMadeGood %d gotFixResidualDistance %d gotFixResidualBearing %d gotFixResidualPercentDistanceTraveled %d\n", gotTimeFix, gotLatitudeFix, gotLongitudeFix, gotHorizontalPathLengthSinceLastFix, gotFixDistanceMadeGood, gotFixResidualDistance, gotFixResidualBearing, gotFixResidualPercentDistanceTraveled );
        if( verbosity_ > 0 ) logger_.syslog( got, Syslog::DEBUG );
    }
    else
    {
        logger_.syslog( "Could not read any data about the new fix.", Syslog::ERROR );
    }
}