/**
  * Retrieve data from InfrastructeManagement
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, SLSE, 2016-04-14
  */

#include "ism_handler.h"
#include "main_handler.h"
#include "settings.h"
#include <QJsonDocument>
#include <QJsonParseError>
#include <QJsonObject>

namespace TrainingAreaBase
{

IsmHandler::IsmHandler(MainHandler *mainHandler)
  : m_mainHandler(mainHandler)
  , m_dtcIsm(nullptr)
  , m_distanceIpsToGps(Settings::distanceIpsToGps())
  , m_distanceOpsToGps(Settings::distanceOpsToGps())
  , m_logger(getLogger("IsmHandler"))
{}

bool IsmHandler::init()
{
  if (m_dtcIsm)
    return true;

  m_dtcIsm = InfrastructureManagement::Interface::Dtc::create(m_mainHandler->ddsHandler()->ddsConnector());
  connect(m_dtcIsm.get(), &InfrastructureManagement::Interface::Dtc::instrumentationAssetChanged,
          this, &IsmHandler::onInstrumentationAssetChanged);
  connect(m_dtcIsm.get(), &InfrastructureManagement::Interface::Dtc::mountingPointAssetChanged,
          this, &IsmHandler::onMountingPointAssetChanged);
  connect(m_dtcIsm.get(), &InfrastructureManagement::Interface::Dtc::mainStructureElementChanged,
          this, &IsmHandler::onMainStructureElementChanged);
  connect(m_dtcIsm.get(), &InfrastructureManagement::Interface::Dtc::mainFootprintChanged,
          this, &IsmHandler::onMainFootprintChanged);

  if (!m_dtcIsm->enableInstrumentationAsset() ||
      !m_dtcIsm->enableMainStructureElement() ||
      !m_dtcIsm->enableMountingPointAsset()   ||
      !m_dtcIsm->enableMainFootprint())
  {
    ERROR_LOG(m_logger) << "Can not read data from ISM";
    return false;
  }

//  int exerciseId = m_mainHandler->ddsHandler()->getExerciseId();

//  m_mainFootprints = m_dtcIsm->getFootprintsByExerciseId(exerciseId);

//  // Get 'static' data for all MainFootprints
//  {
//    auto cb = [this](const InfrastructureManagement::MainFootprint & mfp)
//    {
//      onMainFootprintChanged(mfp, DataModification::eAdded);
//    };
//    if (!m_dtcIsm->getFootprintsByExerciseId(exerciseId))
//      return false;
//  }
  return true;
}

IsmHandler::OpsData IsmHandler::dataFromOpsCode(int ipsCode)
{
  if ( m_opsData.contains(ipsCode))
  {
    DEBUG_LOG(m_logger) << "dataFromOpsCode contains ipsCode:" << ipsCode
                        << " assetId=" << m_opsData.value(ipsCode).assetId
                        << " roomId=" << m_opsData.value(ipsCode).roomId
                        << " mainStructureElementId=" << m_opsData.value(ipsCode).mainStructureElementId
                        << " structureElementId=" << m_opsData.value(ipsCode).structureElementId
                        << " mountingPointTypeId" << m_opsData.value(ipsCode).mountingPointTypeId;

    if (m_opsData.value(ipsCode).mainStructureElementId > 0 || m_opsData.value(ipsCode).structureElementId > 0)
      return m_opsData.value(ipsCode);
  }

  int assetId = m_dtcIsm->getAssetIdFromOpticalCode(ipsCode);
  OpsData opsData;

//  auto it = m_opsCode2AssetId.find(ipsCode);
//  if (it == m_opsCode2AssetId.end())
  if ( assetId < 0)
  {
    WARNING_LOG(m_logger) << "No asset for ops code=" << ipsCode << " found";
    return {};
  }
  opsData.opsCode = ipsCode;
  opsData.assetId = assetId;

  // if (opsData.structureElementId < 0 )
  // {
  //   WARNING_LOG(m_logger) << "Asset assetId=)" << assetId << " not mounted";
  //   return {};
  // }

 

  DEBUG_LOG(m_logger) << "OpsData.structureElementId=" << opsData.structureElementId
                      << " assetId=" << opsData.assetId
                      << " mountingPointTypeId=" << opsData.mountingPointTypeId
                      << " opsCode=" << opsData.opsCode
                      << " mainStructureElementId=" << opsData.mainStructureElementId
                      << " roomId=" << opsData.roomId;

  {
    // [GUEZE-3496] Teilnehmer im Gebaeudeviewer nicht sichtbar
    // Wenn optische Codes kommen, die nicht in der Verwaltung vorhanden sind, wird sie aktualisiert.
    DEBUG_LOG(m_logger) <<"---------------- UPDATE OPS-Data ------------------";
    updateOpsData(opsData, true);
    opsData= m_opsData[opsData.opsCode] ;
  }
  //
  return opsData;
}

void IsmHandler::onInstrumentationAssetChanged(const InfrastructureManagement::InstrumentationAsset &data, DataModification modType)
{
  if (data.mountingPointTypeId() != InfrastructureManagement::MountingPointType::IndoorIPS &&
      data.mountingPointTypeId() != InfrastructureManagement::MountingPointType::OutdoorIPS)
    return;
  switch (modType)
  {
  case DataModification::eAdded:
  case DataModification::eModified:
  {
    int opsCode = 0;

    QHashIterator<int, OpsData> i(m_opsData);
    while (i.hasNext()) {
        i.next();
        if ( i.value().assetId == data.id())
        {
          opsCode = i.value().opsCode;
          DEBUG_LOG(m_logger) << "onInstrumentationAssetChanged assetId=" << data.id();
          break;
        }
    }

    OpsData &d = m_opsData[opsCode];
    d.assetId = data.id();
    d.opsCode = getOpsCodeFromJSon(data.properties());
    d.opsTunnel = getIsOpsTunnel(data.properties());
    d.mountingPointTypeId = data.mountingPointTypeId().underlying();
    updateOpsData(d);
    break;
  }
  case DataModification::eRemoved:
  {
    int opsCode = 0;
    QHashIterator<int, OpsData> i(m_opsData);
    while (i.hasNext()) {
        i.next();
        if ( i.value().assetId == data.id())
        {
          opsCode = i.value().opsCode;
          DEBUG_LOG(m_logger) << "onInstrumentationAssetChanged assetId=" << data.id();
          break;
        }
    }
    m_opsData.remove(opsCode);
    break;
  }
  }
}

void IsmHandler::onMountingPointAssetChanged(const InfrastructureManagement::MountingPointAsset &data, DataModification modType)
{
  switch (modType)
  {
  case DataModification::eAdded:
  case DataModification::eModified:
  {
    OpsData opsData;

    opsData.opsCode = m_dtcIsm->getOpticalCodeFromAssetId(data.assetId());
    opsData.assetId = data.assetId();

    updateOpsData(opsData, true);
    break;
  }
  case DataModification::eRemoved:
    DEBUG_LOG(m_logger) << "onMountingPointAssetChanged Unmount id=" << data.id()
                         << " assetId=" << data.assetId();

    removeOpsAsset(data.assetId());
    break;
  }
}

void IsmHandler::rotateBuildingFootprint(int mseId)
{
//  InfrastructureManagement::MainStructureElement mse = m_mainStructureElements.value(mseId);

  if ( m_relBuildingFootprintsRotated.contains(mseId))
  {
    DEBUG_LOG(m_logger) << " mainStructureElements.Rotate already done id=" << mseId;
    return;
  }
  InfrastructureManagement::MainStructureElement mse = m_dtcIsm->mainStructureElement(mseId);
  TetraIcd::RelativePosition relBldgPosition = m_mainHandler->posHandler()->geographicToRelative(mse.geoPosition());

  DEBUG_LOG(m_logger) << " mainStructureElements.Rotate id=" << mseId;
  QMatrix matrix;
  QPolygonF relBuildingFootprintMgrs;
  qreal angle = mse.angle();
  matrix.rotate(angle);
  const QPolygonF relBuildingFootprint = m_relBuildingFootprints.value(mseId);
  const QPolygonF relBuildingFootprintRotated = matrix.map(relBuildingFootprint);
  m_relBuildingFootprintsRotated.insert(mseId, relBuildingFootprintRotated);
  relBuildingFootprintMgrs.reserve(m_relBuildingFootprintsRotated.size());
  for (const QPointF &p : relBuildingFootprintRotated)
  {
    QPointF relBuildingPoint(p.x() + relBldgPosition.offsetX,
                             p.y() + relBldgPosition.offsetY);
    relBuildingFootprintMgrs.append(relBuildingPoint);
    DEBUG_LOG(m_logger) << " relBuildingFootprintRotated = " << p.x() << "/" << p.y()
                        << " relBuildingPointMgrs=" << relBuildingPoint.x() << "/" << relBuildingPoint.y()
                        << " angle=" << angle;
  }
  m_relBuildingFootprintsRotatedMgrs.insert(mseId, relBuildingFootprintMgrs);
  m_buildingBoundingBoxesMgrs.insert(mseId, relBuildingFootprintMgrs.boundingRect());
  DEBUG_LOG(m_logger) << "BoundingBox bottomLeft=" << relBuildingFootprintMgrs.boundingRect().bottomLeft().x() << "/" << relBuildingFootprintMgrs.boundingRect().bottomLeft().x()
                      << " topRight="  << relBuildingFootprintMgrs.boundingRect().topRight().x() << "/" << relBuildingFootprintMgrs.boundingRect().topRight().x();
}

void IsmHandler::onMainStructureElementChanged(const InfrastructureManagement::MainStructureElement &data, DataModification modType)
{
  //TetraIcd::RelativePosition relBldgPosition = m_mainHandler->posHandler()->geographicToRelative(data.geoPosition());
  switch (modType)
  {
  case DataModification::eAdded:
  case DataModification::eModified:
  {
    int exerciseId = m_mainHandler->ddsHandler()->getExerciseId();
    if ( !m_dtcIsm->isElementAssignedToExercise(data, exerciseId))
    {
      INFO_LOG(m_logger) << "HandlerBuildingData::onMainStructureElementChanged() id=" << data.id() << " remove";
      //m_mainStructureElements.remove(data.id());
      //m_mainFootprints.remove(data.id());
      m_buildingBoundingBoxes.remove(data.id());
      m_relBuildingFootprints.remove(data.id());
      m_relBuildingFootprintsRotated.remove((data.id()));
      m_buildingBoundingBoxesMgrs.remove(data.id());
      m_relBuildingFootprintsRotatedMgrs.remove(data.id());

      for (const auto &se : data.elements())
      {
        if (se.mountingPointTypeId() != InfrastructureManagement::MountingPointType::IndoorIPS &&
            se.mountingPointTypeId() != InfrastructureManagement::MountingPointType::OutdoorIPS)
          continue;
        removeOpsDataByStructureElementId(se.id());
      }

      // Use mutable iterator to remove element(s):
      QMutableHashIterator<int, OpsData> iter(m_opsData);
      while (iter.hasNext())
      {
        iter.next();
        if (iter.value().mainStructureElementId == data.id())
        {
          DEBUG_LOG(m_logger) << "remove from m_opsData, key=" << iter.key();
          iter.remove();
          break;
        }
      } // while

      return;
    }


    //m_mainStructureElements.insert(data.id(), data);
    //DEBUG_LOG(m_logger) << " m_mainStructureElements.insert key=" << data.id()
    //                    << " angle=" << data.angle();
    for (const InfrastructureManagement::StructureElement &se : data.elements())
    {
      if (se.mountingPointTypeId() != InfrastructureManagement::MountingPointType::IndoorIPS &&
          se.mountingPointTypeId() != InfrastructureManagement::MountingPointType::OutdoorIPS)
        continue;
      auto it = std::find_if(m_opsData.begin(), m_opsData.end(), [se](const OpsData & d)
      {
        return d.structureElementId == se.id();
      });
      if (it != m_opsData.end())
        updateOpsData(it.value());
    }
    DEBUG_LOG(m_logger) << " mainStructureElements Vor Rotate id=" << data.id();
    QMap<int, InfrastructureManagement::MainFootprint> mainFootprints = m_dtcIsm->getFootprintsByExerciseId(exerciseId);

    // Rotate Building-Footprint
    if (mainFootprints.contains(data.id()))
    {
      rotateBuildingFootprint(data.id());
    }
    break;
  }
  case DataModification::eRemoved:
    //m_mainStructureElements.remove(data.id());
    for (const auto &se : data.elements())
    {
      if (se.mountingPointTypeId() != InfrastructureManagement::MountingPointType::IndoorIPS &&
          se.mountingPointTypeId() != InfrastructureManagement::MountingPointType::OutdoorIPS)
        continue;
      removeOpsDataByStructureElementId(se.id());
    }
    break;
  }
}

void IsmHandler::onMainFootprintChanged(const InfrastructureManagement::MainFootprint &data, DataModification modType)
{
  DEBUG_LOG(m_logger) << "onMainFootprintChanged id=" << data.id()
                      << " size=" <<  data.elements().size();

  switch (modType)
  {
  case DataModification::eAdded:
  case DataModification::eModified:
  {
    //int exerciseId = m_mainHandler->ddsHandler()->getExerciseId();
    //QMap<int, InfrastructureManagement::MainFootprint> mainFootprints = m_dtcIsm->getFootprintsByExerciseId(exerciseId);
    DEBUG_LOG(m_logger) << "mainStructureElements onMainFootprintChanged id=" << data.id();
    double minX = 0;
    double maxX = 0;
    double minY = 0;
    double maxY = 0;
    QPolygonF relBuildingFootprint;
    relBuildingFootprint.reserve(data.elements().size());
    for (const InfrastructureManagement::FootprintElement &fpElem : data.elements())
    {
      const Core::RelativePosition &relPos = fpElem.relPosition();
      minX = std::min(minX, relPos.offsetX());
      maxX = std::max(maxX, relPos.offsetX());
      minY = std::min(minY, relPos.offsetY());
      maxY = std::min(maxY, relPos.offsetY());

      QPointF point(relPos.offsetX(), relPos.offsetY());
      relBuildingFootprint.append(point);
      DEBUG_LOG(m_logger) << "relBuildingFootprint.append() " << point.x() << "/" << point.y() << ")";

    }
    // do not use boundingRect() because the top and bottom are swapped
    DEBUG_LOG(m_logger) << "minX=" << minX
                        << " maxX=" << maxX
                        << " minY=" << minY
                        << " maxY=" << maxY
                        << " m_distanceIpsToGps=" << m_distanceIpsToGps
                        << " settings distanceIpsToGps=" << Settings::distanceIpsToGps();

    QPointF boundingBoxTopLeft(minX - m_distanceIpsToGps, maxY + m_distanceIpsToGps);
    QPointF boundingBoxBottomRight(maxX + m_distanceIpsToGps, minY - m_distanceIpsToGps);

    QRectF BoundingBox(boundingBoxTopLeft, boundingBoxBottomRight);
    m_buildingBoundingBoxes.insert(data.id(), BoundingBox);

    m_relBuildingFootprints.insert(data.id(), relBuildingFootprint);
    for (const QPointF &p : qAsConst(relBuildingFootprint))
      DEBUG_LOG(m_logger) << "relBuildingFootprint = " << p.x() << "/" << p.y();
    rotateBuildingFootprint(data.id());
    break;
  }
  case DataModification::eRemoved:
  {
    //m_mainFootprints.remove(data.id());
    break;
  }
  }
}

void IsmHandler::removeOpsDataByStructureElementId(int structureElementId)
{
  auto copy = m_opsData;
  for (const OpsData &pd : qAsConst(copy))
  {
    if (pd.structureElementId == structureElementId)
    {
      removeOpsAsset(pd.assetId);
      break;
    }
  }
}

void IsmHandler::removeOpsAsset(int assetId)
{
int  opsCode = 0;

  for (const OpsData &pd : qAsConst(m_opsData))
  {
    if (pd.assetId == assetId)
    {
      m_structureElementId2AssetId.remove(pd.structureElementId);
      opsCode = pd.opsCode;
      break;
    }
  }
  m_opsData.remove(opsCode);
}

int IsmHandler::getOpsCodeFromJSon(const std::string &s) const
{
  QJsonParseError jError;
  QJsonDocument doc(QJsonDocument::fromJson(s.c_str(), &jError));
  if (jError.error != QJsonParseError::NoError ||
      !doc.isObject())
  {
    ERROR_LOG(m_logger) << "Can not read json data from IPS asset - properties are not parsable: "
                        << jError.errorString() << " - '" << s << "'";
    return 0;
  }
  QJsonObject obj(doc.object());
  return obj[QStringLiteral("opticalCode")].toInt();
}

bool IsmHandler::getIsOpsTunnel(const std::string &s) const
{
  QJsonParseError jError;
  QJsonDocument doc(QJsonDocument::fromJson(s.c_str(), &jError));
  if (jError.error != QJsonParseError::NoError ||
      !doc.isObject())
  {
    ERROR_LOG(m_logger) << "Can not read json data from IPS asset - properties are not parsable: "
                        << jError.errorString() << " - '" << s << "'";
    return false;
  }
  QJsonObject obj(doc.object());
  if ( obj.contains(QStringLiteral("opsTunnel")))
    return obj[QStringLiteral("opsTunnel")].toBool();
  else
    return false;
}

void IsmHandler::updateOpsData(OpsData &data, bool force)
{
  DEBUG_LOG(m_logger) << "updateOpsData"
                       << " opsCode=" << data.opsCode
                       << " assetId=" << data.assetId
                       << " force=" << force;
  int exerciseId = m_mainHandler->ddsHandler()->getExerciseId();
  DEBUG_LOG(m_logger) << "updateOpsData for asset " << data.assetId << " with ops id " << data.opsCode;
  const int seId = m_dtcIsm->getStructueElementIdFromAssetId(data.assetId);

  data.structureElementId = 0;  // in case MainStructureElement is not yet here
  data.mainStructureElementId = 0;
  data.roomId = 0;
  data.position = Core::GeoPosition();

  const auto &mainStructureElements = m_dtcIsm->mainStructureElements();

  // search for structure element in MainStructureElements and update data if found
  for (const auto &mse : qAsConst(mainStructureElements))
  {
    //auto mse = m_mainStructureElements.value(data.mainStructureElementId);

    if (!force && !m_dtcIsm->isElementAssignedToExercise(mse, exerciseId))
      continue;
    for (const InfrastructureManagement::StructureElement &se : mse.elements())
    {
      if (se.id() == seId)
      {
        data.structureElementId = seId;
        data.mainStructureElementId = mse.id();
        // last is own id
        if (se.path().size() > 1)
          data.roomId = se.path().at(se.path().size() - 2);
        data.position = se.geoPosition();
        data.position.z(data.position.z() - mse.geoPosition().z()); // above msl -> above ground
        m_structure_ElementId2AssetTypeId[seId] = se.mountingPointTypeId().underlying();
        DEBUG_LOG(m_logger) << "found position for asset " << data.assetId << " with se id " << seId
                            << " " << data.position.lon() << "/" << data.position.lat() << "/" << data.position.z()
                            << " name=" << se.name()
                            << " type=" << se.mountingPointTypeId()
                            << " m_structure_ElementId2AssetTypeId.size()=" << m_structure_ElementId2AssetTypeId.size();
        data.mountingPointTypeId = se.mountingPointTypeId().underlying();
       // m_opsAssets.insert(data.assetId , data );
        m_opsData[data.opsCode] = data;
        return;
      }
    }
  }
  DEBUG_LOG(m_logger) << "found no position for asset " << data.assetId << " with se id " << seId;
}

bool IsmHandler::posInExtendedBuildingFootprint(int mainStructureElementId, const Core::GeoPosition &position, const int mountPointTypeId) const
{
  int distance = 0;
  DEBUG_LOG(m_logger) << "IsmHandler::posInExtendedBuildingFootprint mainStructureElementId=" << mainStructureElementId
                      << " mountPointTypeId=" << mountPointTypeId;
  if (m_distanceIpsToGps <= 0 && m_distanceOpsToGps <= 0)
    return false;

  switch (mountPointTypeId) {
  case InfrastructureManagement::MountingPointType_def::OutdoorIPS:
    DEBUG_LOG(m_logger) << "posInExtendedBuildingFootprint Distance = m_distanceOpsToGps";
    distance = m_distanceOpsToGps;
    break;
  case InfrastructureManagement::MountingPointType_def::IndoorIPS:
    DEBUG_LOG(m_logger) << "posInExtendedBuildingFootprint Distance = m_distanceIpsToGps";
    distance = m_distanceIpsToGps;
    break;
  default:
    return false;
  }

  /////////////////////////////////////////////////
  // 1: Check, if Position is inside Bounding Box
  /////////////////////////////////////////////////
  DEBUG_LOG(m_logger) << "mainStructureElementId=" << mainStructureElementId
                      << " position=" << position;
//  if (!m_mainStructureElements.contains(mainStructureElementId))
//    return false;
  int exerciseId = m_mainHandler->ddsHandler()->getExerciseId();
  //auto mse = m_mainStructureElements.value(data.mainStructureElementId);
  InfrastructureManagement::MainStructureElement mse = m_dtcIsm->mainStructureElement(mainStructureElementId);

  if ( !m_dtcIsm->isElementAssignedToExercise(mse, exerciseId))
    return false;
  if (!m_buildingBoundingBoxes.contains(mainStructureElementId))
    return false;


  TetraIcd::RelativePosition relBldgPosition = m_mainHandler->posHandler()->geographicToRelative(mse.geoPosition());
  TetraIcd::RelativePosition relPosition = m_mainHandler->posHandler()->geographicToRelative(position);
  DEBUG_LOG(m_logger) << "relBldgPosition=" << relBldgPosition.offsetX << "/" << relBldgPosition.offsetY
                      << ", relPosition=" << relPosition.offsetX << "/" << relPosition.offsetY;
  QPointF relPositionMgrs(relPosition.offsetX, relPosition.offsetY);


  /////////////////////////////////////////////////
  // 2: Check, if Position is inside Building-Footprint
  /////////////////////////////////////////////////

  QPolygonF relBuildingFootprint = m_relBuildingFootprintsRotated[mainStructureElementId];
  QPointF relPositionF(relPosition.offsetX - relBldgPosition.offsetX, relPosition.offsetY - relBldgPosition.offsetY);
  if (relBuildingFootprint.containsPoint(relPositionF, Qt::OddEvenFill))
  {
    DEBUG_LOG(m_logger) << "Position is inside Building-Footprint";
    return true;
  }

  /////////////////////////////////////////////////
  // 3. Check whether the position is within an expanded segment
  /////////////////////////////////////////////////
  DEBUG_LOG(m_logger) << "relBuildingFootprint.size=" << relBuildingFootprint.size();
  for (int i = 0; i < relBuildingFootprint.size() ; i++)
  {
    int nextPoint = i + 1;
    if (i == relBuildingFootprint.size() - 1)
      nextPoint = 0;

    QPointF l1(relBuildingFootprint[i].x() + relBldgPosition.offsetX, relBuildingFootprint[i].y() + relBldgPosition.offsetY);
    QPointF l2(relBuildingFootprint[nextPoint].x() + relBldgPosition.offsetX, relBuildingFootprint[nextPoint].y() + relBldgPosition.offsetY);
    double distancePointLineValue = distancePointLine(relPositionMgrs, l1, l2);
    DEBUG_LOG(m_logger) << "distancePointLineValue=" << distancePointLineValue
                        << " Point =" << relPositionMgrs.x() << "/" << relPositionMgrs.y()
                        << " Line=" << l1.x() << "/" << l1.y()
                        << "   " << l2.x() << "/" << l2.y();
    if (distancePointLineValue < distance)
    {
      DEBUG_LOG(m_logger) << "IsmHandler::posInExtendedBuildingFootprint return true";
      return true;
    }
  }
  DEBUG_LOG(m_logger) << "IsmHandler::posInExtendedBuildingFootprint return false";
  return false;
}

double IsmHandler::distancePointLine(const QPointF &p, const QPointF &l1,
                                     const QPointF &l2) const
{
  const double u = (((p.x() - l1.x()) * (l2.x() - l1.x())) +
                    ((p.y() - l1.y()) * (l2.y() - l1.y()))) / squareLen(l2, l1);

  if (u < 0.0f || u > 1.0f)
  {
    QLineF pToP1(p, l1);
    QLineF pToP2(p, l2);
    qreal length1 = pToP1.length();
    qreal length2 = pToP2.length();
    if (length1 < length2)
    {
      DEBUG_LOG(m_logger) << "a distancePointLine p=" << p.x() << "/" << p.y()
                          << " line: p1=" << l1.x() << "/" << l1.y()
                          << " p2=" << l2.x() << "/" << l2.y()
                          << " distance=" << length1
                          << ", return=" << length1;
      return length1;
    }
    else
    {
      DEBUG_LOG(m_logger) << "b distancePointLine p=" << p.x() << "/" << p.y()
                          << " line: p1=" << l1.x() << "/" << l1.y()
                          << " p2=" << l2.x() << "/" << l2.y()
                          << " distance=" << length2
                          << ", return=" << length2;
      return length2;
    }
  }

  QPointF intersectPnt(l1.x() + u * (l2.x() - l1.x()),
                       l1.y() + u * (l2.y() - l1.y()));

  double distance = sqrt(squareLen(p, intersectPnt));

  DEBUG_LOG(m_logger) << "c distancePointLine p=" << p.x() << "/" << p.y()
                      << " line: p1=" << l1.x() << "/" << l1.y()
                      << " p2=" << l2.x() << "/" << l2.y()
                      << " distance=" << distance;
  return distance;
}

double IsmHandler::squareLen(const QPointF &p1, const QPointF &p2)
{
  double vx = p2.x() - p1.x();
  double vy = p2.y() - p1.y();
  return vx * vx + vy * vy;
}


} // namespace TrainingAreaBase