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

#include "TrackAcousticContact.h"
#include "TrackAcousticContactIF.h"

#include <math.h>    /* atan2 and asin */
#include <algorithm>    // std::nth_element
#include <vector>       // std::vector
#include <numeric>      // std::accumulate
#include <cmath>        // std::pow

#include "sensorModule/DATIF.h"
#include "sensorModule/DUSBL_HydroidIF.h"

#include "data/Location.h"
#include "data/Slate.h"
#include "data/SimSlate.h"
#include "data/UniversalDataReader.h"
#include "data/BlobReader.h"
#include "data/BlobWriter.h"

#include "units/Units.h"

#include "utils/AuvMath.h"

TrackAcousticContact::TrackAcousticContact( const Str& prefix, const Module* module )
    : Behavior( prefix, module, true, true, TrackAcousticContactIF::ENABLE_BROADCAST ),
      contactLabelSetting_( 0 ),
      numberOfSamplesSetting_( 1 ),
      numFixesLowPassSetting_( 3 ),
      numStartingFixesToIgnoreSetting_( 1 ),
      contactLabel_( 0 ),
      contactUpdateCount_( 0 ),
      rangeToContact_( nan( "" ) ),
      contactLatitude_( nan( "" ) ),
      contactLongitude_( nan( "" ) ),
      contactDepth_( nan( "" ) ),
      contactRxTime_( Timestamp::NOT_SET_TIME ),
      contactLatitudeLowPass_( nan( "" ) ),
      contactLongitudeLowPass_( nan( "" ) ),
      contactDepthLowPass_( nan( "" ) ),
      contactBearingLowPass_( nanf( "" ) ),
      contactHorizontalRangeLowPass_( nanf( "" ) ),
      rotationFSK2NED_(),
      directionFSK_( 0.0 ),
      relPosFSK_( 0.0 ),
      azimuthFSK_( nan( "" ) ),
      elevationFSK_( nan( "" ) ),
      directionNED_( 0.0 ),
      relPosNED_( 0.0 ),
      bearingNED_( nan( "" ) ),
      inclinationNED_( nan( "" ) ),
      contactDepthSetting_( nan( "" ) ),
      contactLatBin_( 5 ),
      contactLonBin_( 5 ),
      contactDepthBin_( 5 ),
      windowOverlap_( true ),
      queryTime_( Timestamp::NOT_SET_TIME ),
      updatePeriod_( Timespan( 20.0 ) ),
      verbosity_( 0 )
{

    // Mission inputs
    contactLabelSettingReader_ = newSettingReader( TrackAcousticContactIF::CONTACT_LABEL_SETTING );
    contactDepthSettingReader_ = newSettingReader( TrackAcousticContactIF::CONTACT_DEPTH_SETTING );
    numFixesLowPassSettingReader_ = newSettingReader( TrackAcousticContactIF::NUMBER_OF_FIXES_LOWPASS_SETTING );
    lowPassOverlapSettingReader_ = newSettingReader( TrackAcousticContactIF::LOWPASS_WINDOW_OVERLAP_SETTING );
    numSamplesReader_ = newSettingReader( TrackAcousticContactIF::NUMBER_OF_SAMPLES_SETTING );
    numStartingFixesToIgnoreSettingReader_ = newSettingReader( TrackAcousticContactIF::NUMBER_OF_STARTING_FIXES_TOIGNORE_SETTING );
    updatePeriodReader_ = newSettingReader( TrackAcousticContactIF::UPDATE_PERIOD_SETTING );

    // Universal inputs from the slate
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    vehicleOrientationReader_ = newUniversalBlobReader( UniversalURI::PLATFORM_ORIENTATION_MATRIX );

    contactAddressReader_ = newUniversalReader( UniversalURI::ACOUSTIC_CONTACT_ADDRESS_READING );
    rangeToContactReader_ = newUniversalReader( UniversalURI::ACOUSTIC_CONTACT_RANGE_READING );
    directionToContactReader_ = newUniversalBlobReader( UniversalURI::ACOUSTIC_CONTACT_DIRECTION_VF );

    // Slate outputs to DUSBL or DAT
    bool enabled( false );
    if( Slate::ReadOnce( DATIF::LOAD_AT_STARTUP, Units::BOOL, enabled, logger_ ) && enabled )
    {
        requestContactLabelWriter_ = newDataWriter( DATIF::QUERY_ADDRESS_REQUESTED );
        requestNumPingsWriter_ = newDataWriter( DATIF::NUMBER_OF_PINGS_REQUESTED );
    }
    else if( Slate::ReadOnce( DUSBL_HydroidIF::LOAD_AT_STARTUP, Units::BOOL, enabled, logger_ ) && enabled )
    {
        requestContactLabelWriter_ = newDataWriter( DUSBL_HydroidIF::TRANSPONDER_CODE_REQUESTED );
        requestNumPingsWriter_ = newDataWriter( DUSBL_HydroidIF::PING_REQUESTED );
    }
    else
    {
        logger_.syslog( "No USBL component is enabled. Check vehicle configuration.", Syslog::CRITICAL );
    }

    // Outputs to the slate
    contactLabelWriter_ = newDataWriter( TrackAcousticContactIF::CONTACT_LABEL );
    contactLatitudeWriter_ = newDataWriter( TrackAcousticContactIF::CONTACT_LATITUDE );
    contactLongitudeWriter_ = newDataWriter( TrackAcousticContactIF::CONTACT_LONGITUDE );
    eastingsToContactWriter_ = newDataWriter( TrackAcousticContactIF::EASTINGS_TO_CONTACT );
    northingsToContactWriter_ = newDataWriter( TrackAcousticContactIF::NORTHINGS_TO_CONTACT );
    contactDepthWriter_ = newDataWriter( TrackAcousticContactIF::CONTACT_DEPTH );
    rangeToContactWriter_ = newDataWriter( TrackAcousticContactIF::RANGE_TO_CONTACT );
    azimuthToContactWriter_ = newDataWriter( TrackAcousticContactIF::AZIMUTH_TO_CONTACT_VF );
    elevationToContactWriter_ = newDataWriter( TrackAcousticContactIF::ELEVATION_TO_CONTACT_VF );
    headingToContactWriter_ = newDataWriter( TrackAcousticContactIF::HEADING_TO_CONTACT );
    vDirectionWriter_ = newBlobWriter( TrackAcousticContactIF::DIRECTION_VF );
    nDirectionWriter_ = newBlobWriter( TrackAcousticContactIF::DIRECTION_NF );
    vRelPosWriter_ = newBlobWriter( TrackAcousticContactIF::RELATIVE_POSITION_VF );
    nRelPosWriter_ = newBlobWriter( TrackAcousticContactIF::RELATIVE_POSITION_NF );

    rangeToContactLowPassWriter_ = newDataWriter( TrackAcousticContactIF::CONTACT_HORIZONTAL_RANGE_LOWPASS );
    headingToContactLowPassWriter_ = newDataWriter( TrackAcousticContactIF::CONTACT_HEADING_LOWPASS );
    contactLatitudeLowPassWriter_ = newDataWriter( TrackAcousticContactIF::CONTACT_LATITUDE_LOWPASS );
    contactLongitudeLowPassWriter_ = newDataWriter( TrackAcousticContactIF::CONTACT_LONGITUDE_LOWPASS );
    contactDepthLowPassWriter_ = newDataWriter( TrackAcousticContactIF::CONTACT_DEPTH_LOWPASS );
}


