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

using namespace TrainingAreaBase;

PositionHandler::PositionHandler(MainHandler *mainHandler) : QObject()
  , m_mainHandler(mainHandler),
    m_logger(NGL::NGLLoggerBootstrap::getNGLRootLogger()) {}

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; 
  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;
  }
  
  m_dtcAsm.reset();

  
  const LtsCoordTrans::UtmCoordinate bottomLeft(LtsCoordTrans::UtmCoordinate::fromGeographic(ta.bottomLeft.lon, ta.bottomLeft.lat));
  const QPointF mid(bottomLeft.geographic(ta.width / 2, ta.height / 2));

  
  
  QString mgrs = LtsCoordTrans::mgrsFromGeographic(mid.x(), mid.y(), -1);
  mgrs += QStringLiteral("00000000000000");

  
  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))
  {
    
    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();

  
  const LtsCoordTrans::UtmCoordinate point(LtsCoordTrans::UtmCoordinate::fromGeographic(p.lon, p.lat, m_utm2Geographic.zone));
  
  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));
  
  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); 
  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;
}