/* -*- Mode:C++; c-file-style:"gnu"; indent-tabs-mode:nil; -*- */
/*
 * Copyright (c) 2017 Fraunhofer ESK
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Author: Karsten Roscher <karsten.roscher@esk.fraunhofer.de>
 */
#include "traci-roi-manager.h"

#include <algorithm>
#include <vector>

#include <ns3/assert.h>
#include <ns3/log.h>
#include <ns3/simulator.h>

NS_LOG_COMPONENT_DEFINE("TraCiRoiManager");

namespace ns3 {
namespace traci {

RoiManager::RoiManager () : m_roi (0, 0, 0, 0)
{
}

void
RoiManager::SetRegionOfInterest (const Region& roi)
{
  m_roi = roi;
}

bool
RoiManager::HasRegionOfInterest () const
{
  return IsValidRegion (m_roi);
}

void
RoiManager::AddVehicle (Ptr<Vehicle> vehicle)
{
  NS_ASSERT (vehicle);
  m_outsideRoi.emplace(vehicle->GetId (), ObservedVehicle (vehicle));
}

void
RoiManager::RemoveVehicle (std::string vehicleId)
{
  NS_LOG_FUNCTION (this << vehicleId);

  // remove from outside map
  m_outsideRoi.erase (vehicleId);

  // check if in inside map, remove and notify
  auto it = m_insideRoi.find (vehicleId);
  if (it != m_insideRoi.end ())
    {
      auto v = it->second.vehicle;
      m_insideRoi.erase (it);
      if (m_onLeave)
	{
	  NS_LOG_LOGIC ("Vehicle leaving ROI (removal): " << vehicleId);
	  m_onLeave (v);
	}
      else
	{
	  NS_LOG_DEBUG ("'leave' function not set");
	}
    }
}

void
RoiManager::RemoveVehicle (Ptr<Vehicle> vehicle)
{
  NS_ASSERT (vehicle);
  RemoveVehicle (vehicle->GetId ());
}

void
RoiManager::CheckForUpdates ()
{
  if (!IsValidRegion (m_roi))
    {
      UpdateWithoutRegion ();
    }
  else
    {
      UpdateWithRegion ();
    }
}

void
RoiManager::SetRoiEnterSignal (StateUpdateSignal onEnter)
{
  m_onEnter = onEnter;
}

void
RoiManager::SetRoiLeaveSignal (StateUpdateSignal onLeave)
{
  m_onLeave = onLeave;
}

void
RoiManager::UpdateWithoutRegion ()
{
  // simply add everything from the outside map
  if (m_onEnter)
  {
    for (auto const& v : m_outsideRoi)
      {
	m_onEnter (v.second.vehicle);
      }
  }
  else
  {
    NS_LOG_DEBUG ("'enter' function not set");
  }

  // copy contents
  m_insideRoi.insert (m_outsideRoi.begin (), m_outsideRoi.end ());

  // clear map
  m_outsideRoi.clear ();
}

void
RoiManager::UpdateWithRegion ()
{
  NS_ASSERT (IsValidRegion (m_roi));

  auto now = Simulator::Now ();

  std::vector<Ptr<Vehicle>> vehiclesEntered;
  std::vector<Ptr<Vehicle>> vehiclesLeft;

  NS_LOG_LOGIC ("Checking vehicles previously outside ROI");
  for (auto it = m_outsideRoi.begin (); it != m_outsideRoi.end (); )
    {
      if (now >= it->second.nextCheck)
	{
	  auto const& vehicle = it->second.vehicle;
	  auto pos = vehicle->GetPosition ();
	  if (m_roi.IsInside(pos))
	    {
	      NS_LOG_LOGIC ("Vehicle entered ROI: " << it->first);
	      vehiclesEntered.push_back (vehicle);
	      it = m_outsideRoi.erase (it);
	    }
	  else
	    {
	      NS_LOG_LOGIC ("Vehicle is still not in ROI: " << it->first);
	      // calculate distance to ROI
	      auto distanceToRoi = DistanceToRegion (pos, m_roi);
	      auto vMax = vehicle->GetMaxSpeed ();
	      double minTimeToRoi = distanceToRoi / vMax;

	      Time timeToCheck = now + Seconds (minTimeToRoi);

	      NS_LOG_LOGIC ("distanceToRoi=" << distanceToRoi
			    << ", vMax=" << vMax
			    << ", minTimeToRoi=" << minTimeToRoi
			    << ", timeToCheck=" << timeToCheck);

	      it->second.nextCheck = timeToCheck;
	      ++it;
	    }
	}
      else
	{
	  NS_LOG_LOGIC ("Skipping (not due): " << it->first);
	  ++it;
	}
    }

  NS_LOG_LOGIC ("Checking vehicles previously inside ROI");
  for (auto it = m_insideRoi.begin (); it != m_insideRoi.end (); )
    {
      auto const& vehicle = it->second.vehicle;
      auto pos = vehicle->GetPosition ();

      if (!m_roi.IsInside(pos))
	{
	  NS_LOG_LOGIC ("Vehicle left ROI: " << it->first);
	  vehiclesLeft.push_back (vehicle);
	  it = m_insideRoi.erase (it);
	}
      else
	{
	  NS_LOG_LOGIC ("Vehicle is still in ROI: " << it->first);
	  ++it;
	}
    }

  NS_LOG_DEBUG ("entered=" << vehiclesEntered.size () <<
		", left=" << vehiclesLeft.size ());

  // call signals and reenter the vehicles in their new destination maps

  for (auto const& v : vehiclesEntered)
    {
      if (m_onEnter)
	{
	  m_onEnter (v);
	}

      m_insideRoi.emplace (v->GetId (), ObservedVehicle (v));
    }

  for (auto const& v : vehiclesLeft)
    {
      if (m_onLeave)
	{
	  m_onLeave (v);
	}

      m_outsideRoi.emplace (v->GetId (), ObservedVehicle (v));
    }
}


RoiManager::ObservedVehicle::ObservedVehicle (Ptr<Vehicle> v)
  : vehicle (v), nextCheck (Simulator::Now ())
{
}

bool
IsValidRegion (const RoiManager::Region& region)
{
  return (region.xMin < region.xMax) && (region.yMin < region.yMax);
}

double
DistanceToRegion (const Vector& position, const RoiManager::Region& region)
{
  double dx = std::max(
      std::max<double>(region.xMin - position.x, 0),
      position.x - region.xMax);

  double dy = std::max(
      std::max<double>(region.yMin - position.y, 0),
      position.y - region.yMax);

  return std::sqrt(dx * dx + dy * dy);
}

}  // namespace traci
}  // namespace ns3