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

#include <math.h>
#include <cstdlib>
#include <netdb.h>
#include <sys/ioctl.h>
#include <net/if.h>

#include "ExternalSim.h"

#include "data/Location.h"
#include "data/Matrix6x6.h"
#include "data/Point3D.h"
#include "data/Point6D.h"
#include "data/SimSlate.h"
#include "data/SimACommsDataHandler.h"
#include "data/SimACommsRangeHandler.h"
#include "data/Slate.h"
#include "data/ConfigReader.h"
#include "data/UniversalDataReader.h"
#include "simulatorModule/ParameterHandler.h"
#include "simulatorModule/SimCommsStruct.h"
#include "simulatorModule/SimulatorUtils.h"
#include "io/SocketException.h"
#include "supervisor/CommandExec.h"
#include "units/UnitRegistry.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
//#include "Tools/newmat-10D/newmatio.h"

// To get the relevant names for shared vars
#include "ExternalSimIF.h"
#include "controlModule/VerticalControlIF.h"
#include "controlModule/HorizontalControlIF.h"
#include "controlModule/SpeedControlIF.h"
#include "sensorModule/DropWeightIF.h"
#include "servoModule/BuoyancyServoIF.h"
#include "servoModule/ElevatorServoIF.h"
#include "servoModule/MassServoIF.h"
#include "servoModule/RudderServoIF.h"
#include "servoModule/ThrusterServoIF.h"
#include "SimulatorIF.h"

// Characteristics of introduced noise
const double ExternalSim::DEFAULT_AHRS_NOISE_AMPLITUDE = 0.00;
/// Default value for additive noise (in +/- meters)
const double ExternalSim::DEFAULT_DEPTH_NOISE_AMPLITUDE = 0.00;
// Constants used internal to the driver
//const double ExternalSim::GpsCutoffDepth_s = 0.25;

