/** \file
 *
 *  Contains the ExternalSimGazebo 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>

#ifdef __GAZEBO
#include <chrono>

#include <gz/math/Vector3.hh>
#include <gz/msgs/Utility.hh>

#include <gz/msgs/imu.pb.h>
#include <gz/msgs/magnetometer.pb.h>

#include "VehicleIF.h"

#include "lrauv_gazebo_plugins/lrauv_command.pb.h"
#include "lrauv_gazebo_plugins/lrauv_init.pb.h"
#include "lrauv_gazebo_plugins/lrauv_state.pb.h"

#include "ExternalSimGazebo.h"
#include "ExternalSimGazeboIF.h"
#include "ExternalSimGazeboUtils.h"
#endif

#include "data/Location.h"
#include "data/Matrix6x6.h"
#include "data/Point3D.h"
#include "data/Point6D.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/ConfigReader.h"
#include "data/UniversalDataReader.h"
#include "simulatorModule/SimCommsStruct.h"
#include "simulatorModule/SimulatorUtils.h"
#include "io/SocketException.h"
#include "process/Handler.h"
#include "supervisor/CommandLine.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"

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

//=== Constructor/destructor ==
/// Constructor
ExternalSimGazebo::ExternalSimGazebo( const Module* module )
    : SyncSimulatorComponent( ExternalSimGazeboIF::NAME, module ),
      depthNoiseAmplitude_( DEFAULT_DEPTH_NOISE_AMPLITUDE ),
      altitude_( 0.0f ),
      buoyancyNeutralOffset_( 0.0f ),
      massPositionOffset_( 0.0f ),
      entrainedAir_( 0.0f ),
      bottomLockGone_( 100.0f ),
      bottomLockout_( 0.2f ),
      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 );

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

#ifdef __GAZEBO
    timeGzWriter_ = newDataWriter( ExternalSimGazeboIF::TIME_GZ_SIM ),
    timeExtWriter_ = newDataWriter( ExternalSimGazeboIF::TIME_EXT_SIM ),
#endif

    // 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 );

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

#ifdef __GAZEBO
    initTime_ = Timestamp::Now();

    // Advertise the init topic ahead of time to give discovery time to connect to
    // subscribers before we publish.
    initPub_ = node_.Advertise<lrauv_gazebo_plugins::msgs::LRAUVInit>(
                   initTopic_.data() );
    if( !initPub_ )
    {
        logger_.syslog( "Error advertising Gazebo topic [" +
                        initTopic_ + "].", Syslog::ERROR );
    }
#endif
}

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

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

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

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

    SimInitStruct init;
    ok_  = SimulatorUtils::LoadInit( init, this, logger_ );
    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_ );

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

#ifdef __GAZEBO

    // Read vehicle name for Gazebo topic namespace
    ok_ &= Slate::ReadOnce( VehicleIF::NAME_CFG, vehicleName_, logger_ );

    // Read vehicle local address ID for Gazebo accoms topic namespace
    ok_ &= Slate::ReadOnce( VehicleIF::ID_CFG, Units::COUNT, vehicleLocalAddress_, logger_ );

    // Prepend vehicle name as namespaces to Gazebo topics
    stateTopic_ = vehicleName_.asString() + "/" + stateTopic_;
    ahrsImuTopic_ = vehicleName_.asString() + "/" + ahrsImuTopic_;
    ahrsMagTopic_ = vehicleName_.asString() + "/" + ahrsMagTopic_;
    dvlVelocityTopic_ = vehicleName_.asString() + "/" + dvlVelocityTopic_;
    commandTopic_ = vehicleName_.asString() + "/" + commandTopic_;

    // Initialize Gazebo transport
    // If want to be robust to Gazebo restarting, this needs to be
    // in run(), so that it reconnects to a new Gazebo instance. But there is
    // the additional overhead of resubscribing in each control iteration.
    if( !node_.Subscribe( stateTopic_.cStr(),
                          &ExternalSimGazebo::stateCallback, this ) )
    {
        logger_.syslog( "Error subscribing to Gazebo topic [" +
                        stateTopic_ + "].", Syslog::ERROR );
        return;
    }

    const size_t maxBufferSize = 10u;
    ahrsSub_ = SynchronizedGazeboSubscriber::Subscribe(
                   node_, ahrsImuTopic_.cStr(), ahrsMagTopic_.cStr(),
                   maxBufferSize, &ExternalSimGazebo::ahrsCallback, this );
    if( !ahrsSub_ )
    {
        logger_.syslog( "Error subscribing to Gazebo topics [" +
                        ahrsImuTopic_ + "] and [" + ahrsMagTopic_ + "].",
                        Syslog::ERROR );
        return;
    }

    if( !node_.Subscribe( dvlVelocityTopic_.cStr(),
                          &ExternalSimGazebo::dvlVelocityCallback, this ) )
    {
        logger_.syslog( "Error subscribing to Gazebo topic [" +
                        dvlVelocityTopic_ + "].", Syslog::ERROR );
        return;
    }

    commandPub_ = node_.Advertise<lrauv_gazebo_plugins::msgs::LRAUVCommand>(
                      commandTopic_.data() );
    if( !commandPub_ )
    {
        logger_.syslog( "Error advertising Gazebo topic [" +
                        commandTopic_ + "].", Syslog::ERROR );
    }

    lrauv_gazebo_plugins::msgs::LRAUVInit initMsg;
    initMsg.mutable_id_()->set_data( vehicleName_.asString().cStr() );
    initMsg.set_acommsaddress_( vehicleLocalAddress_ );
    initMsg.set_initlat_( init.initLat_ );
    initMsg.set_initlon_( init.initLon_ );
    initMsg.set_initz_( init.initZ_ );
    initMsg.set_initpitch_( init.initPitch_ );
    initMsg.set_initroll_( init.initRoll_ );
    initMsg.set_initheading_( init.initHeading_ );

    initPub_.Publish( initMsg );

    logger_.syslog( "Published init to Gazebo on topic [" + initTopic_ + "]:\n" +
                    initMsg.DebugString().c_str(), Syslog::INFO );

#endif

}

/// Run
void ExternalSimGazebo::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_ );


#ifdef __GAZEBO

    // Set everything except for the timestamp, which should be set as close to
    // Publish() as possible
    lrauv_gazebo_plugins::msgs::LRAUVCommand cmdMsg;

    cmdMsg.set_propomega_( runParams_.propOmega_ );
    cmdMsg.set_rudderangle_( runParams_.rudderAngle_ );
    cmdMsg.set_elevatorangle_( runParams_.elevatorAngle_ );
    cmdMsg.set_massposition_( runParams_.massPosition_ );
    cmdMsg.set_buoyancyposition_( runParams_.buoyancyPosition_ );
    cmdMsg.set_dropweightstate_( bool( runParams_.dropWeightState_ ) );

    cmdMsg.set_propomegaaction_( runParams_.propOmegaAction_ );
    cmdMsg.set_rudderangleaction_( runParams_.rudderAngleAction_ );
    cmdMsg.set_elevatorangleaction_( runParams_.elevatorAngleAction_ );
    cmdMsg.set_masspositionaction_( runParams_.massPositionAction_ );
    cmdMsg.set_buoyancyaction_( runParams_.buoyancyAction_ );
    // cmdMsg.set_density_( );

    // Take real current time, as opposed to iteration start time
    runParams_.time_ = Timestamp::Now().asDouble();
#endif

    // TODO: For FTRT, change dt_ based on dt from Gazebo header timestamps
    runParams_.dt_ = dt_.asFloat();
    if( isnan( runParams_.dt_ ) || ( runParams_.dt_ <= 0 ) )
    {
        runParams_.dt_ = 0.4;
    }

#ifdef __GAZEBO
    cmdMsg.set_dt_( runParams_.dt_ );

    // Convert to sim time
    //cmdMsg.set_time_( runParams_.time_ - worldStatHandler_.getInitTimeAsDouble() );
    //cmdMsg.set_time_( runParams_.time_ - Handler::getInitTimeAsDouble() );
    // TODO: Need to access initTime_ in Handler. Currently not using this
    // timestamp in Gazebo, but it is the only way to know if control loop
    // duration changes.
    cmdMsg.set_time_( runParams_.time_ );

    commandPub_.Publish( cmdMsg );
    // Print once a second
    /*
    if( DEBUG_ &&
        ( prevCmdPrintTime_ < 0 || cmdMsg.time_() - prevCmdPrintTime_ >= 1 ) )
    {
        logger_.syslog( "Published command to Gazebo (printed only once in a while):", Syslog::INFO );
        logger_.syslog( "  dropWeightState: ", cmdMsg.dropweightstate_(), Syslog::INFO );
        logger_.syslog( "  propOmegaAction: ", cmdMsg.propomegaaction_(), Syslog::INFO );
        logger_.syslog( "  rudderAngleAction: ", cmdMsg.rudderangleaction_(), Syslog::INFO );
        logger_.syslog( "  elevatorAngleAction: ", cmdMsg.elevatorangleaction_(), Syslog::INFO );
        logger_.syslog( "  massPositionAction: ", cmdMsg.masspositionaction_(), Syslog::INFO );
        logger_.syslog( "  buoyancyAction: ", cmdMsg.buoyancyaction_(), Syslog::INFO );
        logger_.syslog( "  dt: ", cmdMsg.dt_(), Syslog::INFO );
        logger_.syslog( "  time: ", cmdMsg.time_(), Syslog::INFO );
        logger_.syslog( "  (some fields omitted in printout)", Syslog::INFO );
        prevCmdPrintTime_ = cmdMsg.time_();
    }
    */

    // Mirror the blocking read behavior in original simulation
    {
        std::unique_lock<std::mutex> lock{resultsMutex_};
        const double lastTimeGz =
            ! isnan( results_.timeGz_ ) ? results_.timeGz_ : -1;
        auto predicate = [&]()
        {
            return results_.timeGz_ > lastTimeGz;
        };
        // Add a timeout so that it doesn't hang when user exits
        if( resultsUpdate_.wait_for( lock, std::chrono::seconds( 10 ), predicate ) )
        {
            // Write State data to Slate
            publishState( entrainedBuoyancy );
        }
    }
