#include <shared_workspace/Snapshotting.h>

Snapshotting::Snapshotting(ros::NodeHandle& nh, gpu_voxels::GpuVoxelsSharedPtr gvl, boost::shared_ptr<Config> config)
  : m_snapshot_service_server(nh.advertiseService("make_snapshot", &Snapshotting::triggerCB, this))
  , m_config(config)
  , m_gvl(gvl)
{
  gvl->addMap(MT_BITVECTOR_VOXELLIST, "SnapshotVoxellist");
  m_snapshot_voxellist = boost::dynamic_pointer_cast<voxellist::BitVectorVoxelList, gpu_voxels::GpuVoxelsMap>(gvl->getMap("SnapshotVoxellist"));
  if (m_config->getDynRec()->visualize_maps)
  {
    m_gvl->visualizeMap("SnapshotVoxellist");
  }

  m_make_snapshot_trigger = false;
  m_recording_voxels_for_snapshot = false;
  m_snapshot_iterations_counter = 0;
  m_iterations_for_snapshot = 20;
}

Snapshotting::~Snapshotting()
{
}

bool Snapshotting::triggerCB(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
{
  m_make_snapshot_trigger = true;
  res.success = true;
  res.message = "Starting to make snapshot of the live_environment_voxellist.";
  return true;
}

void Snapshotting::handle(boost::shared_ptr<gpu_voxels::voxellist::CountingVoxelList> env)
{
  if (m_make_snapshot_trigger)
  {
    m_snapshot_voxellist->clearMap();

    ROS_INFO_STREAM("Starting to take a snapshot of the live environment.");

    m_snapshot_iterations_counter = 0;
    m_recording_voxels_for_snapshot = true;

    m_make_snapshot_trigger = false;
  }

  if (m_recording_voxels_for_snapshot)
  {
    const BitVoxelMeaning bvm = eBVM_OCCUPIED;

    m_snapshot_voxellist->merge(env, Vector3f(), &bvm);
    m_snapshot_iterations_counter++;

    if (m_snapshot_iterations_counter >= m_iterations_for_snapshot)
    {
      ROS_INFO_STREAM("Finished taking a snapshot of the live environment. Snapshot has " << m_snapshot_voxellist->getDimensions().x << " voxels");
      m_recording_voxels_for_snapshot = false;
    }
  }

  if (!m_recording_voxels_for_snapshot)
  {
    if (m_config->getDynRec()->visualize_maps)
    {
      m_gvl->visualizeMap("SnapshotVoxellist");
    }
    env->subtractFromCountingVoxelList(m_snapshot_voxellist.get(), Vector3f());
  }
}