//=== Constructor/destructor ==
/// Constructor
ExternalSim::ExternalSim( const Module* module )
    : SyncSimulatorComponent( ExternalSimIF::NAME, module ),
      addr_( 0x0100007F ),  // inet addr:127.0.0.1
      pid_( 0x0 ),
      ahrsNoiseAmplitude_( DEFAULT_AHRS_NOISE_AMPLITUDE ),
      depthNoiseAmplitude_( DEFAULT_DEPTH_NOISE_AMPLITUDE ),
      altitude_( 0.0f ),
      buoyancyNeutralOffset_( 0.0f ),
      massPositionOffset_( 0.0f ),
      entrainedAir_( 0.0f ),
      bottomLockGone_( 100.0f ),
      beaconMap_(),
      homingSensorTatCfg_( 0.3f ),
      numberOfHomingTragetsCfg_( 3 ),
      useGeographic_( true ),
      oceanModelVarCount_( 0 ),
      oceanModelVarNames_( NULL ),
      oceanModelVarUnits_( NULL )
{

    simDaemonServerCfgReader_ = newConfigReader( ExternalSimIF::SIM_DAEMON_SERVER );

    // The following variables are outputs from the sim to the slate
    //
    // - rate is the velocity state vector (xdot, ydot, zdot, rolldot, pitchdot, yawdot)
    // - pos is the position state vector (x, y, z, roll, pitch, yaw)
    //    (both above can then be mediated through "sim sensors" i.e. "Sim compass" gives you yaw)
    //  (both above updated at end of motion)
    // - Actual thruster positions, prop speed

    latitudeWriter_ = newDataWriter( ExternalSimIF::LATITUDE_SIM );
    longitudeWriter_ = newDataWriter( ExternalSimIF::LONGITUDE_SIM );
    eastingWriter_ = newDataWriter( ExternalSimIF::EASTING_SIM );
    northingWriter_ = newDataWriter( ExternalSimIF::NORTHING_SIM );
    utmZoneWriter_ = newDataWriter( ExternalSimIF::UTM_ZONE_SIM );
    rateUWriter_ = newDataWriter( ExternalSimIF::RATE_U_SIM );
    rateVWriter_ = newDataWriter( ExternalSimIF::RATE_V_SIM );
    rateWWriter_ = newDataWriter( ExternalSimIF::RATE_W_SIM );
    ratePWriter_ = newDataWriter( ExternalSimIF::RATE_P_SIM );
    rateQWriter_ = newDataWriter( ExternalSimIF::RATE_Q_SIM );
    rateRWriter_ = newDataWriter( ExternalSimIF::RATE_R_SIM );
    propThrustWriter_ = newDataWriter( ExternalSimIF::PROP_THRUST_SIM );
    propTorqueWriter_ = newDataWriter( ExternalSimIF::PROP_TORQUE_SIM );
    netBuoyWriter_ = newDataWriter( ExternalSimIF::NET_BUOY_SIM );
    forceXWriter_ = newDataWriter( ExternalSimIF::FORCE_X_SIM );
    forceYWriter_ = newDataWriter( ExternalSimIF::FORCE_Y_SIM );
    forceZWriter_ = newDataWriter( ExternalSimIF::FORCE_Z_SIM );
    positionXWriter_ = newDataWriter( ExternalSimIF::POS_X_SIM );
    positionYWriter_ = newDataWriter( ExternalSimIF::POS_Y_SIM );
    positionZWriter_ = newDataWriter( ExternalSimIF::POS_Z_SIM );
    positionRollWriter_ = newDataWriter( ExternalSimIF::ROLL_SIM );
    positionPitchWriter_ = newDataWriter( ExternalSimIF::PITCH_SIM );
    positionHeadingWriter_ = newDataWriter( ExternalSimIF::HEADING_SIM );
    positionXDotWriter_ = newDataWriter( ExternalSimIF::POS_X_DOT_SIM );
    positionYDotWriter_ = newDataWriter( ExternalSimIF::POS_Y_DOT_SIM );
    positionZDotWriter_ = newDataWriter( ExternalSimIF::POS_Z_DOT_SIM );

    homingSensorAddressWriter_ = newDataWriter( ExternalSimIF::HOMING_SENSOR_ADDRESS_SIM );
    homingSensorRangeWriter_ = newDataWriter( ExternalSimIF::HOMING_SENSOR_RANGE_SIM );
    homingSensorAzimWriter_ = newDataWriter( ExternalSimIF::HOMING_SENSOR_AZIM_SIM );
    homingSensorElevWriter_ = newDataWriter( ExternalSimIF::HOMING_SENSOR_ELEV_SIM );

    // Ahrs slate readers and writers
    ahrsNoiseAmplitudeReader_ = newDataReader( ExternalSimIF::AHRS_NOISE_AMPLITUDE_SETTING ); //, this, Units::RADIAN( DEFAULT_AHRS_NOISE_AMPLITUDE ) );

    // Depth slate readers and writers
    depthNoiseAmplitudeReader_ = newDataReader( ExternalSimIF::DEPTH_NOISE_AMPLITUDE_SETTING ); //, this, Units::METER( DEFAULT_DEPTH_NOISE_AMPLITUDE ) );

    // GPS slate readers and writers

    // Tailcone slate readers and writers
    propOmegaActionReader_ = newDataReader( SpeedControlIF::PROP_OMEGA_ACTION ); //, this, Units::RADIAN_PER_SECOND( 0.0 ) );
    elevatorAngleActionReader_ = newDataReader( VerticalControlIF::ELEVATOR_ANGLE_ACTION ); //, this, Units::RADIAN( 0.0 ) );
    rudderAngleActionReader_ = newDataReader( HorizontalControlIF::RUDDER_ANGLE_ACTION ); //, this, Units::RADIAN( 0.0 ) );
    massPositionActionReader_ = newDataReader( VerticalControlIF::MASS_POSITION_ACTION ); //, this, Units::METER( 0.0 ) );
    buoyancyActionReader_ = newDataReader( VerticalControlIF::BUOYANCY_ACTION ); //, this, Units::CUBIC_CENTIMETER( 0.0 ) );
    dropWeightStateReader_ = newDataReader( DropWeightIF::DROP_WEIGHT_STATE );
    propOmegaReader_ = newDataReaderFromUniversal( ThrusterServoIF::NAME, UniversalURI::PLATFORM_PROPELLER_ROTATION_RATE ); //.cStr(), this, Units::RADIAN_PER_SECOND( 0.0 ) );
    elevatorAngleReader_ = newDataReaderFromUniversal( ElevatorServoIF::NAME, UniversalURI::PLATFORM_ELEVATOR_ANGLE ); //.cStr(), this, Units::RADIAN( 0.0 ) );
    rudderAngleReader_ = newDataReaderFromUniversal( RudderServoIF::NAME, UniversalURI::PLATFORM_RUDDER_ANGLE ); //.cStr(), this, Units::RADIAN( 0.0 ) );
    massPositionReader_ = newDataReaderFromUniversal( MassServoIF::NAME, UniversalURI::PLATFORM_MASS_POSITION ); //.cStr(), this, Units::METER( 0.0 ) );
    buoyancyPositionReader_ = newDataReaderFromUniversal( BuoyancyServoIF::NAME, UniversalURI::PLATFORM_BUOYANCY_POSITION ); //.cStr(), this, Units::CUBIC_CENTIMETER( nan( "" ) ) );

    altitudeReader_ = newUniversalReader( UniversalURI::HEIGHT_ABOVE_SEA_FLOOR );

    // Get the internet address of this machine.
    int numreqs = 30;
    struct ifconf ifc;
    int sd = socket( AF_INET, SOCK_STREAM, 0 );
    if( sd >= 0 )
    {
        ifc.ifc_buf = NULL;
        ifc.ifc_len = sizeof( struct ifreq ) * numreqs;
        ifc.ifc_buf = ( char* ) realloc( ifc.ifc_buf, ifc.ifc_len );
        if( ifc.ifc_buf )
        {
            struct ifreq* ifr = ifc.ifc_req;
            if( ioctl( sd, SIOCGIFCONF, &ifc ) >= 0 )
            {
                for( int n = 0; 0x0100007F == addr_ && n < ifc.ifc_len; n += sizeof( struct ifreq ) )
                {
                    struct in_addr *ia = ( struct in_addr * )( ( ifr->ifr_ifru.ifru_addr.sa_data ) + 2 );
                    addr_ = ia -> s_addr;
                    ifr++;
                }
            }
            free( ifc.ifc_buf );
        }
        close( sd );
    }

    // Get the process id of this process
    pid_ = getpid();

    // Uncomment for debugging output
    //debugLevel_ = Syslog::INFO;
}

