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

#include <math.h>
#include <stdlib.h> // rand

#include "SimACommsTarget.h"
#include "SimResultStruct.h"
#include "utils/AuvMath.h"
#include "utils/Datum.h"
#include "data/Location.h"


const float SimACommsTarget::ONEWAY_MODE_PING_INTERVAL( 0.5f );

SimACommsTarget::SimACommsTarget( int beaconAddress, double beaconLat, double beaconLon, double beaconDepth, Logger& logger )
    : address_( beaconAddress ),
      lat_( beaconLat ),
      lon_( beaconLon ),
      depth_( beaconDepth ),
      bcnN_( nanf( "" ) ),
      bcnE_( nanf( "" ) ),
      homingTat_( 0 )
{
    long error;
    bool northernHemi;
    unsigned int utmZone;
    error = Wgs84::LatLonToUtm( lat_, lon_, bcnN_, bcnE_, utmZone, northernHemi );
    if( error != 0 )
    {
        logger.syslog( "Error on conversion of position to UTM: ", Wgs84::UtmErrorToString( error ), Syslog::ERROR );
    }

    Str sysMsg = "Adding new SimAcommsTarget:";
    sysMsg += " address = " + Str( address_ );
    sysMsg += ", lat = " + Str( R2D( lat_ ), 5 ) + " deg";
    sysMsg += ", lon = " + Str( R2D( lon_ ), 5 ) + " deg";
    sysMsg += ", depth = " + Str( depth_, 2 ) + " m.";
    logger.syslog( sysMsg, Syslog::INFO );
}

SimACommsTarget::~SimACommsTarget()
{}

/// Returns homing data for this traget in the *interrogating* vehicle frame of reference
void SimACommsTarget::getHomingDataInVehicleFrame( const SimResultStruct& local, float& range, float& azimuth, float& elevation, bool useGeographic )
{
    range = getSlantRange( local, useGeographic );
    if( isnan( range ) || range <= 0.0f )
        return;

    float local_az, local_el;
    // Compute NED bearing/inclination angles (rad)
    float bearing = getBearing( local, useGeographic );
    float inclination = getInclination( local, range );

    // Assemble the vehicle-frame (FSK) to nav-frame (NED) Euler transformation matrix
    Point6D zzzrph( 0, 0, 0, local.roll_, local.pitch_, AuvMath::ModPi( local.heading_ ) );
    Matrix3x3 rotationMat = Matrix3x3( zzzrph ); // init as a rotation from FSK to NED...
    rotationMat.transpose(); // ...now transpose to get rotation from NED to FSK

    // Set to 0 here so subsequent addProduct calls just do a dot product
    Point3D directionNED = 0.0;
    Point3D directionFSK = 0.0;

    // Assemble the NED direction vector
    AuvMath::AzimuthAndElevationToUnitVector( bearing, inclination, directionNED );
    // Now rotate the NED direction vector to FSK
    directionFSK.addProduct( rotationMat, directionNED );
    // Decompose azimuth/elevation from the direction vector
    AuvMath::UnitVectorToAzimuthAndElevation( directionFSK, local_az, local_el );
    //AuvMath::UnitVectorToAzimuthAndElevation( directionFSK, azimuth, elevation );
    if( range <= 2.0f || fabs( local_az ) > M_PI / 2.0f )
    {
        //Simulate the bearing dropping out, but range persisting.
        return;
    }
    else
    {
        azimuth = local_az;
        elevation = local_el;
    }

    // Finally, make sure we're within bounds
    AuvMath::NormalizeAngle( elevation, ( float ) - M_PI / 2, ( float ) M_PI / 2 );
    AuvMath::NormalizeAngle( azimuth, ( float ) - M_PI, ( float ) M_PI );
}

