/*
 * Copyright (C) 2021 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#ifndef WORLDSTATHANDLER_H_
#define WORLDSTATHANDLER_H_

#ifdef __GAZEBO
#include <atomic>
#include <condition_variable>
#include <mutex>

#include <gz/msgs/world_stats.pb.h>
#include <gz/transport/Node.hh>

#include "logger/Logger.h"
#include "utils/Timestamp.h"

class WorldStatHandler
{

public:
    /// Constructor
    WorldStatHandler( const Timestamp& initTime, Logger& logger );

    /// Destructor
    ~WorldStatHandler();

    /// Initialize the handler
    void initialize( void );

    /// Uninitialize the handler
    void uninitialize( void );

    /// Get the real time offset of initialization
    double getInitTimeAsDouble( void );

    /// Handle time synchronization of control loop and Gazebo clock in each
    /// control iteration
    void handleGazeboSync( const Timestamp& nextIterTime );

    /// Callback function for Gazebo world statistics
    /// \param[in] msg Stats message
    void worldStatCallback( const gz::msgs::WorldStatistics& msg );

private:
    /// Blocks execution while waiting for time synchronization callback
    void waitForSync( std::unique_lock<std::mutex>& lock );

    /// Time of intialization. Real time. This will be a constant offset that sim
    /// time gets added to, because clock cannot be set to 0 to match sim time.
    Timestamp initTime_;

    /// Latest sim time from Gazebo world statistics. It is an elapsed time.
    Timestamp latestSimT_;

    /// The expected time that the next control loop iteration will start. This
    /// keeps track of Gazebo sim time. It is an elapsed time.
    Timestamp nextIterSimT_;

    std::mutex iterSimMutex_;

    std::condition_variable iterSimReached_;

    std::atomic<bool> waitForIter_;

    bool initialized_;

    gz::transport::Node node_;

    Logger& logger_;

    bool debug_;

};

#endif /*__GAZEBO*/

#endif /*WORLDSTATHANDLER_H_*/