/**
  * MainHandler for TetraRadio
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, SLSE, 2016-04-14
  */

#include "position_handler.h"
#include "exm_handler.h"
#include "logger.h"
#include <AssetManagement/asm_dtc.h>

using namespace TrainingAreaBase;

PositionHandler::PositionHandler(MainHandler *mainHandler)
  : QObject()
  , m_mainHandler(mainHandler)
  , m_logger(getLogger("PositionHandler"))
{}

bool PositionHandler::init()
{
  m_dtcAsm = AssetManagement::Interface::Dtc::create(m_mainHandler->ddsHandler()->ddsConnector());
  return m_dtcAsm->enableTrainingArea();
}

bool PositionHandler::initCoordinateConversion() const
{
  if (m_dtcAsm == nullptr)
    return true;

  if (m_mainHandler->exerciseId() == 0)
    return true;

  constexpr int ddsTimeout = 10 * 1000; // 10s
  const auto ex = m_mainHandler->exmHandler()->exercise(m_mainHandler->exerciseId());
  if (ex.id() <= 0)
  {
    ERROR_LOG(m_logger) << "Can not retrieve exercise information for id " << m_mainHandler->exerciseId();
    return false;
  }
  if (!m_dtcAsm->waitForHistoricalDataTrainingArea(ddsTimeout))
  {
    ERROR_LOG(m_logger) << "Error initializing AssetManagement DTC";
    return false;
  }
  const auto ta = m_dtcAsm->trainingArea(ex.trainingAreaId());
  if (ta.id() <= 0)
  {
    ERROR_LOG(m_logger) << "Can not retrieve training are information for id " << ex.trainingAreaId();
    return false;
  }
  // no longer needed
  m_dtcAsm.reset();

  // calc tetra base point from training area center
  const LtsCoordTrans::UtmCoordinate bottomLeft(LtsCoordTrans::UtmCoordinate::fromGeographic(ta.bottomLeft().lon(), ta.bottomLeft().lat()));
  const QPointF mid(bottomLeft.geographic(ta.width() / 2, ta.height() / 2));

  // our center point is NNxYY 00000 00000 by definition
  // (we can also use current ta center but the it's hard to debug the coordinates in Tetra LogViewer)
  QString mgrs = LtsCoordTrans::mgrsFromGeographic(mid.x(), mid.y(), -1);
  mgrs += QStringLiteral("00000000000000");

  // now we've the correct tetra base point
  m_utm2Geographic = LtsCoordTrans::UtmCoordinate::fromMgrs(mgrs);

  LtsCoordTrans::UtmCoordinate topRight(bottomLeft);
  topRight.x += ta.width();
  topRight.y += ta.height();

  m_relPosBottomLeft = bottomLeft.relativeTo(m_utm2Geographic);
  m_relPosTopRight = topRight.relativeTo(m_utm2Geographic);

  NOTICE_LOG(m_logger) << "Training area bottomLeft: " << m_relPosBottomLeft.x() << "," << m_relPosBottomLeft.y()
                      << ", topRight: " << m_relPosTopRight.x() << "," << m_relPosTopRight.y()
                      << ", base point: " <<  mgrs
                      << ", width:" << ta.width()
                      << ", height: " <<  ta.height();

  return true;
}

Core::GeoPosition2D PositionHandler::relativeToGeographic2D(const TetraIcd::RelativePosition &p) const
{
  initCoordinateConversion();

  Core::GeoPosition2D ret;
  if (!isPositionMarkedInvalid(p))
  {
    // the relative position is relative to the base point
    const QPointF point(m_utm2Geographic.geographic(p.offsetX, p.offsetY));
    ret.lon(point.x());
    ret.lat(point.y());
  }
  else
  {
    ret.lon(nan(""));
    ret.lat(nan(""));
  }
  return ret;
}

Core::GeoPosition PositionHandler::relativeToGeographic(const TetraIcd::RelativePosition &p) const
{
  const Core::GeoPosition2D pos2d(relativeToGeographic2D(p));
  Core::GeoPosition ret;
  ret.lon(pos2d.lon());
  ret.lat(pos2d.lat());
  ret.z(p.offsetZ);
  DEBUG_LOG(m_logger) << "x/y -> lon/lat: " << p.offsetX << "," << p.offsetY
                      << " -> " << ret.lon() * LtsCoordTrans::rad2deg() << "," << ret.lat() * LtsCoordTrans::rad2deg();
  return ret;
}

QPointF PositionHandler::geographicToRelativePointF(const Core::GeoPosition2D &p) const
{
  return geographicToRelativePointF(Core::GeoPosition(p.lon(), p.lat(), 0));
}

QPointF PositionHandler::geographicToRelativePointF(const Core::GeoPosition &p) const
{
  initCoordinateConversion();

  // convert lon/lat to utm with the given zone from the center point
  const LtsCoordTrans::UtmCoordinate point(LtsCoordTrans::UtmCoordinate::fromGeographic(p.lon(), p.lat(), m_utm2Geographic.zone));
  // now calc distance from point to base point == relative position
  return QPointF(point.x - m_utm2Geographic.x, point.y - m_utm2Geographic.y);
}

TetraIcd::RelativePosition PositionHandler::geographicToRelative(const Core::GeoPosition2D &p, double z) const
{
  return geographicToRelative(Core::GeoPosition(p.lon(), p.lat(), z));
}