#endif

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

}

/// Deinitialize
void ExternalSimGazebo::uninitialize( void )
{
    logger_.syslog( "Uninitialize ExternalSimGazebo Component." );
}

void ExternalSimGazebo::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 );

    // 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 );

#ifdef __GAZEBO
    // Gazebo timestamp
    timeGzWriter_->write( Units::SECOND, results_.timeGz_ );
    // Write elapsed time since initialization
    timeExtWriter_->write( Units::SECOND, initTime_.elapsed().asDouble() );
#endif

    // 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_ );

        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_ );
        }
    }

    // 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 );
    // Take Gazebo data instead
    SimSlate::Write( SimSlate::MASS_CONCENTRATION_OF_CHLOROPHYLL_UG_PER_L, results_.values_[0] );
    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 );
}

#ifdef __GAZEBO
void ExternalSimGazebo::dvlVelocityCallback(
    const lrauv_gazebo_plugins::msgs::DVLVelocityTracking &msg )
{
    SimDvlStruct dvlResults;
    dvlResults.valid = true;
    dvlResults.timestamp = Timestamp(
                               msg.header().stamp().sec(),
                               msg.header().stamp().nsec() / 1000 );

    if( msg.has_target() )
    {
        using DVLTrackingTarget =
            lrauv_gazebo_plugins::msgs::DVLTrackingTarget;
        if( msg.target().type() == DVLTrackingTarget::DVL_TARGET_BOTTOM )
        {
            dvlResults.bottomRange = msg.target().range().mean();
            if( msg.has_velocity() )
            {
                // Change frame conventions from SFM to FSK
                dvlResults.velocityWrtBottom.setU( msg.velocity().mean().y() );
                dvlResults.velocityWrtBottom.setV( msg.velocity().mean().x() );
                dvlResults.velocityWrtBottom.setW( -msg.velocity().mean().z() );
            }
        }
        if( msg.beams_size() > 4 )
        {
            logger_.syslog( "More than 4 beams reported by incoming DVL data on [" +
                            dvlVelocityTopic_ + "]. Ignoring beams in excess.",
                            Syslog::ERROR );
        }
        for( int i = 0; i < std::min( msg.beams_size(), 4 ); ++i )
        {
            if( msg.beams( i ).locked() )
            {
                dvlResults.beamRanges[i] = msg.beams( i ).range().mean();
            }
        }
    }

    SimSlate::Write( dvlResults );
}

