/** \file
 *
 *  Contains the ExternalSimGazebo class declaration.
 *
 *  ExternalSimGazebo.h should only be included by ExternalSimGazebo.cpp
 *  Other classes should include ExternalSimIF.h
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#ifndef EXTERNALSIMGAZEBO_H_
#define EXTERNALSIMGAZEBO_H_

#include "SimResultStruct.h"
#include "SimRunStruct.h"

#include "component/SyncComponent.h"
#include "data/Location.h"
#include "data/Matrix6x6.h"
#include "data/Point3D.h"
#include "data/Point6D.h"
#include "io/SocketClient.h"
#include "utils/AuvMath.h"
#include "utils/Datum.h"
#include "utils/Timestamp.h"

#ifdef __GAZEBO
#include <condition_variable>
#include <mutex>
#include <gz/msgs/imu.pb.h>
#include <gz/msgs/magnetometer.pb.h>
#include <gz/transport/Node.hh>
#include "lrauv_gazebo_plugins/dvl_velocity_tracking.pb.h"
#include "lrauv_gazebo_plugins/lrauv_state.pb.h"
#include "data/StrValue.h"

#include "ExternalSimGazeboUtils.h"
#endif

class ConfigReader;
class UniversalDataReader;

/**
 *  Interfaces via TCP/IP with a SimDaemon to provide dynamic
 *  and environmental simulation of the vehicle.
 *
 *  ExternalSimGazebo.h should only be included by ExternalSimGazebo.cpp
 *  Other classes should include ExternalSimIF.h
 *
 *  \ingroup modules_simulator
 */
class ExternalSimGazebo : public SyncSimulatorComponent
{

public:
    /// Constructor.
    ExternalSimGazebo( const Module* module );

    /// Destructor.
    virtual ~ExternalSimGazebo();

    /// Initialize the simulator
    virtual void initialize( void );

    /// Run the simulation
    virtual void run( void );

    /// Uninitialize the simulation
    virtual void uninitialize( void );

#ifdef __GAZEBO
    /// Callback function for Gazebo AHRS measurements
    /// \param[in] imuMessage IMU message
    /// \param[in] magMessage Magnetometer message
    void ahrsCallback(
        const gz::msgs::IMU& imuMessage,
        const gz::msgs::Magnetometer& magMessage );

    /// Callback function for Gazebo DVL velocity measurements
    /// \param[in] msg DVL velocity tracking message
    void dvlVelocityCallback(
        const lrauv_gazebo_plugins::msgs::DVLVelocityTracking &msg );

    /// Callback function for Gazebo state
    /// \param[in] msg State message
    void stateCallback( const lrauv_gazebo_plugins::msgs::LRAUVState& msg );
#endif

private:

    /// Indicates wheter the external sim is running
    bool ok_;

    // write State to Slate
    virtual void publishState( float entrainedBuoyancy );

    // configuration readers
    ConfigReader* simDaemonServerCfgReader_;

    //*------------------- slate variables -----------------------*/

    DataWriter* latitudeWriter_;
    DataWriter* longitudeWriter_;
    DataWriter* eastingWriter_;
    DataWriter* northingWriter_;
    DataWriter* utmZoneWriter_;
    DataWriter* propThrustWriter_;
    DataWriter* propTorqueWriter_;
    DataWriter* netBuoyWriter_;
    DataWriter* forceXWriter_;
    DataWriter* forceYWriter_;
    DataWriter* forceZWriter_;
    DataWriter* positionXWriter_;
    DataWriter* positionYWriter_;
    DataWriter* positionZWriter_;
    DataWriter* positionRollWriter_;
    DataWriter* positionPitchWriter_;
    DataWriter* positionHeadingWriter_;
    DataWriter* positionXDotWriter_;
    DataWriter* positionYDotWriter_;
    DataWriter* positionZDotWriter_;
    DataWriter* rateUWriter_;
    DataWriter* rateVWriter_;
    DataWriter* rateWWriter_;
    DataWriter* ratePWriter_;
    DataWriter* rateQWriter_;
    DataWriter* rateRWriter_;
    DataWriter* homingSensorRangeWriter_;
    DataWriter* homingSensorAzimWriter_;
    DataWriter* homingSensorElevWriter_;
    //DataWriter* utmZoneWriter_;
    //DataWriter* northernHemiWriter_;
#ifdef __GAZEBO
    DataWriter* timeGzWriter_;
    DataWriter* timeExtWriter_;
#endif
    //----------------------------------------------------------------
    // Depth specific parameters:
    // Default value for the depthadditive noise amplitude (in meters)
    static const double DEFAULT_DEPTH_NOISE_AMPLITUDE;
    double depthNoiseAmplitude_;
    // Slate inputs
    DataReader *depthNoiseAmplitudeReader_;

