/**
  * @RDE_HEADER@
  * Math claculations for VWM
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, SLSE, 2017-01-24
  */

#include "vwm_math.h"

#include <logger.h>

#include <cmath>

// [GUEZE-483] RPL Nr 756: Steilfeuer Mörser auf Gebäude
// values take from xml file have 3 digits. Therefore 0.00001 is to small
// use 0.002
#define epsilon 0.002

namespace BuildingControlUnit
{
namespace VWM
{
namespace Math
{

static NGL::INGLLogger &logger()
{
  return BuildingControlUnit::getLogger("VWM.Math");
}

LtsUtils::QPoint3D rotateAroundYAxis(const LtsUtils::QPoint3D &in, double phi)
{
  LtsUtils::QPoint3D a1( cos(phi), 0, -sin(phi));
  LtsUtils::QPoint3D a2(        0, 1,         0);
  LtsUtils::QPoint3D a3( sin(phi), 0,  cos(phi));

  return LtsUtils::QPoint3D(LtsUtils::QPoint3D::dotProduct(a1, in),
                            LtsUtils::QPoint3D::dotProduct(a2, in),
                            LtsUtils::QPoint3D::dotProduct(a3, in));
}


LtsUtils::QPoint3D rotateAroundZAxis(const LtsUtils::QPoint3D &in, double phi)
{
  LtsUtils::QPoint3D a1( cos(phi), sin(phi), 0);
  LtsUtils::QPoint3D a2(-sin(phi), cos(phi), 0);
  LtsUtils::QPoint3D a3(        0,        0, 1);

  return LtsUtils::QPoint3D(LtsUtils::QPoint3D::dotProduct(a1, in),
                            LtsUtils::QPoint3D::dotProduct(a2, in),
                            LtsUtils::QPoint3D::dotProduct(a3, in));
}

static inline double getDet2(double a11, double a12, double a21, double a22)
{
  return a11 * a22 - a12 * a21;
}

static inline double getDet3(const LtsUtils::QPoint3D &a1, const LtsUtils::QPoint3D &a2, const LtsUtils::QPoint3D &a3)
{
  double sum = 0;
  sum += (a1.x() * a2.y() * a3.z() + a1.y() * a2.z() * a3.x() + a1.z() * a2.x() * a3.y());
  sum -= (a1.z() * a2.y() * a3.x() - a1.y() * a2.x() * a3.z() - a1.x() * a2.z() * a3.y());
  return sum;
}

VwmPlane::VwmPlane(const LtsUtils::QPoint3D &pos, const LtsUtils::QPoint3D &vec1, const LtsUtils::QPoint3D &vec2)
  : m_pos(pos)
  , m_vec1(vec1 - pos)
  , m_vec2(vec2 - pos)
{
  m_det.setX(getDet2(m_vec1.y(), m_vec1.z(), m_vec2.y(), m_vec2.z()));
  m_det.setY(getDet2(m_vec1.z(), m_vec1.x(), m_vec2.z(), m_vec2.x()));
  m_det.setZ(getDet2(m_vec1.x(), m_vec1.y(), m_vec2.x(), m_vec2.y()));

  m_planeC = LtsUtils::QPoint3D::dotProduct(m_pos, m_det);
}

bool VwmPlane::getPlaneEquation() const
{
  if (qFuzzyIsNull(m_det.z()) && qFuzzyIsNull(m_det.x()) && qFuzzyIsNull(m_det.y()))
    return false;
  return true;
}

bool VwmPlane::isPlaneLineIntersection(const LtsUtils::QPoint3D &vec) const
{
  const double det = getDet3(vec, m_vec1, m_vec2);
  if (qFuzzyIsNull(det))
    return false;
  return true;
}

bool VwmPlane::isPlaneLineOrthogonal(const LtsUtils::QPoint3D &dir) const
{
  const double nv = LtsUtils::QPoint3D::dotProduct(m_det, dir);
  return (qFuzzyIsNull(nv));
}

double VwmPlane::getPlaneLineSkalar(const LtsUtils::QPoint3D &pos, const LtsUtils::QPoint3D &dir) const
{
  const double nv = LtsUtils::QPoint3D::dotProduct(m_det, dir);
  if (qFuzzyIsNull(nv))
    return 0;
  const double np = LtsUtils::QPoint3D::dotProduct(m_det, pos);
  return (m_planeC - np) / nv;
}

LtsUtils::QPoint3D VwmPlane::getPlaneLineIntersection(const LtsUtils::QPoint3D &pos, const LtsUtils::QPoint3D &dir) const
{
  const double lambda = getPlaneLineSkalar(pos, dir);

  LtsUtils::QPoint3D ret (pos + (dir * lambda));
  return ret;
}

double VwmPlane::distPlaneToPoint(const LtsUtils::QPoint3D &pos, const LtsUtils::QPoint3D &dir) const
{
  const double f = m_det.length();
  const double dp = LtsUtils::QPoint3D::dotProduct(m_det, pos);
  double dist = (m_planeC - dp) / f;
  dist *= (LtsUtils::QPoint3D::dotProduct(m_det, dir) < 0) ? -1.0 : 1.0;

  return dist;
}

bool vecIsLinearlyDependent(const LtsUtils::QPoint3D &v1, const LtsUtils::QPoint3D &v2)
{
  double lambda;

  if (fabs(v1.x()) > epsilon)
    lambda = v2.x() / v1.x();

  else if (fabs(v1.y()) > epsilon)
    lambda = v2.y() / v1.y();

  else if (fabs(v1.z()) > epsilon)
    lambda = v2.z() / v1.z();
  else
    return true; // dependent

  if (    (fabs(lambda*v1.x()-v2.x()) < epsilon)
       && (fabs(lambda*v1.y()-v2.y()) < epsilon)
       && (fabs(lambda*v1.z()-v2.z()) < epsilon))
  {
    return true; // dependent
  }

  return false; // independent
}

//
// polyIsLinearlyDependent is replaced by getCoordsForPlane();
// return value '0' can happen if isLinearlyDependent and as return at the end....
//int polyIsLinearlyDependent(const QVector<LtsUtils::QPoint3D> &poly)
//{
//  NGL::INGLLogger &l = logger();

//  // Check, if vector != 0
///*  for (int i = 0; i < poly.size()-1; ++i)
//  {
//    LtsUtils::QPoint3D v1 = poly.at(i+1) - poly.at(i);
//    DEBUG_LOG(l) << "isLinearlyDependent=" << std::boolalpha << isLinearlyDependent
//                  << " i=" << i
//                  << " v1=" << v1.x() << "/" << v1.y() << "/" << v1.z();
//    if ( v1.x() * v1.x() + v1.y() * v1.y() + v1.z() * v1.z() > epsilon)
//      return i;

//  }*/


//  // Check, if point is Linearly Dependent
//  for (int i = 0; i < poly.size()-2; ++i)
//  {
//    LtsUtils::QPoint3D v1 = poly.at(i+1) - poly.at(i);
//    LtsUtils::QPoint3D v2 = poly.at(i+2) - poly.at(i);
//    bool isLinearlyDependent = vecIsLinearlyDependent(v1, v2);
//    DEBUG_LOG(l) << "isLinearlyDependent=" << std::boolalpha << isLinearlyDependent
//                  << " i=" << i
//                  << " v1=" << v1.x() << "/" << v1.y() << "/" << v1.z()
//                  << " v2=" << v2.x() << "/" << v2.y() << "/" << v2.z();
//    if (isLinearlyDependent)
//      return i;
//  }
//  return 0;
//}


/*
[GUEZE-483] RPL Nr 756: Steilfeuer Mörser auf Gebäude
 Bei weiterer Betrachtung ist aufgefallen, das bei bldg_1104 AWES auf die Ecken
 des Bldgs NICHT zu einem Zerstörung führen - Fehler.
 Bei der Analyse ergeben sich folgende Erkenntnisse:
 - Der epsilon-Wert war auf 0.0001.
    Da aber die Werte aus den den XML-Dateien eine Auflösung von 3Stellen (mm)
    aufweisen, sind Rechenfehler auch in dieser Größenordnung zu erwarten.
    Der epsilon-Wert wird in dem Zuge auf 0.002 gesetzt.
- vecIsLinearlyDependent
    Hier war der return-Value vertauscht -> korrigiert.
- polyIsLinearlyDependent
    War missverständlich/fehleranfällig, da im Falle von isLinearlyDependent
    grundsätzlich auch '0' zurückgegeben werden kann. dies aber auch der return-Value
    am Ende der Funktion ist... Was bedeutet nun also '0'???
- getCoordsForPlane
    wurde als Nachfolger von polyIsLinearlyDependent() eingeführt.
    Aus dem WallElement werden die ggf. vorhandenen linearen Abhänigkeiten entfernt.
    Der Aufrufer kann dann daraus eine Plane erstellen (Nutzung der 3 ersten Werte)
- Der UnitTest 'VwmMathUnitTest' wurde erweitert:
    - Es werden einfache Gut- und Schlechtfälle wurden konstruiert.
    - Es werden realen Polygonzüge geprüft. (bldg 1104, Raum 401; bldg_salchau577...)
*/
QVector<LtsUtils::QPoint3D> getCoordsForPlane(const QVector<LtsUtils::QPoint3D> &poly)
{
  NGL::INGLLogger &l = logger();

  if (poly.size() < 3)
    return {};

  QVector<int> idxToRemove;
  for (int i = 0; i < poly.size()-2; ++i)
  {
    const LtsUtils::QPoint3D &v1 = poly.at(i+1) - poly.at(i);
    const LtsUtils::QPoint3D &v2 = poly.at(i+2) - poly.at(i);
    bool isLinearlyDependent = vecIsLinearlyDependent(v1, v2);
    DEBUG_LOG(l) << "isLinearlyDependent=" << isLinearlyDependent
                  << " i=" << i
                  << " v1=" << v1.x() << "/" << v1.y() << "/" << v1.z()
                  << " v2=" << v2.x() << "/" << v2.y() << "/" << v2.z();
    if (isLinearlyDependent)
      idxToRemove.push_front(i+1);

  } // for

  QVector<LtsUtils::QPoint3D> ret = poly;
  for (auto idx : idxToRemove)
    ret.remove(idx);
  return ret;
  // Maybe there is a LinearlyDependency regarding beginning and and of vector.
  // Until now, this seems to be not relevant, since the first coordinates are taken.
}

bool isPointInPolygon(const QVector<LtsUtils::QPoint3D> &poly, const LtsUtils::QPoint3D &pos)
{
  NGL::INGLLogger &l = logger();
  double anglesum = 0.0;
  double vz = 1.0;
  LtsUtils::QPoint3D vn;

  if (poly.size() < 3)
    return false;

  for (int i = 0; i < poly.size(); ++i)
  {
    DEBUG_LOG(l) << "calculate point i=" << i;
    const int nextIdx1 = (i + 1) % poly.size();
    const int nextIdx2 = (i + 2) % poly.size();

    const LtsUtils::QPoint3D &p1 = poly.at(i) - pos;
    const LtsUtils::QPoint3D &p2 = poly.at(nextIdx1) - pos;
    if (i == 0)
      vn = LtsUtils::QPoint3D::crossProduct(p1,p2);

    const double dist1 = p1.length();
    const double dist2 = p2.length();
    if (qFuzzyIsNull(dist1 * dist2))
    {
      DEBUG_LOG(l) << "isPointInPolygon = false";
      return false;
    }

    const double cosTheta = LtsUtils::QPoint3D::dotProduct(p1, p2) / (dist1 * dist2);
    anglesum += vz * acos(cosTheta);

    const LtsUtils::QPoint3D &n = LtsUtils::QPoint3D::crossProduct(vn, p2);
    const LtsUtils::QPoint3D &p3 = poly.at(nextIdx2) - pos;
    const double dp = LtsUtils::QPoint3D::dotProduct(n, p3);

    vz = (dp < 0) ? -1.0 : 1.0;
  }

  const double delta = 0.01;
  const double sum = (M_PI * 2) - fabs(anglesum);
  bool result = -delta < sum && sum < delta;
  DEBUG_LOG(l) << "isPointInPolygon=" << std::boolalpha << result << " (sum=" << sum << ", anglesum=" << anglesum << ")"
               << ", pos=" << pos;
  return result;
}

static inline bool getHitPositionInternal(const QVector<LtsUtils::QPoint3D> &planePoints,
                                          const LtsUtils::QPoint3D &position,
                                          const LtsUtils::QPoint3D &direction,
                                          HitPosition &hitPos)
{
  NGL::INGLLogger &l = logger();
  if (planePoints.size() < 3)
  {
    WARNING_LOG(l) << "Wall with fewer than 3 points - modelling error!";
    return false;
  }
  QVector<LtsUtils::QPoint3D> localPoly = getCoordsForPlane(planePoints);
  if (localPoly.size() < 3)
  {
    ERROR_LOG(l) << "planePoints are not sufficiant";
    for (auto coord : planePoints)
      ERROR_LOG(l) << "planePoints are not sufficiant: << " << coord;
    for (auto coord : qAsConst(localPoly))
      ERROR_LOG(l) << "localPoly are not sufficiant: << " << coord;
    return false;
  }

  const VwmPlane vwmPlane(localPoly.at(0), localPoly.at(1), localPoly.at(2));
  for ( int i = 0; i < localPoly.size(); ++i)
  {
    DEBUG_LOG(l) << "vwmPlane at(" << i << ")=" << localPoly.at(i).x()
                 << "/" << localPoly.at(i).y()
                 << "/" << localPoly.at(i).z() ;
  }
  DEBUG_LOG(l) << "   plane params: p1 = " << vwmPlane.pos()
               << ", p2 = " << vwmPlane.vec1()
               << ", p3 = " << vwmPlane.vec2()
               << ", det = " << vwmPlane.det()
               << ", C = " << vwmPlane.planeC()
               << ", isVertical=" << vwmPlane.isVertical();

  if (!vwmPlane.getPlaneEquation())
    return false;
  DEBUG_LOG(l) << "   getPlaneEquation = true";

  if (!vwmPlane.isPlaneLineIntersection(direction))
    return false;
  DEBUG_LOG(l) << "   isPlaneLineIntersection = true";

  const double lambda = vwmPlane.getPlaneLineSkalar(position, direction);
  if (lambda <= 0)
    return false;
  DEBUG_LOG(l) << "   lambda = " << lambda;

  const LtsUtils::QPoint3D pos = vwmPlane.getPlaneLineIntersection(position, direction);
  DEBUG_LOG(l) << "pos=" << pos.x() << "/" << pos.y() << "/" << pos.z()
               << " position=" << position.x() << "/" << position.y() << "/" << position.z()
               << " direction=" << direction.x() << "/" << direction.y() << "/" << direction.z();

  if (!isPointInPolygon(planePoints, pos))
    return false;
  DEBUG_LOG(l) << "   isPointInPolygon = true (pos=" << pos << ")";

  // never negative since this will bailed out with getPlaneLineSkalar() above
  const double curDist = (pos - position).length();
//  const double curDist = vwmPlane.distPlaneToPoint(position, direction);
//  ERROR_LOG(l) << "l: " << curDist << " = " << (pos - position).length();
  DEBUG_LOG(l) << "   distance = " << curDist;
  if (curDist > hitPos.distance)
    return false;
  hitPos.distance = curDist;

  return true;
}

HitPosition getHitPosition(const LtsUtils::QPoint3D &position, const LtsUtils::QPoint3D &direction, const QMap<int, Room> &rooms)
{
  NGL::INGLLogger &l = logger();
  INFO_LOG(l) << "XXXXX Searching hit position for position " << position
               << ", direction " << direction
               << ", #rooms = " << rooms.size();
  HitPosition hitPos;

  for (const Room &room : rooms)
  {
    DEBUG_LOG(l) << "****** roomId: " << room.id() << ", #walls = " << room.outerWalls().size();
    for (int idxOuterWall = 0; idxOuterWall < room.outerWalls().size(); ++idxOuterWall)
    {
      const OuterWall &wall = room.outerWalls().at(idxOuterWall);
      DEBUG_LOG(l) << " roomId: " << room.id() << ", wallIdx = " << idxOuterWall << ", #elements = " << wall.elements().size();
      for (int idxWallElement = 0; idxWallElement < wall.elements().size(); ++idxWallElement)
      {
        const WallElement &wallElement = wall.elements().at(idxWallElement);
        DEBUG_LOG(l) << " roomId: " << room.id() << ", wallIdx = " << idxOuterWall << ", wElementIdx = " << idxWallElement;
        if (!getHitPositionInternal(wallElement, position, direction, hitPos))
          continue;

        DEBUG_LOG(l) << "   nearest distance = " << hitPos.distance << " on outer wall: roomId: " << room.id() << ", wallIdx = " << idxOuterWall << ", wElementIdx = " << idxWallElement;
        hitPos.state = HitPosition::State::OuterWall;
        hitPos.roomId = room.id();
        hitPos.outerWallIdx = idxOuterWall;
        hitPos.exclusionZoneIdx = -1;

        for (int idxExclusionZone = 0; idxExclusionZone < wall.exclusionZones().size(); ++idxExclusionZone)
        {
          const ExclusionZone &ez = wall.exclusionZones().at(idxExclusionZone);
          DEBUG_LOG(l) << "  roomId: " << room.id() << ", wallIdxx = " << idxOuterWall << ", wElementIdx = " << idxWallElement
                       << " - exclusion zone element idx = " << idxExclusionZone << ", #elements = " << ez.elements().size();
          for (int idxExclusionZoneElement = 0; idxExclusionZoneElement < ez.elements().size(); ++idxExclusionZoneElement)
          {
            const ExclusionZoneElement &ezElement = ez.elements().at(idxExclusionZoneElement);
            DEBUG_LOG(l) << "  roomId: " << room.id() << ", wallIdx = " << idxOuterWall << ", wElementIdx = " << idxWallElement
                         << " - exclusion zone element idx = " << idxExclusionZoneElement;
            if (!getHitPositionInternal(ezElement, position, direction, hitPos))
              continue;

            hitPos.state = HitPosition::State::ExclusionZone;
            hitPos.exclusionZoneIdx = idxExclusionZone;

            DEBUG_LOG(l) << "   nearest distance = " << hitPos.distance << " on exclusion zone";
          }
        }
      }
    }
  }
  INFO_LOG(l) << "XXXXX HitPosition: " << hitPos.state
               << ", roomId = " << hitPos.roomId
               << ", outerWallIndex = " << hitPos.outerWallIdx
               << ", exclusionZoneIndex = " << hitPos.exclusionZoneIdx;
  return hitPos;
}

// ThorstenMeyer: Beim Treffer eines BSMs wird die HitRoomID wie folgt bestimmt
//1. Berechnung des Strahls S durch das BSM in Schußrichtung:
//    Die Richtung „vwmDir3D“ wird mit der Funktion übergeben:
//    getHitPosition(VwmPosition3D* vwmCoord,VwmPosition3D* vwmDir3D)

//2. Berechnung von 2 linear unabhängigen Vektoren durch die Differenzbildung von
//    Polygoneckpunkten einer OuterWall:
//    if (vwmPlaneOW.getIsLinearlyIndependent(…)   // Achtung isLinearly_In_dependent vs isLinearlyDependent

//3. Berechnung der durch die 2 linear unabhängigen Vektoren aufgespannten Ebene E:
//    vwmPlaneOW.getPlaneEquation(…)

//4. Berechnung der Schnittpunktmenge des Strahls S und der Ebene E:
//    if (vwmPlaneOW.isPlaneLineIntersection(vwmDir3D))

//5. Überprüfung, ob die Schnittpunktmenge von S und E ein Punkt P ist:
//    if (vwmPlaneOW.getPlaneLineSkalar(vwmCoord,vwmDir3D) > 0)

//6. Im Fall #Schnittpunktmenge=1: Überprüfung, ob der Punkt P im Polygon liegt.
//    if (vwmPolygonOW->calcPointInPolygon(posOW))

//7. Im Fall „Punkt in Polygon“: Berechnung Abstand von BSM zum „Punkt im Polygon“.
//    dist = vwmPlaneOW.distPlaneToPoint(vwmCoord);

//8. Berechnung Abstände BSM zum „Punkt im Polygon“ für alle OuterWalls.

//9. Bestimmung OuterWall OW (getroffene Außenwand) mit dem kürzesten Abstand BSM zum Punkt
//    im OuterWall-Polygon.

//10. HitRoomID = Raumnummer des Raums, zu dem die OuterWall OW gehört.

HitPosition checkBsmPosition(const LtsUtils::QPoint3D &position, const LtsUtils::QPoint3D &bsmViewDirection, const QMap<int, Room> &rooms)
{
  NGL::INGLLogger &l = logger();
  const LtsUtils::QPoint3D direction(bsmViewDirection * -1);
  INFO_LOG(l) << "XXXXX Searching nearest room for bsm position " << position
              << ", bsmViewDirection " << bsmViewDirection
              << ", hit direction " << direction
              << ", #rooms = " << rooms.size();
  HitPosition hitPos;

  for (const Room &room : rooms)
  {
    DEBUG_LOG(l) << "room[" << room.id() << "] #walls = " << room.outerWalls().size();
    for (int idxOuterWall = 0; idxOuterWall < room.outerWalls().size(); ++idxOuterWall)
    {
      const OuterWall &wall = room.outerWalls().at(idxOuterWall);
      DEBUG_LOG(l) << "room[" << room.id() << "]" << " wall[" << idxOuterWall << "] #elements = " << wall.elements().size();
      for (int idxWallElement = 0; idxWallElement < wall.elements().size(); ++idxWallElement)
      {
        const WallElement &wallElement = wall.elements().at(idxWallElement);
        DEBUG_LOG(l) << "room[" << room.id() << "]" << "  wallElement[" << idxWallElement << "] #points = " << wallElement.size();
        if (wallElement.size() < 3)
          continue;

        // fix wallElement regarding polyIsLinearlyDependent
        // [GUEZE-483] RPL Nr 756: Steilfeuer Mörser auf Gebäude
        const QVector<LtsUtils::QPoint3D> &localWallElement = getCoordsForPlane(wallElement);
        DEBUG_LOG(l) << "room[" << room.id() << "]" << "  wallElement[" << idxWallElement << "] #points = " << localWallElement.size() << " after removing Linearly Dependent)";

        if (localWallElement.size() < 3)
        {
          ERROR_LOG(l) << "localWallElement are not sufficiant";
          for (auto coord : wallElement)
            ERROR_LOG(l) << "wallElement are not sufficiant: << " << coord;
          for (auto coord : qAsConst(localWallElement))
            ERROR_LOG(l) << "localWallElement are not sufficiant: << " << coord;
          continue;
        }

        for (auto pointOfWallElementLinear : qAsConst(localWallElement))
          DEBUG_LOG(l) << "room[" << room.id() << "]" << " wall[" << idxOuterWall << "] " << " wallElement[" << idxWallElement << "] #points = " << localWallElement.size() << ", pointOfWallElementLinear=" << pointOfWallElementLinear;

        const VwmPlane vwmPlaneOW(localWallElement.at(0), localWallElement.at(1), localWallElement.at(2));
        DEBUG_LOG(l) << "room[" << room.id() << "]" << " wall[" << idxOuterWall << "]" << " wallElement[" << idxWallElement << "]"
                     << ", planeOW params: p1 = " << vwmPlaneOW.pos()
                     << ", p2 = " << vwmPlaneOW.vec1()
                     << ", p3 = " << vwmPlaneOW.vec2()
                     << ", det = " << vwmPlaneOW.det()
                     << ", C = " << vwmPlaneOW.planeC()
                     << ", vwmPlaneOW.isVertical=" << vwmPlaneOW.isVertical();




        if (!vwmPlaneOW.getPlaneEquation())
          continue;
        DEBUG_LOG(l) << "   getPlaneEquation() = true";

        if (!vwmPlaneOW.isPlaneLineIntersection(direction))
          continue;
        DEBUG_LOG(l) << "   isPlaneLineIntersection() = true  -> direction=" << direction;

        // only check for orthogonal, do not check for correct direction (in contrast to getHitPosition() above)
        if (vwmPlaneOW.isPlaneLineOrthogonal(direction))
          continue;



        const LtsUtils::QPoint3D posOW = vwmPlaneOW.getPlaneLineIntersection(position, direction);
        DEBUG_LOG(l) << "   point to check: " << posOW;

        if (!isPointInPolygon(localWallElement, posOW))
          continue;
        DEBUG_LOG(l) << "   isPointInPolygon() = true";

        const double curDistOW = vwmPlaneOW.distPlaneToPoint(position, direction);
        const double absDistOW = fabs(curDistOW);
        DEBUG_LOG(l) << "   distance = " << curDistOW << " - room " << room.id();
        if (absDistOW > fabs(hitPos.distance))
          continue;
        hitPos.distance = curDistOW;
        hitPos.state = HitPosition::State::OuterWall;
        hitPos.roomId = room.id();
        hitPos.outerWallIdx = idxOuterWall;
        DEBUG_LOG(l) << " XXXXX  nearest distance = " << curDistOW << " on outer wall in room " << room.id()
                     << "room[" << room.id() << "]" << " wall[" << idxOuterWall << "]" << " wallElement[" << idxWallElement << "]";
        // we don't care about exclusion zones since they should be on the same plane as the wall

      } // for
    } // for
  } // for
  INFO_LOG(l) << "XXXXX HitPosition: " << hitPos.state
              << ", roomId = " << hitPos.roomId
              << ", outerWallIndex = " << hitPos.outerWallIdx
              << ", exclusionZoneIndex = " << hitPos.exclusionZoneIdx;
  return hitPos;
}


///////////////////////////////////////////////////////////////////////////////////////////////////
// Intersection methods
//

LtsUtils::QPoint3D getIntersectionInternal(
  const QVector<LtsUtils::QPoint3D> &planePoints, const LtsUtils::QPoint3D &position, const LtsUtils::QPoint3D &direction
)
{
  NGL::INGLLogger &l = logger();
  if (planePoints.size() < 3)
  {
    WARNING_LOG(l) << "Wall with fewer than 3 points - modelling error!";
    return invalidPosition();
  }
  const QVector<LtsUtils::QPoint3D> &localPoly = getCoordsForPlane(planePoints);

  if (localPoly.size() < 3)
  {
    ERROR_LOG(l) << "planePoints are not sufficiant";
    for (auto coord : qAsConst(planePoints))
      ERROR_LOG(l) << "planePoints are not sufficiant: << " << coord;
    for (auto coord : qAsConst(localPoly))
      ERROR_LOG(l) << "localPoly are not sufficiant: << " << coord;
    return invalidPosition();
  }

  const VwmPlane vwmPlane(localPoly.at(0), localPoly.at(1), localPoly.at(2));
  if (l.isDebugEnabled())
  {
    for ( int i = 0; i < localPoly.size(); ++i)
      DEBUG_LOG(l) << "vwmPlane at(" << i << ")=" << localPoly.at(i).x() << "/" << localPoly.at(i).y() << "/"
                   << localPoly.at(i).z();
    DEBUG_LOG(l) << "plane params: p1 = " << vwmPlane.pos() << ", p2 = " << vwmPlane.vec1() << ", p3 = " << vwmPlane.vec2()
                 << ", det = " << vwmPlane.det() << ", C = " << vwmPlane.planeC();
  } // if

  if (!vwmPlane.getPlaneEquation())
    return invalidPosition();
  DEBUG_LOG(l) << "getPlaneEquation() = true";

  if (!vwmPlane.isPlaneLineIntersection(direction))
    return invalidPosition();
  DEBUG_LOG(l) << "isPlaneLineIntersection() = true";

  const double planeLineScalar = vwmPlane.getPlaneLineSkalar(position, direction);
  if (planeLineScalar <= 0)
    return invalidPosition();
  DEBUG_LOG(l) << "getPlaneLineSkalar() = " << planeLineScalar;

  const LtsUtils::QPoint3D pos = vwmPlane.getPlaneLineIntersection(position, direction);

  bool bIsPointInPolygon = isPointInPolygon(localPoly, pos);
  DEBUG_LOG(l) << "bIsPoinliotInPolygon = " << bIsPointInPolygon;

  return bIsPointInPolygon ? pos : invalidPosition();
}


LtsUtils::QPoint3D intersection(const LtsUtils::QPoint3D &start, const LtsUtils::QPoint3D &end, const QMap<int, Room> &rooms)
{
  NGL::INGLLogger &l = logger();
  DEBUG_LOG(l) << "Searching for intersection: start = " << start << ", end = " << end;

  LtsUtils::QPoint3D intersectionPoint = invalidPosition();
  double distance = (end - start).length();
  const LtsUtils::QPoint3D direction = (end - start).normalized();

  DEBUG_LOG(l) << "Searching for intersection: START"
               << ", intersectionPoint = " << intersectionPoint
               << ", distance = " << distance
               << ", direction = " << direction;

  int cnt = 0;
  int roomId4Debugging = 1;
  for (const Room &room : qAsConst(rooms))
    for (const OuterWall &outerWall : qAsConst(room.outerWalls()))
      for (const WallElement &wallElement : qAsConst(outerWall.elements()))
      {
        const LtsUtils::QPoint3D point = getIntersectionInternal(wallElement, start, direction);
        DEBUG_LOG(l) << "getIntersectionInternal:"
                     << " cnt = " << cnt++
                     << " room.id = " << room.id()
                     << "  -  point=" << point;
        if (point == invalidPosition())
          continue;
        const double dist = (point - start).length();
        if (dist < distance)
        {
          DEBUG_LOG(l) << "Intersection found for room " << room.id() << ": " << point;
          intersectionPoint = point;
          distance = dist;
          roomId4Debugging =room.id();
        } // if
      } // for

  if (roomId4Debugging > 0)
    DEBUG_LOG(l) << "Searching for intersection: FOUND"
                 << ", intersectionPoint = " << intersectionPoint
                 << ", distance = " << distance
                 << ", direction = " << direction
                 << ", at roomId=" << roomId4Debugging;

  return intersectionPoint;
}


bool hasIntersection(const LtsUtils::QPoint3D &start, const LtsUtils::QPoint3D &end, const QMap<int, Room> &rooms)
{
  const double distance = (end - start).length();
  const LtsUtils::QPoint3D direction = (end - start).normalized();
  for (const Room &room : qAsConst(rooms))
    for (const OuterWall &outerWall : qAsConst(room.outerWalls()))
      for (const WallElement &wallElement : qAsConst(outerWall.elements()))
      {
        const LtsUtils::QPoint3D point = getIntersectionInternal(wallElement, start, direction);
        if (point == invalidPosition())
          continue;
        if ((start - point).length() > distance)
          continue;
        return true;
      } // for
  return false;
}


LtsUtils::QPoint3D invalidPosition()
{
  return {100000., 100000., -10000.};
}

} // namespace Math
} // namespace VWM
} // namespace BuildingControlUnit