/// Returns the slant range to this traget
float SimACommsTarget::getSlantRange( const SimResultStruct& local, bool useGeographic )
{
    float hrange( nanf( "" ) );
    float slant( nanf( "" ) );
    double latitude  = D2R( local.latitudeDeg_ );
    double longitude = D2R( local.longitudeDeg_ );

    if( useGeographic )
    {
        hrange  = Location::GetDistance( latitude, longitude, lat_, lon_ );
    }
    else
    {
        hrange = sqrt( pow( ( bcnN_ - local.posX_ ), 2. ) + pow( ( bcnE_ - local.posY_ ), 2. ) );
    }

    // Planar trig applies within AC sensor range.  Vector from vehicle to beacon.
    slant = sqrt( pow( hrange, 2. ) + pow( ( depth_ - local.depth_ ), 2. ) );

    // Add noise in range...
    slant += ( float )( rand() - RAND_MAX / 2 ) / ( float ) RAND_MAX * 100 * slant / 1000;
    // ...and make sure we didn't break the trig that follows
    if( slant < fabs( depth_ - local.depth_ ) || slant < fabs( hrange ) )
    {
        slant = sqrt( pow( hrange, 2. ) + pow( ( depth_ - local.depth_ ), 2. ) );
    }

    return slant;
}

/// Returns bearing to this traget in the navigation frame
float SimACommsTarget::getBearing( const SimResultStruct& local, bool useGeographic )
{
    float bearing   = nanf( "" );
    double latitude  = D2R( local.latitudeDeg_ );
    double longitude = D2R( local.longitudeDeg_ );

    if( useGeographic )
    {
        bearing = Location::GetBearing( latitude, longitude, lat_, lon_ );
    }
    else
    {
        bearing = atan2( bcnE_ - local.posY_, bcnN_ - local.posX_ );
    }
    // Add noise to bearing
    bearing += ( float )( rand() - RAND_MAX / 2 ) / ( float ) RAND_MAX * .1;
    return AuvMath::NormalizeAngle( bearing );
}

/// Returns Inclination to this traget in the navigation frame
float SimACommsTarget::getInclination( const SimResultStruct& local, float slantRange )
{
    float inclination = nanf( "" );
    // Planar trig again. DAT says Inclination is negative when the beacon is below the vehicle.
    if( slantRange != 0. ) inclination = asin( -1.0 * ( depth_ - local.depth_ ) / slantRange );
    // Add noise to inclination
    inclination += ( float )( rand() - RAND_MAX / 2 ) / ( float ) RAND_MAX * .17;
    return AuvMath::NormalizeAngle( inclination, ( float ) - M_PI / 2, ( float ) M_PI / 2 );
}

/// "Sends" an interrogation signal to get homing data for this traget. Projects TAT for return time.
void SimACommsTarget::ping( const SimResultStruct& local, int numPings, int msgID, Timespan& tatDelay, float speedOfSound )
{
    txTime_ = Timestamp::Now();

    // Compute range to determine the TAT
    float range = getSlantRange( local );

    if( isnan( range ) || range <= 0 || range > 5000.0f || speedOfSound <= 0 )
        return;

    homingTat_ = Timespan::Seconds( 2.0f * range / speedOfSound + tatDelay.asFloat() );
    numPingsRequested_ = numPings;
    numPingsSent_ = 0;
    msgID_ = msgID;
}

/// "Returns" homing data in response to interrogation signal
int SimACommsTarget::respond( const SimResultStruct& local, int& source, float& range, float& azimuth, float& elevation, int& msgID, Timestamp& rxTime, bool useGeographic )
{
    numPingsSent_ += 1;
    getHomingDataInVehicleFrame( local, range, azimuth, elevation, useGeographic );
    rxTime = txTime_ + homingTat_;
    source = address_;
    msgID = msgID_;

    // Add half a second to the homing TAT to simulate DAT returns in USBL mode.
    homingTat_ += Timespan::Seconds( ONEWAY_MODE_PING_INTERVAL );

    if( numPingsSent_ >= numPingsRequested_ )
    {
        // This is the last response. Reset TX time.
        txTime_ = Timestamp::NOT_SET_TIME;
    }
    return numPingsSent_;
}

/// True when response homing data is "received"
bool SimACommsTarget::responseReady()
{
    return txTime_ != Timestamp::NOT_SET_TIME
           && txTime_.elapsed() >= homingTat_;
}