TrackAcousticContact::~TrackAcousticContact()
{}


void TrackAcousticContact::initialize( void )
{
    logger_.syslog( "Initializing TrackAcousticContact.", Syslog::INFO );
    requestVehiclePose( true );
    requestAcousticData( true );
    queryTime_ = Timestamp::Now(); // Start the clock so we know when to sample next
}


void TrackAcousticContact::uninitialize( void )
{
    logger_.syslog( "Uninitializing TrackAcousticContact.", Syslog::DEBUG );
    requestVehiclePose( false );
    requestAcousticData( false );
}


void TrackAcousticContact::requestVehiclePose( bool req )
{
    latitudeReader_->requestData( req );
    longitudeReader_->requestData( req );
    depthReader_->requestData( req );
    vehicleOrientationReader_->requestData( req );
}


void TrackAcousticContact::requestAcousticData( bool req )
{
    contactAddressReader_->requestData( req );
    rangeToContactReader_->requestData( req );
    directionToContactReader_->requestData( req );
}


void TrackAcousticContact::readSettings( void )
{
    if( contactLabelSettingReader_->read( Units::COUNT, contactLabelSetting_ )
            && ( contactLabelSetting_ >= 0 ) && ( contactLabelSetting_ < 255 ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Tracking contact label: ", contactLabelSetting_, Syslog::INFO );
    }
    else
    {
        logger_.syslog( "Received invalid contact label: ", contactLabelSetting_, Syslog::CRITICAL );
        contactLabelSetting_ = 0;
    }

    if( !contactDepthSettingReader_->read( Units::METER, contactDepthSetting_ ) )
    {
        contactDepthSetting_ = nan( "" );
    }

    if( !numSamplesReader_->read( Units::COUNT, numberOfSamplesSetting_ ) )
    {
        numberOfSamplesSetting_ = 1;
    }
    if( numberOfSamplesSetting_ > 120 )
    {
        logger_.syslog( "Received request " + Str( numberOfSamplesSetting_ ) + " consecutive samples, reducing to 120.", Syslog::INFO );
        numberOfSamplesSetting_ = 120;
    }

    if( !numStartingFixesToIgnoreSettingReader_->read( Units::COUNT, numStartingFixesToIgnoreSetting_ ) )
    {
        numStartingFixesToIgnoreSetting_ = 1;
    }

    if( numFixesLowPassSettingReader_->read( Units::COUNT, numFixesLowPassSetting_ ) )
    {
        size_t binSize = ( size_t )numFixesLowPassSetting_;
        contactLatBin_.resize( binSize );
        contactLonBin_.resize( binSize );
        contactDepthBin_.resize( binSize );
    }

    lowPassOverlapSettingReader_->read( windowOverlap_ );

    double update_period;
    updatePeriodReader_->read( Units::SECOND, update_period );
    if( update_period < 0 )
    {
        updatePeriod_ = Timespan( 0 );
    }
    else if( update_period > 600 )
    {
        updatePeriod_ = Timespan( 600 );
        logger_.syslog( "Received request for long update period " + Str( update_period / 60 ) + " minutes, decreasing to 5 minutes.", Syslog::INFO );
    }
    else
    {
        updatePeriod_ = Timespan( update_period );
    }

}


void TrackAcousticContact::run( void )
{
    //if( verbosity_ > 0 ) logger_.syslog( "Tracking...", Syslog::DEBUG );

    // Read in mission settings
    readSettings();

    if( rangeToContactReader_->isActive() && rangeToContactReader_->wasTouchedSinceLastRun( this )
            && directionToContactReader_->isActive() && directionToContactReader_->wasTouchedSinceLastRun( this ) )
    {
        if( readAcousticData() )
        {
            double vlat( nan( "" ) ), vlon( nan( "" ) ), vdepth( nan( "" ) );
            if( readVehicleParams( vlat, vlon, vdepth ) )
            {
                processAcousticData( vlat, vlon, vdepth );
                publishData();
            }
        }
    }
    else if( queryTime_.elapsed() > updatePeriod_ )
    {
        if( verbosity_ > 0 ) logger_.syslog( "update period (" + Str( updatePeriod_.asDouble() ) + " s) has elapsed, querying...", Syslog::INFO );
        queryAcousticContact();
    }
    else
    {
        if( verbosity_ > 0 ) logger_.syslog( "waiting for update period (" + Str( updatePeriod_.asDouble() ) + " s) to elapse", Syslog::INFO );
    }

}


bool TrackAcousticContact::readAcousticData()
{
    bool ok( true );
    ok &= contactAddressReader_->read( Units::COUNT, contactLabel_ );
    ok &= rangeToContactReader_->read( Units::METER, rangeToContact_ );
    ok &= directionToContactReader_->read1DClass( Units::NONE, directionFSK_ );
    if( ok ) contactRxTime_ = rangeToContactReader_->getTimestamp();

    if( ok && ( contactLabel_ != contactLabelSetting_ ) )
    {
        logger_.syslog( "DAT did not provide matching contact label.", Syslog::INFO );
        ok = false;
    }

    if( verbosity_ > 2 )
    {
        logger_.syslog( "direction FSK: " + directionFSK_.toString(), Syslog::INFO );
    }

    return ok;
}

bool TrackAcousticContact::readVehicleParams( double &lat, double &lon, double &depth )
{
    if( !latitudeReader_->read( Units::RADIAN, lat ) || isnan( lat )
            || !longitudeReader_->read( Units::RADIAN, lon ) || isnan( lon )
            || !depthReader_->read( Units::METER, depth ) || isnan( depth ) )
    {
        if( verbosity_ > 0 ) logger_.syslog( "Failed to read valid vehicle position.", Syslog::DEBUG );
        // Don't try to set the writers invalid, because they aren't universals.
        return false;
    }

    return true;
}

/**
 * Transform observations into North-East-Down coordinate frame, and
 * (optionally) average several observations.
 */
void TrackAcousticContact::processAcousticData( double vlat, double vlon, double vdepth )
{
    if( verbosity_ > 0 ) logger_.syslog( "Processing response from acoustic contact.", Syslog::DEBUG );

    // Reset direction vectors to 0 so subsequent calls to addProduct
    // only perform a dot product.
    directionNED_ = 0;
    relPosNED_ = 0;

    // Read rotation matrix from FSK to NED
    vehicleOrientationReader_->read2DClass( Units::NONE, rotationFSK2NED_ );

    double deltaDepth( nan( "" ) );
    if( ( directionFSK_.getZ() == 0.0 ) && !isnan( contactDepthSetting_ ) ) // If the elevation angle is ingored (i.e., deemed zero) and the acoustic contact's depth is preknown and fixed (e.g., a surface vessel).
    {
        //                   * contact
        //                 / |
        // rangeToContact /  |  deltaDepth
        //         lrauv *___|
        //             2D range <- this is the range we want since we're ignoring the elevation angle.

        deltaDepth = vdepth - contactDepthSetting_;
        if( fabs( deltaDepth ) <= rangeToContact_ ) // should always be true, and yet...
        {
            // solve for 2D range
            rangeToContact_ = sqrt( pow( rangeToContact_, 2 ) - pow( deltaDepth, 2 ) );
            if( verbosity_ > 2 ) logger_.syslog( "rangeToContact 2D = " + Str( rangeToContact_ ) + " m", Syslog::INFO );
        }
    }

    // Compute the relative position offsets (meter) to the contact in FSK
    relPosFSK_ = directionFSK_ * rangeToContact_;
    // Now rotate the FSK direction and relative position to NED
    directionNED_.addProduct( rotationFSK2NED_, directionFSK_ );
    relPosNED_.addProduct( rotationFSK2NED_, relPosFSK_ );

    // Decompose azimuth and elevation (FSK) and bearing and inclination (NED)
    AuvMath::UnitVectorToAzimuthAndElevation( directionFSK_, azimuthFSK_, elevationFSK_ );
    AuvMath::UnitVectorToAzimuthAndElevation( directionNED_, bearingNED_, inclinationNED_ );

    // NED bearing should be geographic heading in [0, 2PI)
    AuvMath::NormalizeAngle( bearingNED_ );

    // Calculate the contact's absolute position of in geographic coordinates
    contactLatitude_  = Location::NorthingsDelta( vlat, relPosNED_.getX() );
    contactLongitude_ = Location::EastingsDelta( vlat, vlon, relPosNED_.getY() );
    contactDepth_     = vdepth + relPosNED_.getZ();
    contactUpdateCount_++;

    // Finally, deal with the filtered measurements
    smoothAcousticData( vlat, vlon );

    if( verbosity_ > 2 ) printDebug();
}

void TrackAcousticContact::smoothAcousticData( double vlat, double vlon )
{
    if( contactUpdateCount_ <= numStartingFixesToIgnoreSetting_ )
    {
        if( verbosity_ > 2 )
            logger_.syslog( "First numStartingFixesToIgnoreSetting_ hits rejected because the vehicle just left the surface, and the bearing estimate can be way wrong.", Syslog::INFO );
        return;
    }

    contactLatitudeLowPass_        = nanf( "" );
    contactLongitudeLowPass_       = nanf( "" );
    contactDepthLowPass_           = nanf( "" );
    contactHorizontalRangeLowPass_ = nanf( "" );
    contactBearingLowPass_         = nanf( "" );

    // Add most recent measurements
    contactLatBin_.add( contactLatitude_ );
    contactLonBin_.add( contactLongitude_ );
    contactDepthBin_.add( contactDepth_ );

    if( ( contactUpdateCount_ - numStartingFixesToIgnoreSetting_ ) >= ( numFixesLowPassSetting_ + 1 ) )
    {
        if( windowOverlap_ || ( int )contactLatBin_.count() == numFixesLowPassSetting_ )
        {
            // Apply median filter
            contactLatitudeLowPass_ = contactLatBin_.median();
            contactLongitudeLowPass_ = contactLonBin_.median();
            contactDepthLowPass_ = contactDepthBin_.median();
            // Output the filtered absolute positions in geographic coordinates
            contactBearingLowPass_ = Location::GetBearing( vlat, vlon, contactLatitudeLowPass_, contactLongitudeLowPass_ );
            contactBearingLowPass_ = AuvMath::NormalizeAngle( contactBearingLowPass_ );
            contactHorizontalRangeLowPass_ = Location::GetDistance( vlat, vlon, contactLatitudeLowPass_, contactLongitudeLowPass_ );
            return;
        }
    }
    else
    {
        if( verbosity_ > 0 ) logger_.syslog( "Still accumulating hits to do low-pass filtering (after rejecting starting hits). Will return contact position after NumberOfFixesLowPass_+1 hits have been received.", Syslog::DEBUG );
        return;
    }
}


void TrackAcousticContact::queryAcousticContact()
{
    logger_.syslog( "*** querying acoustic contact ***", Syslog::INFO );
    queryTime_ = Timestamp::Now();
    requestContactLabelWriter_->write( Units::COUNT, contactLabelSetting_, queryTime_ );
    requestNumPingsWriter_->write( Units::COUNT, numberOfSamplesSetting_, queryTime_ );
    requestAcousticData( true );
}

void TrackAcousticContact::publishData()
{
    // Outputs are logged with the most recent contact RX timestamp.
    if( contactRxTime_ != Timestamp::NOT_SET_TIME )
    {
        // Contact label, range, and heading
        contactLabelWriter_->write( Units::COUNT, contactLabel_, contactRxTime_ );
        rangeToContactWriter_->write( Units::METER, rangeToContact_, contactRxTime_ );
        headingToContactWriter_->write( Units::RADIAN, bearingNED_, contactRxTime_ );

        // Relative estimates in FSK
        vRelPosWriter_->write1DClass( Units::METER, relPosFSK_, contactRxTime_ );
        vDirectionWriter_->write1DClass( Units::NONE, directionFSK_, contactRxTime_ );
        azimuthToContactWriter_->write( Units::RADIAN, azimuthFSK_, contactRxTime_ );
        elevationToContactWriter_->write( Units::RADIAN, elevationFSK_, contactRxTime_ );

        // Relative estimates in NED
        nRelPosWriter_->write1DClass( Units::METER, relPosNED_, contactRxTime_ );
        nDirectionWriter_->write1DClass( Units::NONE, directionNED_, contactRxTime_ );

        // Don't bother writing inclination in geographic frame, rely on depth instead.
        eastingsToContactWriter_->write( Units::METER, relPosNED_.getY(), contactRxTime_ );
        northingsToContactWriter_->write( Units::METER, relPosNED_.getX(), contactRxTime_ );

        // Absolute estimates in geographic frame
        contactLatitudeWriter_->write( Units::RADIAN, contactLatitude_, contactRxTime_ );
        contactLongitudeWriter_->write( Units::RADIAN, contactLongitude_, contactRxTime_ );
        contactDepthWriter_->write( Units::METER, contactDepth_, contactRxTime_ );

        // Valid smoothed data
        if( !isnan( contactBearingLowPass_ ) )
        {
            // Written with latest RX timestamp.
            contactLatitudeLowPassWriter_->write( Units::RADIAN, contactLatitudeLowPass_, contactRxTime_ );
            contactLongitudeLowPassWriter_->write( Units::RADIAN, contactLongitudeLowPass_, contactRxTime_ );
            contactDepthLowPassWriter_->write( Units::METER, contactDepthLowPass_, contactRxTime_ );
            rangeToContactLowPassWriter_->write( Units::METER, contactHorizontalRangeLowPass_, contactRxTime_ );
            headingToContactLowPassWriter_->write( Units::RADIAN, contactBearingLowPass_, contactRxTime_ );

            if( !windowOverlap_ )
            {
                // No window overlap. Clear temporal bin filters.
                contactLatBin_.clear();
                contactLonBin_.clear();
                contactDepthBin_.clear();
            }

        }

        contactRxTime_ = Timestamp::NOT_SET_TIME;
    }
}

void TrackAcousticContact::printDebug()
{
    logger_.syslog( "slant range: " + Str( rangeToContact_ ) + " m", Syslog::INFO );
    logger_.syslog( "rotation FSK->NED: " + rotationFSK2NED_.toString(), Syslog::INFO );
    logger_.syslog( "direction FSK: " + directionFSK_.toString(), Syslog::INFO );
    logger_.syslog( "direction NED: " + directionNED_.toString(), Syslog::INFO );
    logger_.syslog( "rel pos FSK:   " + relPosFSK_.toString(), Syslog::INFO );
    logger_.syslog( "rel pos NED:   " + relPosNED_.toString(), Syslog::INFO );
    logger_.syslog( "FSK: azumith=" + Str( R2D( azimuthFSK_ ) ) + " deg, elevation=" + Str( R2D( elevationFSK_ ) ) + " deg.", Syslog::INFO );
    logger_.syslog( "NED: bearing=" + Str( R2D( bearingNED_ ) ) + " deg, inclination=" + Str( R2D( inclinationNED_ ) ) + " deg.", Syslog::INFO );
    logger_.syslog( "NED: lat=" + Str( R2D( contactLatitude_ ) ) + " deg, lon=" + Str( R2D( contactLongitude_ ) ) + " deg, depth=" + Str( contactDepth_ ) + " m.", Syslog::INFO );
}

/// Mission Component factory interface
Behavior* TrackAcousticContact::CreateBehavior( const Str& prefix, const Module* module )
{
    return new TrackAcousticContact( prefix, module );
}