/// Destructor
ExternalSim::~ExternalSim()
{
    delete[] oceanModelVarNames_;
    delete[] oceanModelVarUnits_;
}

/// Initialize
void ExternalSim::initialize( void )
{

    logger_.syslog( "ExternalSim initializing...", Syslog::INFO );

    memset( ( void* )&runParams_, 0, sizeof( runParams_ ) );

    Str remoteSimDaemonServer( simDaemonServerCfgReader_->asString( Units::NONE ) );

#ifndef __arm__ // only try local SimDaemon if you are not on the ARM target
    try
    {
        socketClient_ = new SocketClient( "localhost", SIMPORT );
    }
    catch( SocketException & se1 )
    {
        logger_.syslog( "Connection to local simulation server failed due to: ", se1.what(), Syslog::INFO );
        logger_.syslog( "Trying remote simulation server at " + remoteSimDaemonServer, Syslog::INFO );
#endif // #ifndef __arm__
        try
        {
            socketClient_ = new SocketClient( remoteSimDaemonServer.cStr(), SIMPORT );
        }
        catch( SocketException & se2 )
        {
            logger_.syslog( "Error making socket connection to simulation server at " + remoteSimDaemonServer + ": " + se2.what(), Syslog::ERROR );
            socketClient_ = NULL;
        }
#ifndef __arm__
    }
#endif //#ifndef __arm__

    memset( dataBuf_, 0, sizeof( dataBuf_ ) );

    SimulatorUtils::InitHead( runHead_, SimHeadStruct::SIM_RUN_STRUCT, addr_, pid_ );

    SimulatorUtils::InitHead( commsHead_, SimHeadStruct::SIM_COMMS_STRUCT, addr_, pid_ );

    SimHeadStruct initHead;
    SimulatorUtils::InitHead( initHead, SimHeadStruct::SIM_INIT_STRUCT, addr_, pid_ );

    SimInitStruct init;
    ok_ = SimulatorUtils::LoadInit( init, this, logger_ );

    Str oceanModelStr = ( char* )init.oceanModelData_;
    int oceanModelStrNParts = 0;
    Str* oceanModelStrParts = oceanModelStr.split( oceanModelStrNParts, ":" );
    if( oceanModelStrNParts > 2 )
    {
        oceanModelVarCount_ = ( oceanModelStrNParts - 1 ) / 2;
        oceanModelVarNames_ = new Str[oceanModelVarCount_];
        oceanModelVarUnits_ = new const Unit*[oceanModelVarCount_];
        for( int i = 0; i < oceanModelVarCount_; ++i )
        {
            oceanModelVarNames_[i] = oceanModelStrParts[i * 2 + 1];
            const Unit* unit = UnitRegistry::FindUnit( oceanModelStrParts[i * 2 + 2] );
            if( NULL == unit )
            {
                unit = &Units::NONE;
            }
            oceanModelVarUnits_[i] = unit;
        }
    }
    delete[] oceanModelStrParts;

    ok_ &= Slate::ReadOnce( SimulatorIF::BUOY_NEUTRAL_OFFSET_CFG, Units::CUBIC_METER, buoyancyNeutralOffset_, logger_ );
    ok_ &= Slate::ReadOnce( SimulatorIF::MASS_POSITION_OFFSET_CFG, Units::METER, massPositionOffset_, logger_ );
    ok_ &= Slate::ReadOnce( SimulatorIF::VEHICLE_ENTRAINED_AIR_CFG, Units::CUBIC_METER, entrainedAir_, logger_ );
    ok_ &= Slate::ReadOnce( SimulatorIF::DVL_BOTTOM_LOCK_GONE_CFG, Units::METER, bottomLockGone_, logger_ );
    ok_ &= Slate::ReadOnce( SimulatorIF::NUM_HOMING_TARGETS_CFG, Units::COUNT, numberOfHomingTragetsCfg_, logger_ );

    float HomingSensorTat;
    ok_ &= Slate::ReadOnce( SimulatorIF::HOMING_SENSOR_TAT_CFG, Units::SECOND, HomingSensorTat, logger_ );
    homingSensorTatCfg_ = Timespan::Seconds( HomingSensorTat );

    for( int i = 1; i <= numberOfHomingTragetsCfg_; ++i )
    {
        Str beaconName = "beacon" + Str( i );

        int beaconAddress;
        double beaconLat, beaconLon, beaconDepth;
        ok_ &= Slate::ReadOnce( "Config/workSite", beaconName + "Address", Units::COUNT, beaconAddress, logger_ );
        ok_ &= Slate::ReadOnce( "Config/workSite", beaconName + "Lat", Units::RADIAN, beaconLat, logger_ );
        ok_ &= Slate::ReadOnce( "Config/workSite", beaconName + "Lon", Units::RADIAN, beaconLon, logger_ );
        ok_ &= Slate::ReadOnce( "Config/workSite", beaconName + "Depth", Units::METER, beaconDepth, logger_ );

        if( ok_ )
        {
            SimACommsTarget* beacon = new SimACommsTarget( beaconAddress, beaconLat, beaconLon, beaconDepth, logger_ );
            beaconMap_.put( beaconAddress, beacon, true );
        }
    }

    if( ok_ )
    {
        //was simulator_->initialize( params, results, resultsSize );
        bool commsError = false;
        if( NULL != socketClient_ )
        {
            try
            {
                char params[ sizeof( initHead ) + sizeof( init )];
                memcpy( params, &initHead, sizeof( initHead ) );
                memcpy( params + sizeof( initHead ), &init, sizeof( init ) );
                socketClient_->send( params, sizeof( params ) );
                socketClient_->recv( ( char* )&results_, sizeof( results_ ) );
            }
            catch( SocketException& se )
            {
                logger_.syslog( "Simulator initialization communications error: " + Str( se.what() ), Syslog::ERROR );
                commsError = true;
            }
        }

        if( *results_.errorMessage_ )
        {
            ok_ = false;
            logger_.syslog( "Simulator initialization error: ", results_.errorMessage_, Syslog::ERROR );
        }
        else if( !commsError )
        {
            logger_.syslog( "Simulator initialized" );
            publishState( entrainedAir_ );
        }
    }
    else
    {
        logger_.syslog( "Unable to load simulator parameters from config files", Syslog::ERROR );
    }
}

