/*
 * 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.
 *
 */

#ifdef __GAZEBO

#include <cassert>

#include "controlModule/LoopControlIF.h"

#include "WorldStatHandler.h"

/// Constructor
WorldStatHandler::WorldStatHandler( const Timestamp& initTime, Logger& logger )
    : initTime_( initTime ),
      latestSimT_( 0.0 ),
      nextIterSimT_( 0.0 ),
      waitForIter_( false ),
      initialized_( false ),
      logger_( logger ),
      debug_( false )
{
    initialize();
}

/// Destructor
WorldStatHandler::~WorldStatHandler()
{
    if( initialized_ ) uninitialize();
}

void WorldStatHandler::uninitialize()
{
    node_.Unsubscribe( "/stats" );
    initialized_ = false;
    iterSimReached_.notify_all();
}

void WorldStatHandler::initialize()
{
    logger_.syslog( "Initializing WorldStatHandler.", Syslog::INFO );

    if( !node_.Subscribe( "/stats", &WorldStatHandler::worldStatCallback, this ) )
    {
        logger_.syslog( "Error subscribing to Gazebo topic [/stats].", Syslog::ERROR );
        return;
    }

    initialized_ = true;
}

double WorldStatHandler::getInitTimeAsDouble( void )
{
    return initTime_.asDouble();
}

void WorldStatHandler::handleGazeboSync( const Timestamp& nextIterTime )
{
    std::unique_lock<std::mutex> lock( this->iterSimMutex_ );
    this->nextIterSimT_ = nextIterTime;
    waitForSync( lock );
}

void WorldStatHandler::waitForSync( std::unique_lock<std::mutex>& lock )
{
    if( !initialized_ )
    {
        logger_.syslog( "WorldStatHandler::handler() called before initialize().", Syslog::ERROR );
        return;
    }

    if( debug_ )
    {
        logger_.syslog( "Waiting for Gazebo time sync:"
                        " latest Gz time: " + Str( this->latestSimT_.asDouble() ) + " s,"
                        " next control iter: " + Str( this->nextIterSimT_.asDouble() ) + " s,"
                        " wait time: " + Str( ( this->nextIterSimT_ - this->latestSimT_ ).asDouble() ) + " s", Syslog::INFO );
    }

    // Block here while waiting for the Gazebo simulation time to catch up with the
    // time of the next iteration
    assert( !this->waitForIter_ );
    this->waitForIter_ = true;
    this->iterSimReached_.wait( lock, [this]()
    {
        return !this->waitForIter_ || !this->initialized_;
    } );
}

void WorldStatHandler::worldStatCallback( const gz::msgs::WorldStatistics& msg )
{
    if( msg.paused() )
        return;

    // Extract latest Gazebo clock...
    struct timeval latestSimTimeval;
    latestSimTimeval.tv_sec = msg.sim_time().sec();
    latestSimTimeval.tv_usec = msg.sim_time().nsec() / 1000; // nanosec to microsec
    // ... and convert to wall time
    this->latestSimT_ = this->initTime_ + Timespan( latestSimTimeval );

    //printf( "Gz time: %ld s, %d ns.\n", msg.sim_time().sec(), msg.sim_time().nsec() );
    //printf( "LRAUV Gz time: %lf s (tval=%lf s).\n", this->latestSimT_.asDouble(), Timespan( latestSimTimeval ).asDouble() );

    // Check if one full control loop iteration has elapsed
    if( this->waitForIter_ )
    {
        std::unique_lock<std::mutex> lock( this->iterSimMutex_ );

        if( this->latestSimT_ >= this->nextIterSimT_ )
        {
            Timestamp::SetNow( this->latestSimT_ );
            if( debug_ ) logger_.syslog( "Adjusting time to match Gazebo time: " + Str( this->latestSimT_.asDouble() ) + " s.", Syslog::INFO );
            this->waitForIter_ = false;
            lock.unlock();

            this->iterSimReached_.notify_all();
        }
    }
}

#endif /*__GAZEBO*/