TetraIcd::RelativePosition PositionHandler::geographicToRelative(const Core::GeoPosition &p) const
{
  const QPointF rel(geographicToRelativePointF(p));
  const TetraIcd::RelativePosition ret(qRound(rel.x()), qRound(rel.y()), p.z());
  DEBUG_LOG(m_logger) << "lon/lat -> x/y: " << p.lon() * LtsCoordTrans::rad2deg()  << "," << p.lat() * LtsCoordTrans::rad2deg()
                      << " -> " << ret.offsetX << "," << ret.offsetY;
  return ret;
}

const LtsCoordTrans::UtmCoordinate &PositionHandler::baseCoordinate() const
{
  initCoordinateConversion();

  return m_utm2Geographic;
}

double PositionHandler::speed (const TetraIcd::RelativePosition &relPos, const int equipNo, const qint64 timeStamp) const
{
  if (!m_lastPosition.contains(equipNo) )
    return 0.0;

  ParticipantPosition lastParticipantPosition = m_lastPosition.value(equipNo);
  qint64 deltaTime = timeStamp - lastParticipantPosition.time;
  if (deltaTime == 0)
    return 0.0;

  TetraIcd::RelativePosition lastRelPosition = geographicToRelative(lastParticipantPosition.pos.position());

  QLineF line(QPointF(lastRelPosition.offsetX, lastRelPosition.offsetY),
              QPointF(relPos.offsetX, relPos.offsetY));
  // v = s / t
  DEBUG_LOG(m_logger) << "POS alt=" << lastRelPosition.offsetX <<  "/" << lastRelPosition.offsetY
                      << " POS neu=" <<  relPos.offsetX << "/" << relPos.offsetY
                      << " distance= " << line.length()
                      << " timeStamp=" << timeStamp
                      << " lastParticipantPosition=" << lastParticipantPosition.time
                      << " deltaTime=" << deltaTime;


  double speed = line.length() / (deltaTime / 1000); // speed in [m/s]
  return speed;
}


bool PositionHandler::isTimeToPerformSpeedCheckExceeded(const int equipNo, const qint64 timeStamp, const int maxTime) const
{
  if (!m_lastPosition.contains(equipNo))
    return true;

  qint64 deltaTime =  timeStamp - m_lastPosition[equipNo].time;

  return (deltaTime > (maxTime * 1000));
}


TrainingAreaBase::TabPosition PositionHandler::lastPosition(int equipNo) const
{
  ParticipantPosition lastPos;
  TrainingAreaBase::TabPosition ret;
  if ( m_lastPosition.contains(equipNo))
  {
    lastPos = m_lastPosition.value(equipNo);
    return lastPos.pos;
  }
    ret.position(Core::GeoPosition(nan(""), nan(""), nan("")));
    return ret;
}

void PositionHandler::setLastPosition(int equipNo, const  TrainingAreaBase::TabPosition &pos)
{
  ParticipantPosition lastPos;
  if ( m_lastPosition.contains(equipNo))
    lastPos = m_lastPosition.value(equipNo);
  lastPos.pos = pos;
  m_lastPosition.insert (equipNo, lastPos);
}


void PositionHandler::setLastMountPointTypeId(int equipNo, const int lastMountPointTypeId)
{
  ParticipantPosition lastPos;
  if ( m_lastPosition.contains(equipNo))
    lastPos = m_lastPosition.value(equipNo);
  lastPos.mountPointTypeId = lastMountPointTypeId;
  m_lastPosition.insert (equipNo, lastPos);

}

void PositionHandler::setLastBuildingId(int equipNo, const int lastBuildingId)
{
  ParticipantPosition lastPos;
  if ( m_lastPosition.contains(equipNo))
    lastPos = m_lastPosition.value(equipNo);
  lastPos.buildingId = lastBuildingId;
  m_lastPosition.insert (equipNo, lastPos);

}

void PositionHandler::setLastRoomId(int equipNo, const int lastRoomId)
{
  ParticipantPosition lastPos;
  if ( m_lastPosition.contains(equipNo))
    lastPos = m_lastPosition.value(equipNo);
  lastPos.roomId = lastRoomId;
  m_lastPosition.insert (equipNo, lastPos);

}

void PositionHandler::setLastTime(int equipNo, const qint64 time)
{
  ParticipantPosition lastPos;
  if ( m_lastPosition.contains(equipNo))
    lastPos = m_lastPosition.value(equipNo);
  lastPos.time = time;
  m_lastPosition.insert (equipNo, lastPos);

}


int PositionHandler::lastMountPointTypeId(const int equipNo) const
{
  if ( m_lastPosition.contains(equipNo))
    return (m_lastPosition.value(equipNo).mountPointTypeId);
  else
    return -1;
}

int PositionHandler::lastBuildingId(const int equipNo) const
{
  if ( m_lastPosition.contains(equipNo))
    return (m_lastPosition.value(equipNo).buildingId);
  else
    return -1;
}

int PositionHandler::lastRoomId(const int equipNo) const
{
  if ( m_lastPosition.contains(equipNo))
    return (m_lastPosition.value(equipNo).roomId);
  else
    return -1;
}

bool PositionHandler::isInTrainingArea(const TetraIcd::RelativePosition &relPos) const
{
  if (m_relPosBottomLeft.isNull() || m_relPosTopRight.isNull())
    return true;

  if (relPos.offsetX < m_relPosBottomLeft.x() || relPos.offsetX > m_relPosTopRight.x())
    return false;
  if (relPos.offsetY < m_relPosBottomLeft.y() || relPos.offsetY > m_relPosTopRight.y())
    return false;
  return true;
}