/// Run
void ExternalSim::run( void )
{
    if( !ok_ )
    {
        return ;
    }

    // These are things that should be loaded from the slate...
    float depth;
    if( !SimSlate::Read( SimSlate::DEPTH_METER, depth ) )
    {
        depth = 0.0f;
    }
    float latitude;
    if( !SimSlate::Read( SimSlate::LATITUDE_DEGREE, latitude ) )
    {
        latitude = 0.0f;
    }
    else
    {
        latitude *= AuvMath::DEG_TO_RAD;
    }
    float pressure( AuvMath::OceanPressure( depth, latitude ) );
    //printf("At depth=%f, pressure=%f db\n", depth, pressure);
    float entrainedBuoyancy( entrainedAir_ / ( 1 + pressure / 101325.0f ) );
    //printf("entrained buoyancy=%f cc\n", entrainedBuoyancy * 1e6);

    propOmegaActionReader_->read( Units::RADIAN_PER_SECOND, runParams_.propOmegaAction_ );
    rudderAngleActionReader_->read( Units::RADIAN, runParams_.rudderAngleAction_ );
    elevatorAngleActionReader_->read( Units::RADIAN, runParams_.elevatorAngleAction_ );
    massPositionActionReader_->read( Units::METER, runParams_.massPositionAction_ );
    runParams_.massPositionAction_ -= massPositionOffset_;
    buoyancyActionReader_->read( Units::CUBIC_METER, runParams_.buoyancyAction_ );
    runParams_.buoyancyAction_ -= buoyancyNeutralOffset_ - entrainedBuoyancy;

    if( propOmegaReader_->wasTouchedSinceLastRun( this ) )
    {
        propOmegaReader_->read( Units::RADIAN_PER_SECOND, runParams_.propOmega_ );
    }
    else
    {
        runParams_.propOmega_ = nanf( "" );
    }

    if( rudderAngleReader_->wasTouchedSinceLastRun( this ) )
    {
        rudderAngleReader_->read( Units::RADIAN, runParams_.rudderAngle_ );
    }
    else
    {
        runParams_.rudderAngle_ = nanf( "" );
    }

    if( elevatorAngleReader_->wasTouchedSinceLastRun( this ) )
    {
        elevatorAngleReader_->read( Units::RADIAN, runParams_.elevatorAngle_ );
    }
    else
    {
        runParams_.elevatorAngle_ = nanf( "" );
    }

    if( massPositionReader_->wasTouchedSinceLastRun( this ) )
    {
        massPositionReader_->read( Units::METER, runParams_.massPosition_ );
        runParams_.massPosition_ -= massPositionOffset_;
    }
    else
    {
        runParams_.massPosition_ = nanf( "" );
    }

    if( buoyancyPositionReader_->wasTouchedSinceLastRun( this ) )
    {
        buoyancyPositionReader_->read( Units::CUBIC_METER, runParams_.buoyancyPosition_ );
        runParams_.buoyancyPosition_ -= buoyancyNeutralOffset_ - entrainedBuoyancy;
    }
    else
    {
        runParams_.buoyancyPosition_ = nanf( "" );
    }



    dropWeightStateReader_->read( Units::BOOL, runParams_.dropWeightState_ );
    runParams_.dt_ = dt_.asFloat();
    if( isnan( runParams_.dt_ ) || ( runParams_.dt_ <= 0 ) )
    {
        runParams_.dt_ = 0.4;
    }
    runParams_.time_ = timeOfRun_.asDouble();

    memcpy( dataBuf_, &runHead_, sizeof( runHead_ ) );
    memcpy( dataBuf_ + sizeof( runHead_ ), &runParams_, sizeof( runParams_ ) );
    //was simulator_->run( params, results, resultsSize );
    bool commsError = false;
    if( NULL != socketClient_ )
    {
        try
        {
            //DEBUG_LOG( "Normal send " + Str( sizeof( runHead_ ) + sizeof( runParams_ ) ) + " bytes" );
            socketClient_->send( dataBuf_, sizeof( runHead_ ) + sizeof( runParams_ ) );
            int recieved = socketClient_->recv( ( char* )&results_, sizeof( results_ ) );
            //DEBUG_LOG( "Normal recv " + Str( recieved ) + " bytes" );
        }
        catch( SocketException& se )
        {
            logger_.syslog( "Simulator run communications error: ", se.what(), Syslog::ERROR );
            commsError = true;
        }
    }

    if( *results_.errorMessage_ )
    {
        logger_.syslog( "Simulator run error: ", results_.errorMessage_, Syslog::ERROR );
        printf( "Quitting due to simulator run error: %s\n", results_.errorMessage_ );
        CommandExec::DoCommand( "quit", true );
    }
    else if( !commsError )
    {
        publishState( entrainedBuoyancy );
    }

    int destination;
    int source;
    Str message;
    float maxDistance;
    float delay;
    float mediumSpeed;
    if( SimACommsDataHandler::ReadOutgingMessage( destination, source, message, maxDistance, delay, mediumSpeed ) )
    {
        //DEBUG_LOG( "SimSlate outgoing message" );
        SimCommsStructToSim toSim( destination, source, message.length(), maxDistance, delay, mediumSpeed );
        //DEBUG_LOG( "SimSlate outgoing message memcpy head" );
        memcpy( dataBuf_, &commsHead_, sizeof( commsHead_ ) );
        //DEBUG_LOG( "SimSlate outgoing message mcpy struct" );
        memcpy( dataBuf_ + sizeof( commsHead_ ), &toSim, sizeof( SimCommsStructToSim ) );
        //DEBUG_LOG( "SimSlate outgoing message send " +  Str( sizeof( commsHead_ ) + sizeof( SimCommsStructToSim ) ) + " bytes" );
        socketClient_->send( dataBuf_, sizeof( commsHead_ ) + sizeof( SimCommsStructToSim ) );
        if( message.length() > 0 )
        {
            DEBUG_LOG( "SimSlate outgoing messageStr is " + message );
            socketClient_->send( message.cStr(), message.length() );
        }
        SimCommsStructFromSim fromSim;
        //DEBUG_LOG( "SimSlate incoming message recv" );
        unsigned int rcvBytes = socketClient_->recv( ( char* )&fromSim, sizeof( fromSim ) );
        //DEBUG_LOG( "SimSlate incoming message got " + Str( ( int )rcvBytes ) + " bytes" );
        if( fromSim.length_ > 0 )
        {
            //DEBUG_LOG( "SimSlate incoming message recv again" );
            unsigned int msgLen = socketClient_->recv( dataBuf_, AuvMath::Min( sizeof( dataBuf_ ), ( size_t )fromSim.length_ ) );
            message = Str( dataBuf_, msgLen );
            DEBUG_LOG( "SimSlate incoming messageStr setting to " + message );
            SimACommsDataHandler::WriteIncomingMessage( fromSim.destinationId_, fromSim.sourceId_, message );
        }
    }

    // And that's it
    setState( BLOCK_NORMAL );

}