void ExternalSimGazebo::ahrsCallback(
    const gz::msgs::IMU &imuMessage,
    const gz::msgs::Magnetometer &magMessage )
{
    SimAhrsStruct ahrsResults;
    const gz::math::Vector3d orientation =
        gz::msgs::Convert( imuMessage.orientation() ).Euler();
    ahrsResults.roll = orientation.X();
    ahrsResults.pitch = orientation.Y();
    ahrsResults.yaw = orientation.Z();

    ahrsResults.angularVelocity.setP( imuMessage.angular_velocity().x() );
    ahrsResults.angularVelocity.setQ( imuMessage.angular_velocity().y() );
    ahrsResults.angularVelocity.setR( imuMessage.angular_velocity().z() );

    // Gazebo's IMU accelerometer is acceleration positive, whereas Sparton's
    // AHRS-M2 accelerometer is gravity positive. Flip acceleration sign to
    // accomodate this discrepancy. For further reference, check sections 1.4 and
    // 1.5 of NXP's AN5017 https://www.nxp.com/docs/en/application-note/AN5017.pdf
    ahrsResults.linearAcceleration.setU( -imuMessage.linear_acceleration().x() );
    ahrsResults.linearAcceleration.setV( -imuMessage.linear_acceleration().y() );
    ahrsResults.linearAcceleration.setW( -imuMessage.linear_acceleration().z() );

    constexpr double milliGaussPerTesla{1e7};
    const gz::math::Vector3d magneticField =
        gz::msgs::Convert( magMessage.field_tesla() ) *
        milliGaussPerTesla;
    ahrsResults.magneticField.setX( magneticField.X() );
    ahrsResults.magneticField.setY( magneticField.Y() );
    ahrsResults.magneticField.setZ( magneticField.Z() );

    SimSlate::Write( ahrsResults );
}

