/**
  * Simulation for Movement
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by gr, SLSE, 2017-12-13
  */

#include "simulation_movement.h"
#include <LtsCoordTrans/lts_coord_trans.h>
#include <QtCore/QDateTime>

namespace CentralEffectSimulationBasic
{

SimulationMovement::SimulationMovement(const std::shared_ptr<CentralEffectSimulation::DdsHandler> &ddsHandler,
                                       const std::shared_ptr<CentralEffectSimulation::DatabaseInterface> &db,
                                       const std::shared_ptr<CentralEffectSimulation::HandlerParticipant> &participantHandler,
                                       const CentralEffectSimulation::SimulatedMovement &sml,
                                       QObject *parent)
  : QObject(parent)
  , m_ddsHandler(ddsHandler)
  , m_db(db)
  , m_handlerParticipant(participantHandler)
  , m_moveTime(1)
  , m_actSegment()
  , m_newPos()
  , m_simStart(true)
  , m_zone(-1)
  , m_segTimer(new QTimer(this))
  , m_moveTimer(new QTimer(this))
  , m_simMovement(sml)
  , m_logger(CentralEffectSimulation::getLogger("Plugin.Basic.SimulationMovement"))
{
  m_segTimer->setSingleShot(true);
  connect(m_segTimer, &QTimer::timeout, this, &SimulationMovement::startNewSegment);

  m_moveTimer->setSingleShot(true);
  connect(m_moveTimer, &QTimer::timeout, this, &SimulationMovement::move);

  setSimulatedMovementRequest(sml);
}

SimulationMovement::~SimulationMovement()
{
  DEBUG_LOG(m_logger) << "Delete SimulatedMovement "
                      << " id=" << m_simMovement.id
                      << " participantId= " << m_simMovement.participantId;
  m_ddsHandler->dtsCes()->removeSimulatedMovementByKey(m_simMovement.id);
  m_segTimer->stop();
  m_moveTimer->stop();
}

void SimulationMovement::setSimulatedMovementRequest(const CentralEffectSimulation::SimulatedMovement &sml)
{
  int nextPoint = m_simMovement.nextPoint;
  m_simMovement = sml;
  m_simMovement.nextPoint = nextPoint;

  m_moveTime = 1;
  m_segTimer->stop();
  m_moveTimer->stop();

  m_points.clear();
  m_segSpeed.clear();
  m_segDelay.clear();
  m_points.reserve(sml.segments.size());
  m_segSpeed.reserve(sml.segments.size());
  m_segDelay.reserve(sml.segments.size());

  for (const CentralEffectSimulation::SimulatedMovementSegment &point : sml.segments)
  {
    const Core::GeoPosition2D &pointGeo = point.position;
    m_points.push_back(m_zone == -1 ? CentralEffectSimulation::geographic2Utm(pointGeo, &m_zone) : CentralEffectSimulation::geographic2Utm(pointGeo, m_zone));
    DEBUG_LOG(m_logger) << "pointGeo lat=" << pointGeo.lat << " lon=" << pointGeo.lon
                        << " point lat=" << point.position.lat << " lon=" << point.position.lon
                        << " utmPoint=" << CentralEffectSimulation::debug(m_points.last());
    m_segSpeed.append(point.speed);
    m_segDelay.append(point.delay);
  }

  // Only start simulation in future
  DEBUG_LOG(m_logger) << "m_startTime - QDateTime::currentMSecsSinceEpoch()=" << m_simMovement.startTime - QDateTime::currentMSecsSinceEpoch()
                      << " m_startTime=" << m_simMovement.startTime
                      << " currentTime=" << QDateTime::currentMSecsSinceEpoch()
                      << " m_nextPoint=" << m_simMovement.nextPoint;

  if (m_simMovement.nextPoint > 0 && m_points.size() > m_simMovement.nextPoint)
    m_newPos = m_points[m_simMovement.nextPoint].toPoint();

  if (m_simMovement.startTime - QDateTime::currentMSecsSinceEpoch() > 0)
    m_moveTimer->start(m_simMovement.startTime - QDateTime::currentMSecsSinceEpoch());
  else
  {
    DEBUG_LOG(m_logger) << "Service-Start";
    m_moveTimer->start(10); // Service-Start
  }
}

void SimulationMovement::updateNextPointInDb()
{
  if (m_simMovement.id <= 0)
  {
    WARNING_LOG(m_logger) << "Internal error - id not set!";
    return;
  }
  CentralEffectSimulation::Database::CesSimulatedMovement dbSim = toDb(m_simMovement);
  m_db->update(dbSim);
}


void SimulationMovement::startNewSegment()
{
  DEBUG_LOG(m_logger) << "simulate participantId = " << m_simMovement.participantId
                      << " m_nextPoint = " << m_simMovement.nextPoint;

  m_moveTime = 1;
  if (m_simMovement.nextPoint < m_points.size())
  {
    QPointF p1;
    // Next segment
    const CentralEffectSimulation::HandlerParticipant::CesParticipantData pos = m_handlerParticipant->position(m_simMovement.participantId);
    if (pos.participantId == m_simMovement.participantId)
    {
      p1 = CentralEffectSimulation::geographic2Utm(pos.pos, m_zone);
      DEBUG_LOG(m_logger) << "Next segment ParticipantPosition =" << CentralEffectSimulation::debug(p1);
    }
    else
    {
      if (m_simStart)  // Wait for participantPosition
      {
        INFO_LOG(m_logger) << "Wait for participant position participantId =" << m_simMovement.participantId;
        m_segTimer->start(1000);
        return;
      }
      if (m_simMovement.nextPoint > 0)
      {
        p1 = m_points.at(m_simMovement.participantId - 1);
        DEBUG_LOG(m_logger) << "nextPoint>0 (" << m_simMovement.nextPoint << ") ParticipantPosition =" << CentralEffectSimulation::debug(p1);
      }
      else
      {
        p1 = m_points.at(m_points.size() - 1);
        DEBUG_LOG(m_logger) << "nextPoint=" << m_simMovement.nextPoint << " ParticipantPosition =" << CentralEffectSimulation::debug(p1);
      }
    }
    const QPointF &p2 = m_points.at(m_simMovement.nextPoint);
    m_actSegment.setPoints(p1, p2);
    int curDelay = m_segDelay.at(m_simMovement.nextPoint);
    DEBUG_LOG(m_logger) << "New Segment: p1 = " << CentralEffectSimulation::debug(p1) << " p2=" << CentralEffectSimulation::debug(p2)
                        << " delay =" << curDelay;
    m_moveTimer->start(curDelay);
  }
  else
  {
    if (m_simMovement.repeat)
    {
      m_simMovement.nextPoint = 0;
      m_moveTimer->start(m_segDelay.at(m_simMovement.nextPoint));
    }
    else
      m_simMovement.nextPoint = -1; // Invalid value
  }
}

void SimulationMovement::setJump(bool jump)
{
  DEBUG_LOG(m_logger) << "Jump";
  if (jump)
  {
    m_simMovement.nextPoint = 1;
    m_newPos = m_points.at(0);
    m_actSegment.setPoints(m_newPos,
                           m_points.at(m_simMovement.nextPoint));
    m_moveTime = 1;
    m_simStart = false;
    publishNewPos();
    DEBUG_LOG(m_logger) << "Jump done";
  }
}

void SimulationMovement::publishSimulatedMovement()
{
  DEBUG_LOG(m_logger) << " m_nextPoint= " << m_simMovement.nextPoint;
  m_ddsHandler->dtsCes()->publish(m_simMovement);
}

void SimulationMovement::publishNewPos()
{
  TrainingAreaBase::EquipmentPosition participantPos;

  LtsCoordTrans::UtmCoordinate utmPos;
  utmPos.zone = m_zone;
  utmPos.northp = true;
  utmPos.x = m_newPos.x();
  utmPos.y = m_newPos.y();
  const QPointF posGeo = utmPos.geographic();
  DEBUG_LOG(m_logger) << "participantPos zone=" << utmPos.zone
                      << " northp=" << utmPos.northp
                      << " x=" << utmPos.x
                      << " y=" << utmPos.y;

  const Core::GeoPosition pos(Core::GeoPosition(posGeo.x(), posGeo.y(), 0));

  participantPos.equipmentNumber = -m_simMovement.participantId; // negative - treat as participantId
  participantPos.sampleTime = m_ddsHandler->currentPartitionTime();
  participantPos.position.position = pos;
  participantPos.position.reportingSource = Core::PositionReportingSource::MovementSimulation;

  m_ddsHandler->dtsTab()->publish(participantPos);

}

void SimulationMovement::move()
{
  DEBUG_LOG(m_logger) << "move";
  if (m_simMovement.nextPoint < 0)
  {
    DEBUG_LOG(m_logger) << "move next point < 0";
    return;
  }

  const CentralEffectSimulation::HandlerParticipant::CesParticipantData partPos = m_handlerParticipant->position(m_simMovement.participantId);
  const QPointF participantPosUtm = CentralEffectSimulation::geographic2Utm(partPos.pos, m_zone);
  if (partPos.participantId != m_simMovement.participantId)
  {
    DEBUG_LOG(m_logger) << "move no ParticipantId=" << m_simMovement.participantId;
    m_moveTimer->start(1000);
    return;
  }
  if (!partPos.positionValid)
  {
    DEBUG_LOG(m_logger) << "move no ParticipantId=" << m_simMovement.participantId;
    m_moveTimer->start(1000);
    return;
  }
  DEBUG_LOG(m_logger) << " pos=" << CentralEffectSimulation::debug(participantPosUtm);

  QLineF step = m_actSegment;
  if (m_simStart)
  {
    m_actSegment.setPoints(participantPosUtm,
                           m_points.at(m_simMovement.nextPoint));
    step = m_actSegment;
    DEBUG_LOG(m_logger) << "m_simStart set m_actSegment"
                        << " p1: " << CentralEffectSimulation::debug(m_actSegment.p1())
                        << " p2: " << CentralEffectSimulation::debug(m_actSegment.p2());
    m_simStart = false;
  }

  if (m_segSpeed[m_simMovement.nextPoint] <= 0.0)
  {
    DEBUG_LOG(m_logger) << "move next point < 0";
    return;
  }

  m_newPos = m_points[m_simMovement.nextPoint];
  step.setLength(m_segSpeed[m_simMovement.nextPoint] * m_moveTime);
  m_moveTime++;
  // ready criterion
  if (step.length() >= m_actSegment.length() + 0.1 ||
      step.length() < 0.1) // 0.1 Magic Number but 10 cm distance to Point is arrived
  {
    DEBUG_LOG(m_logger) << "move ready step.length()=" << step.length()
                        << " m_actSegment.length()=" << m_actSegment.length();
    m_simMovement.nextPoint = m_simMovement.nextPoint + 1;
    if (m_simMovement.nextPoint >= m_points.size())
    {
      if (m_simMovement.repeat)
        m_simMovement.nextPoint = 0;
      else
      {
        m_simMovement.nextPoint = -1; // Invalid value
        return;
      }
    }
    updateNextPointInDb();
    DEBUG_LOG(m_logger) << "ready move participantId = " << m_simMovement.participantId
                        << " m_nextPoint =" << m_simMovement.nextPoint
                        << " newPos =" << m_newPos.x() << "/" << m_newPos.y()
                        << " m_moveTime=" << m_moveTime
                        << " length=" << m_segSpeed[m_simMovement.nextPoint] * m_moveTime
                        << " step.length=" << step.length()
                        << " m_segSpeed[m_nextPoint]=" << m_segSpeed[m_simMovement.nextPoint]
                        << " m_moveTime=" << m_moveTime
                        << " m_actSegment.length=" << m_actSegment.length()
                        << " m_zone=" << m_zone;
    publishNewPos();
    startNewSegment();
    publishSimulatedMovement();
    return;
  }
  m_newPos = step.p2();
  DEBUG_LOG(m_logger) << "move participantId = " << m_simMovement.participantId
                      << " m_nextPoint =" << m_simMovement.nextPoint
                      << " newPos =" << m_newPos.x() << "/" << m_newPos.y()
                      << " m_moveTime=" << m_moveTime
                      << " length=" << m_segSpeed[m_simMovement.nextPoint] * m_moveTime
                      << " step.length=" << step.length()
                      << " m_speed=" << m_segSpeed[m_simMovement.nextPoint]
                      << " m_moveTime=" << m_moveTime
                      << " m_actSegment.length=" << m_actSegment.length()
                      << " m_zone=" << m_zone;
  publishNewPos();
  m_moveTimer->start(1000); // Calculate new position each second
}

CentralEffectSimulation::Database::CesSimulatedMovement SimulationMovement::toDb(const CentralEffectSimulation::SimulatedMovement &ddsSimMovement)
{
  CentralEffectSimulation::Database::CesSimulatedMovement ret;
  ret.id = ddsSimMovement.id;
  ret.name = QString::fromStdString(ddsSimMovement.name);
  ret.startTime = ddsSimMovement.startTime;
  ret.participantId = ddsSimMovement.participantId;
  ret.repeat = ddsSimMovement.repeat;
  ret.nextpoint = ddsSimMovement.nextPoint;
  return ret;
}

}