/// Deinitialize
void ExternalSim::uninitialize( void )
{
    delete socketClient_;
    socketClient_ = NULL;
}

void ExternalSim::publishState( float entrainedBuoyancy )
{
    // Tailcone outputs
    SimSlate::Write( SimSlate::PROPELLER_OMEGA_RADIAN_PER_SECOND, results_.propOmega_ );
    SimSlate::Write( SimSlate::ELEVATOR_ANGLE_RADIAN, results_.elevatorAngle_ );
    SimSlate::Write( SimSlate::RUDDER_ANGLE_RADIAN, results_.rudderAngle_ );
    SimSlate::Write( SimSlate::MASS_POSITION_METER, results_.massPosition_ + massPositionOffset_ );
    SimSlate::Write( SimSlate::BUOYANCY_POSITION_CUBIC_METER, results_.buoyancyPosition_ + buoyancyNeutralOffset_ - entrainedBuoyancy );

    // AHRS outputs
    SimAhrsStruct ahrsResults;

    if( ahrsNoiseAmplitudeReader_->wasTouchedSinceLastRun( this ) )
    {
        ahrsNoiseAmplitudeReader_->read( Units::RADIAN, ahrsNoiseAmplitude_ );
    }

    ahrsResults.roll  = results_.roll_    + ( float )( rand() - RAND_MAX / 2 ) / ( float ) RAND_MAX * ahrsNoiseAmplitude_;
    ahrsResults.pitch = results_.pitch_   + ( float )( rand() - RAND_MAX / 2 ) / ( float ) RAND_MAX * ahrsNoiseAmplitude_;
    ahrsResults.yaw   = results_.heading_ + ( float )( rand() - RAND_MAX / 2 ) / ( float ) RAND_MAX * ahrsNoiseAmplitude_ ;

    // ROLL_RATE_RADIAN_PER_SECOND
    ahrsResults.angularVelocity.setP( results_.rateP_ );
    // PITCH_RATE_RADIAN_PER_SECOND
    ahrsResults.angularVelocity.setQ( results_.rateQ_ );
    // YAW_RATE_RADIAN_PER_SECOND
    ahrsResults.angularVelocity.setR( results_.rateR_ );

    // Publish AHRS results
    SimSlate::Write( ahrsResults );

    // Depth sensor
    if( depthNoiseAmplitudeReader_->wasTouchedSinceLastRun( this ) )
    {
        depthNoiseAmplitudeReader_->read( Units::METER, depthNoiseAmplitude_ );
    }
    // coverity[secure_coding] // indicate that the code below is indeed safe
    float depth( results_.depth_  + ( float )( rand() - RAND_MAX / 2 ) / ( float ) RAND_MAX * depthNoiseAmplitude_ );
    SimSlate::Write( SimSlate::DEPTH_METER, depth );

    // GPS outputs
    SimSlate::Write( SimSlate::LATITUDE_DEGREE, results_.latitudeDeg_ );
    SimSlate::Write( SimSlate::LONGITUDE_DEGREE, results_.longitudeDeg_ );
    if( !isnan( results_.latitudeDeg_ ) )
    {
        latitudeWriter_->write( Units::DEGREE, results_.latitudeDeg_ );
        longitudeWriter_->write( Units::DEGREE, results_.longitudeDeg_ );
        const double lat( D2R( results_.latitudeDeg_ ) );
        const double lon( D2R( results_.longitudeDeg_ ) );
        double northing;
        double easting;
        unsigned int zone;
        bool northernHemi;
        if( 0 == Wgs84::LatLonToUtm( lat, lon, northing, easting, zone, northernHemi ) )
        {
            eastingWriter_->write( Units::METER, easting );
            northingWriter_->write( Units::METER, northing );
            int sZone = ( northernHemi ? 1 : -1 ) * zone;
            utmZoneWriter_->write( Units::ENUM, sZone );
        }
    }

    if( !isnan( results_.forceX_ ) )
    {
        propThrustWriter_->write( Units::NEWTON, results_.propThrust_ );
        propTorqueWriter_->write( Units::NEWTON_METER, results_.propTorque_ );
        netBuoyWriter_->write( Units::NEWTON, results_.netBuoy_ );
        forceXWriter_->write( Units::NEWTON, results_.forceX_ );
        forceYWriter_->write( Units::NEWTON, results_.forceY_ );
        forceZWriter_->write( Units::NEWTON, results_.forceZ_ );
    }

    // Vector outputs
    if( !isnan( results_.posX_ ) )
    {
        positionXWriter_->write( Units::METER, results_.posX_ );
        positionYWriter_->write( Units::METER, results_.posY_ );
        positionZWriter_->write( Units::METER, results_.posZ_ );
        positionRollWriter_->write( Units::RADIAN, results_.posRoll_ );
        positionPitchWriter_->write( Units::RADIAN, results_.posPitch_ );
        positionHeadingWriter_->write( Units::RADIAN, results_.posHeading_ );
        positionXDotWriter_->write( Units::METER_PER_SECOND, results_.posXDot_ );
        positionYDotWriter_->write( Units::METER_PER_SECOND, results_.posYDot_ );
        positionZDotWriter_->write( Units::METER_PER_SECOND, results_.posZDot_ );
        //utmZoneWriter_->write( Units::ENUM, results_.utmZone_ );
        //northernHemiWriter_->write( Units::BOOL, results_.northernHemi_ );
    }

    if( !isnan( results_.rateU_ ) )
    {
        rateUWriter_->write( Units::METER_PER_SECOND, results_.rateU_ );
        rateVWriter_->write( Units::METER_PER_SECOND, results_.rateV_ );
        rateWWriter_->write( Units::METER_PER_SECOND, results_.rateW_ );
        ratePWriter_->write( Units::RADIAN_PER_SECOND, results_.rateP_ );
        rateQWriter_->write( Units::RADIAN_PER_SECOND, results_.rateQ_ );
        rateRWriter_->write( Units::RADIAN_PER_SECOND, results_.rateR_ );

        SimSlate::Write( SimSlate::NORTHWARD_WATER_VELOCITY_METER_PER_SECOND, results_.northCurrent_ );
        SimSlate::Write( SimSlate::EASTWARD_WATER_VELOCITY_METER_PER_SECOND, results_.eastCurrent_ );

        SimDvlStruct dvlResults;
        dvlResults.valid = true;
        dvlResults.timestamp = Timestamp::Now();

        dvlResults.waterMassVelocityWrtSensor.setX( results_.rateU_ );
        dvlResults.waterMassVelocityWrtSensor.setY( results_.rateV_ );
        dvlResults.waterMassVelocityWrtSensor.setZ( results_.rateW_ );

        //
        // NED coordinates are Earth-fixed, North-East-Down.  "B" is the body
        // fixed coordinate system, x ahead, y to starboard, z down.
        //
        double rateNorthWrtoGnd = results_.posXDot_;
        double rateEastWrtoGnd  = results_.posYDot_;
        double rateVertWrtoGnd  = results_.posZDot_;
        Point3D velWrtoGndinNED( rateNorthWrtoGnd, rateEastWrtoGnd, rateVertWrtoGnd );
        Point6D orientation( 0, 0, 0, results_.roll_, results_.pitch_, results_.heading_ );
        Matrix3x3 xformEarth2Body( orientation );
        xformEarth2Body.transpose();
        Point3D velWrtoGndInB( 0.0 );
        velWrtoGndInB.addProduct( xformEarth2Body, velWrtoGndinNED );

        // Check to see if we're more than 100m off the bottom.
        // If so, simulate a loss of bottom lock
        altitude_ = -1.0;
        // Get navigation charts altitude
        if( !SimSlate::Read( SimSlate::ALTITUDE_METER, altitude_ ) )
        {
            // No altitude coming in from the navigation charts, so fill with invalid value
            SimSlate::Write( SimSlate::ALTITUDE_METER, altitude_ );
        }
        dvlResults.bottomRange = altitude_;
        for( int i = 0; i < 4; ++i )
            dvlResults.beamRanges[i] = altitude_;

        if( altitude_ >= 0.0 && altitude_ < bottomLockGone_ )
        {
            dvlResults.velocityWrtBottom.setX( velWrtoGndInB.getX() );
            dvlResults.velocityWrtBottom.setY( velWrtoGndInB.getY() );
            dvlResults.velocityWrtBottom.setZ( velWrtoGndInB.getZ() );
        }
        else
        {
            dvlResults.velocityWrtBottom = -1.0;
            dvlResults.valid = false;
        }

        SimSlate::Write( dvlResults );
    }

    // (byraanan)TODO: deprecate this SimSlate::LBL_TRANSPONDER interface
    if( !isnan( results_.latitudeDeg_ ) )
    {
        float pingDist[numberOfHomingTragetsCfg_];
        for( int i = 0; i < numberOfHomingTragetsCfg_; ++i )
        {
            pingDist[i] = nanf( "" );
            SimACommsTarget* beacon = beaconMap_.getIndexed( i );

            if( beacon != NULL )
            {
                pingDist[i] = beacon->getSlantRange( results_, useGeographic_ );
            }
        }
        SimSlate::Write( SimSlate::LBL_TRANSPONDER_1_RANGE_M, pingDist[0] );
        SimSlate::Write( SimSlate::LBL_TRANSPONDER_2_RANGE_M, pingDist[1] );
        SimSlate::Write( SimSlate::LBL_TRANSPONDER_3_RANGE_M, pingDist[2] );
    }


    int destination, source, numPings, msgID;
    while( SimACommsRangeHandler::ReadOutgoingMessage( destination, source, numPings, msgID ) )
    {
        SimACommsTarget* beacon = beaconMap_.get( destination );
        if( beacon != NULL )
        {
            // Ping the beacon
            beacon->ping( results_, numPings, msgID, homingSensorTatCfg_ );
        }
    }

    for( int i = 0; i < numberOfHomingTragetsCfg_; ++i )
    {
        SimACommsTarget* beacon = beaconMap_.getIndexed( i );

        if( beacon != NULL
                && beacon->responseReady() )
        {
            Timestamp rxTime;
            int address, msgID;
            float range( nanf( "" ) ), azimuth( nanf( "" ) ), elevation( nanf( "" ) );
            // Get the range query response. RX time includes travel time and TAT delay.
            int pingNum = beacon->respond( results_, address, range, azimuth, elevation, msgID, rxTime, useGeographic_ );

            // Publish results to the sim AComms handler
            if( SimACommsRangeHandler::WriteIncomingMessage( address, range, azimuth, elevation, rxTime, msgID ) )
            {
                // Publish results to SimSlate. (byraanan)TODO: deprecate SimSlate interface
                SimSlate::Write( SimSlate::HOMING_SENSOR_RANGE_M, range );
                SimSlate::Write( SimSlate::HOMING_SENSOR_AZIM_RAD, azimuth );
                SimSlate::Write( SimSlate::HOMING_SENSOR_ELEV_RAD, elevation );
                // Log results to the Slate
                homingSensorAddressWriter_->write( Units::COUNT, address, rxTime );
                homingSensorRangeWriter_->write( Units::METER, range, rxTime );
                homingSensorAzimWriter_ ->write( Units::RADIAN, azimuth, rxTime );
                homingSensorElevWriter_ ->write( Units::RADIAN, elevation, rxTime );
            }
        }
    }

    // Science outputs
    SimSlate::Write( SimSlate::DENSITY_KILOGRAM_PER_CUBIC_METER, results_.density_ );
    SimSlate::Write( SimSlate::SALINITY_PART_PER_THOUSAND, results_.salinity_ );
    SimSlate::Write( SimSlate::TEMPERATURE_DEGREE_CELSIUS, results_.temperature_ );
    float foo = ( ( results_.salinity_ - 32.65 ) * 4 + ( results_.temperature_ - 6.5 ) * 0.5
                  + results_.eastCurrent_ * results_.eastCurrent_ * 700.0f
                  - results_.depth_ / 20.0f - 6.0f ) * 2.0f;
    SimSlate::Write( SimSlate::MASS_CONCENTRATION_OF_CHLOROPHYLL_UG_PER_L, foo );
    SimSlate::Write( SimSlate::MASS_CONCENTRATION_OF_PETROLEUM_HYDROCARBON_KG_PER_CUBIC_METER, foo * 1e5 );
    SimSlate::Write( SimSlate::MASS_CONCENTRATION_OF_OXYGEN_UG_PER_L, foo * 100 + 5000 );
    SimSlate::Write( SimSlate::MOLE_CONCENTRATION_OF_NITRATE_UMOLE_PER_L, foo );
    SimSlate::Write( SimSlate::MAGNETIC_VARIATION_DEGREE, results_.magneticVariation_ );
    SimSlate::Write( SimSlate::SOUND_SPEED_METER_PER_SECOND, results_.soundSpeed_ );

    for( int i = 0; i < oceanModelVarCount_; ++i )
    {
        //printf("Writing %s=%g %s\n", oceanModelVarNames_[i].cStr(),results_.values_[i],oceanModelVarUnits_[i]->getName());
        SimSlate::WriteScience( oceanModelVarNames_[i], *oceanModelVarUnits_[i], results_.values_[i] );
    }

    SimBatteryStruct batteryResults;
    batteryResults.batteryVoltage = results_.batteryVoltage_;
    batteryResults.batteryCurrent = results_.batteryCurrent_;
    batteryResults.batteryCharge = results_.batteryCharge_;
    batteryResults.batteryPercentage = results_.batteryPercentage_;
    SimSlate::Write( batteryResults );
}