void ExternalSimGazebo::stateCallback( const lrauv_gazebo_plugins::msgs::LRAUVState& msg )
{
    // Print once a second
    if( DEBUG_ &&
            ( prevStatePrintTime_ < 0 ||
              msg.header().stamp().sec() - prevStatePrintTime_ >= 1 ) )
    {
        logger_.syslog( "Received state from Gazebo (printed only once in a while):", Syslog::INFO );
        logger_.syslog( "  header.stamp.sec: " + Str( int( msg.header().stamp().sec() ) ), Syslog::INFO );
        logger_.syslog( "  header.stamp.nsec: " + Str( int( msg.header().stamp().nsec() ) ), Syslog::INFO );

        /*
        logger_.syslog( "  propOmega: ", msg.propomega_(), Syslog::INFO );
        logger_.syslog( "  rudderAngle: ", msg.rudderangle_(), Syslog::INFO );
        logger_.syslog( "  elevatorAngle: ", msg.elevatorangle_(), Syslog::INFO );
        logger_.syslog( "  massPosition: ", msg.massposition_(), Syslog::INFO );
        logger_.syslog( "  buoyancyPosition: ", msg.buoyancyposition_(), Syslog::INFO );

        logger_.syslog( "  depth: ", msg.depth_(), Syslog::INFO );
        logger_.syslog( "  roll: ", msg.rph_().x(), Syslog::INFO );
        logger_.syslog( "  pitch: ", msg.rph_().y(), Syslog::INFO );
        logger_.syslog( "  heading: ", msg.rph_().z(), Syslog::INFO );
        logger_.syslog( "  speed: ", msg.speed_(), Syslog::INFO );
        //logger_.syslog( "  latitudeDeg: ", msg.latitudedeg_(), Syslog::INFO );
        //logger_.syslog( "  longitudeDeg: ", msg.longitudedeg_(), Syslog::INFO );

        logger_.syslog( "  pos: " + Str( msg.pos_().x() ) + ", " + Str( msg.pos_().y() ) + ", " + Str( msg.pos_().z() ), Syslog::INFO );
        logger_.syslog( "  posDot: " + Str( msg.posdot_().x() ) + ", " + Str( msg.posdot_().y() ) + ", " + Str( msg.posdot_().z() ), Syslog::INFO );
        */

        logger_.syslog( "  temperature: " + Str( msg.temperature_() ), Syslog::INFO );
        logger_.syslog( "  salinity: " + Str( msg.salinity_() ), Syslog::INFO );
        logger_.syslog( "  density: " + Str( msg.density_() ), Syslog::INFO );
        logger_.syslog( "  values[0]: " + Str( msg.values_( 0 ) ), Syslog::INFO );
        logger_.syslog( "  (some fields omitted in printout)", Syslog::INFO );
        prevStatePrintTime_ = msg.header().stamp().sec();
    }

    {
        std::lock_guard<std::mutex> lock{resultsMutex_};
        results_.errorPad_ = msg.errorpad_();
        results_.utmZone_ = msg.utmzone_();
        results_.northernHemi_ = msg.northernhemi_();

        results_.propOmega_ = msg.propomega_();
        results_.propThrust_ = msg.propthrust_();
        results_.propTorque_ = msg.proptorque_();
        results_.rudderAngle_ = msg.rudderangle_();
        results_.elevatorAngle_ = msg.elevatorangle_();
        results_.massPosition_ = msg.massposition_();
        results_.buoyancyPosition_ = msg.buoyancyposition_();

        results_.depth_ = msg.depth_();
        results_.roll_ = msg.rph_().x();
        results_.pitch_ = msg.rph_().y();
        results_.heading_ = msg.rph_().z();
        results_.speed_ = msg.speed_();

        results_.latitudeDeg_ = msg.latitudedeg_();
        results_.longitudeDeg_ = msg.longitudedeg_();
        results_.netBuoy_ = msg.netbuoy_();

        results_.forceX_ = msg.force_().x();
        results_.forceY_ = msg.force_().y();
        results_.forceZ_ = msg.force_().z();

        results_.posX_ = msg.pos_().x();
        results_.posY_ = msg.pos_().y();
        results_.posZ_ = msg.pos_().z();

        results_.posRoll_ = msg.posrph_().x();
        results_.posPitch_ = msg.posrph_().y();
        results_.posHeading_ = msg.posrph_().z();

        results_.posXDot_ = msg.posdot_().x();
        results_.posYDot_ = msg.posdot_().y();
        results_.posZDot_ = msg.posdot_().z();

        results_.rateU_ = msg.rateuvw_().x();
        results_.rateV_ = msg.rateuvw_().y();
        results_.rateW_ = msg.rateuvw_().z();
        results_.rateP_ = msg.rateuvw_().x();
        results_.rateQ_ = msg.rateuvw_().y();
        results_.rateR_ = msg.rateuvw_().z();

        results_.northCurrent_ = msg.northcurrent_();
        results_.eastCurrent_ = msg.eastcurrent_();
        results_.vertCurrent_ = msg.vertcurrent_();
        results_.magneticVariation_ = msg.magneticvariation_();
        results_.soundSpeed_ = msg.soundspeed_();
        results_.temperature_ = msg.temperature_();
        results_.salinity_ = msg.salinity_();
        results_.density_ = msg.density_();

        results_.batteryVoltage_ = msg.batteryvoltage_();
        results_.batteryCurrent_ = msg.batterycurrent_();
        results_.batteryCharge_ = msg.batterycharge_();
        results_.batteryPercentage_ = msg.batterypercentage_();

        // Copy the vector into an array
        std::copy( msg.values_().begin(), msg.values_().end(), results_.values_ );

        // Convert raw Gazebo Time, which is int8 seconds and int4 nanoseconds,
        // to double seconds (possible loss of precision), because 8-bit int is not
        // implemented in the Slate.
        std::chrono::steady_clock::duration stamp =
            std::chrono::seconds( msg.header().stamp().sec() ) +
            std::chrono::nanoseconds( msg.header().stamp().nsec() );
        results_.timeGz_ = std::chrono::duration<double>( stamp ).count();
    }
    resultsUpdate_.notify_all();
}
#endif