    //----------------------------------------------------------------
    // Tailcone specific parameters:
    // The inputs
    DataReader* propOmegaActionReader_;
    DataReader* elevatorAngleActionReader_;
    DataReader* rudderAngleActionReader_;
    DataReader* massPositionActionReader_;
    DataReader* buoyancyActionReader_;
    DataReader* dropWeightStateReader_;
    DataReader* propOmegaReader_;
    DataReader* elevatorAngleReader_;
    DataReader* rudderAngleReader_;
    DataReader* massPositionReader_;
    DataReader* buoyancyPositionReader_;

    UniversalDataReader* altitudeReader_;
    float altitude_;

    float buoyancyNeutralOffset_; // Value added to buoyancy in the ExternalSimGazebo to achieve "True" neutral
    float massPositionOffset_;    // Value added to mass position in the ExternalSimGazebo to achieve "True" zero pitch
    float entrainedAir_;          // Amount of entrained air in vehicle
    float bottomLockGone_;        // Altitude greater than this value results in loss of bottom lock simulated
    float bottomLockout_;         // Altitude less than this value results in loss of bottom reading simulated

    /// number of extra variables in ocean model data
    int oceanModelVarCount_;

    /// names of extra variables in ocean model data
    Str* oceanModelVarNames_;

    /// units of extra variables in ocean model data
    const Unit** oceanModelVarUnits_;

    double beaconLat_;
    double beaconLon_;
    double beaconDepth_;
    double bcnN_;
    double bcnE_;

    //----------------------------------------------------------------
    // Communications
    SimRunStruct runParams_;
    SimResultStruct results_;

#ifdef __GAZEBO
    //----------------------------------------------------------------
    // Gazebo simulation

    /// Vehicle name, read from config file. Used to prepend to Gazebo topic
    /// names as namespace.
    StrValue vehicleName_;

    /// Vehicle acoustic modem address
    int vehicleLocalAddress_;

    /// Topic on which simulation init message will be received
    Str initTopic_ = "/lrauv/init";

    /// Topic on which robot commands will be received
    Str commandTopic_ = "command_topic";

    /// Topic on which robot state will be published
    Str stateTopic_ = "state_topic";

    /// Topic on which robot AHRS's IMU measurements will be published
    Str ahrsImuTopic_ = "ahrs/imu";

    /// Topic on which robot AHRS' magnetometer measurements will be published
    Str ahrsMagTopic_ = "ahrs/magnetometer";

    /// Topic on which robot DVL velocity measurements will be published
    Str dvlVelocityTopic_ = "dvl/velocity";

    /// Transport node for message passing
    gz::transport::Node node_;

    /// Publisher of initialization
    gz::transport::Node::Publisher initPub_;

    /// Publisher of robot commands
    gz::transport::Node::Publisher commandPub_;

    /// Subscriber of robot AHRS measurements
    SynchronizedGazeboSubscriber ahrsSub_;

    /// Condition variable to simulate blocking read in original simulation with socket
    std::mutex resultsMutex_;
    std::condition_variable resultsUpdate_;

    /// Time of intialization. Real time. Used for validating synchronization
    /// with Gazebo sim time.
    Timestamp initTime_;

    /// Debug printout timer
    double prevCmdPrintTime_ = -1.0;
    double prevStatePrintTime_ = -1.0;

    const bool DEBUG_ = false;
#endif
};

#endif /*EXTERNALSIMGAZEBO_H_*/