/**
  * @RDE_HEADER@
  * Small unit test for math calculations in vwm
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, SLSE, 2016-01-30
  */

#include <nglapplication/nglcoreapplication.h>
#include <QtTest/QtTest>

#include "vwm_math_unit_test.h"
#include "vwm_math.h"
#include "vwm_building.h" // MakePlanar

#include <QVector3D>

int main(int argc, char *argv[])
{
  NGLCoreApplication app(argc, argv);
  app.createLogger("BCU", 1);
  VwmMathUnitTest tc;
  return QTest::qExec(&tc, argc, argv);
}

namespace QTest
{
template<>
char *toString(const LtsUtils::QPoint3D &p)
{
  std::ostringstream s; s << p;
  const std::string str = s.str();
  char *ret = new char[str.size() + 1];
  strcpy(ret, str.c_str());
  return ret;
}

template<>
char *toString(const BuildingControlUnit::VWM::HitPosition::State &s)
{
  const std::string str = std::to_string(static_cast<int>(s));
  char *ret = new char[str.size() + 1];
  strcpy(ret, str.c_str());
  return ret;
}
}


void VwmMathUnitTest::testRotation()
{
  const LtsUtils::QPoint3D pIn(1, 2, 3);
  const LtsUtils::QPoint3D pRot1 = BuildingControlUnit::VWM::Math::rotateAroundYAxis(pIn, 0.12);
  const LtsUtils::QPoint3D pRot2 = BuildingControlUnit::VWM::Math::rotateAroundZAxis(pIn, 0.12);

  const LtsUtils::QPoint3D pExp1 = LtsUtils::QPoint3D(0.6336720139871, 2.0000000000000, 3.0981381148505);
  const LtsUtils::QPoint3D pExp2 = LtsUtils::QPoint3D(1.2322330504317, 1.8659050644199, 3.0000000000000);
  std::ostringstream s1, s2;
  s1 << "Is: " << pRot1 << ", Expected: " << pExp1;
  s1 << "Is: " << pRot2 << ", Expected: " << pExp2;

  QVERIFY2(qFuzzyCompare(pRot1, pExp1), s1.str().c_str());
  QVERIFY2(qFuzzyCompare(pRot2, pExp2), s2.str().c_str());
}

// taken from real world: bldg_1104, room 401, wallIdx 52
static const QVector<LtsUtils::QPoint3D> polyBldg1104_401_52({
                                                               {11.187,  0.000, 10.280}, // dependent1
                                                               {11.187,  0.400, 10.268}, // dependent1
                                                               {11.187,  2.517, 10.204}, // dependent1
                                                               {11.187,  4.362, 10.147}, // dependent1
                                                               { 0.000,  4.362, 10.147}, // dependent2
                                                               { 0.000,  0.403, 10.268}, // dependent2
                                                               { 0.000,  0.000, 10.280}, // dependent2
                                                               {10.766,  0.000, 10.280}, // dependent3
                                                               {10.787,  0.000, 10.280}  // dependent3
                                                             });

static const QVector<LtsUtils::QPoint3D> polyBldg1104_401_52_adapted01({
                                                                         {11.187,  0.000, 10.280},
                                                                         {11.187,  0.400, 10.268},
                                                                         {11.187,  2.517, 10.204}, // copied
                                                                         {11.187,  2.517, 10.204}, // copied
                                                                         {11.187,  2.517, 10.204}, // copied
                                                                         {11.187,  4.362, 10.147},
                                                                         { 0.000,  4.362, 10.147},
                                                                         { 0.000,  0.403, 10.268},
                                                                         { 0.000,  0.000, 10.280},
                                                                         {10.766,  0.000, 10.280},
                                                                         {10.787,  0.000, 10.280}
                                                                       });

// taken from real world: bldg_salchau_577, room 201, wallIdx 1(?)
static const QVector<LtsUtils::QPoint3D> polyBldg577_201_1({
                                                           {0.000,  2.167, -0.180},
                                                           {0.001,  4.506, -0.180},
                                                           {0.001,  4.506, 2.278},
                                                           {0.001,  4.506, 2.341},
                                                           {0.001,  4.506, 2.520},
                                                           {0.000,  4.506, 2.520}
                                                         });

// taken from real world: bldg_salchau_577, room 201, wallIdx 5(?)
static const QVector<LtsUtils::QPoint3D> polyBldg577_201_5({
                                                           {0.001,  4.506, -0.180},
                                                           {0.001,  6.233, -0.180},
                                                           {0.001,  6.233, 2.520},
                                                           {0.001,  4.506, 2.520},
                                                           {0.001,  4.506, 2.278},
                                                           {0.001,  4.506, 0.713}
                                                         });

// taken from real world: bldg_1116, room 206, wallIdx 8(?)
static const QVector<LtsUtils::QPoint3D> polyBldg1116_206_8({
                                                              {21.226,  11.995, 6.431},
                                                              {23.858,  11.995, 6.431},
                                                              {24.038,  11.995, 6.431},
                                                              {24.038,  11.995, 6.223},
                                                              {23.858,  11.995, 6.223},
                                                              {21.226,  11.995, 6.223}
                                                            });


// taken from real world: bldg_1116, room 206, wallIdx 8(?)
static const QVector<LtsUtils::QPoint3D> polyBldg1116_206_9({
                                                              {24.038,  11.995, 6.431},
                                                              {23.858,  11.995, 6.431}, // dependent / will be removed
                                                              {21.226,  11.995, 6.431},
                                                              {21.226,  11.995, 6.449}
                                                            });

static const QVector<LtsUtils::QPoint3D> polyBldg1116_206_X({
                                                              {24.038,  11.815, 3.210},  // GUEZE-672: Kristina hat den Polygonzug händisch korrigiert: r 4672.
                                                              {24.858,   5.896, 3.210},
                                                              {24.038,   5.896, 6.393},
                                                              {24.038,   6.076, 6.393},
                                                              {24.038,   7.715, 5.513}
                                                            });


static const QVector<LtsUtils::QPoint3D> polyBldg3101_306_149({
                                                              {10.905,  16.003, 5.480},
                                                              {12.277,  16.003, 5.480},  // dependent / will be removed
                                                              {17.839,  16.004, 5.480},
                                                              {17.839,  16.004, 8.040},
                                                              {10.905,  16.003, 8.040}
                                                            });
static const QVector<LtsUtils::QPoint3D> polyBldg3101_506_276({
                                                              {10.905,  16.003, 10.990},
                                                              {12.277,  16.003, 10.990},  // dependent / will be removed
                                                              {17.839,  16.005, 10.990},
                                                              {17.839,  16.005, 13.602},
                                                              {10.905,  16.003, 13.602}
                                                            });

static const QVector<LtsUtils::QPoint3D> polyBldg3102_402_210({
                                                                {0.180,  9.676, 5.420},
                                                                {0.525,  9.676, 5.420},  // dependent / will be removed
                                                                {2.201,  9.676, 5.420},
                                                                {2.201,  9.676, 7.910},
                                                                {0.180,  9.676, 7.910},
                                                            });


static const QVector<LtsUtils::QPoint3D> polyBldgUBahn_303_333({
                                                                 {140.006,  3.988, -1.945},
                                                                 {140.006,  3.407, -1.945}, // dependent / will be removed
                                                                 {140.006,  0.594, -1.945},
                                                                 {140.006,  0.594, 0.545},
                                                                 {140.006,  3.988, 0.545},
                                                                });

// BuildingControlTestVwmXml: Bldg1108
// Polygon point 2 ((-0,15.979,0)) and 6 ((-0,15.979,0)) in room 2103, room[5], element[0] are the same! (Buildings/BuildingControlUnit/VWM/vwm_building.cpp:554)
static const QVector<LtsUtils::QPoint3D> polyBldg1108_103_X({
                                                              {-0.000, 15.979, 2.278}, // idx=0
                                                              {-0.000, 15.979, 0.975}, // idx=1 // problem 1: linear dependent
                                                              {-0.000, 15.979, 0.000}, // idx=2
                                                              { 0.180, 15.979, 0.000}, // idx=3
                                                              { 0.180, 15.979, 2.490}, // idx=4
                                                              {-0.000, 15.979, 2.490}, // idx=5
                                                              {-0.000, 15.979, 0.000}  // idx=6 identical to idx=2 - problem???
                                                            });

// BuildingControlTestVwmXml: Bldg3101
static const QVector<LtsUtils::QPoint3D> polyBldg3101_510_309({
                                                                { 7.291, -0.225, 13.744}, // idx=0
                                                                {10.726, -0.225, 13.744}, // idx=1
                                                                {10.726,  4.462, 13.869}, // idx=2
                                                                { 7.291,  4.462, 13.869}  // idx=3
                                                              });
// Bldg_3101, Room 608, wallIdx=1;
static const QVector<LtsUtils::QPoint3D> polyBldg3101_608_387_Org({
                                                                {10.726, 8.619, 16.787}, // idx=0
                                                                {10.726, 8.716, 16.786}, // idx=1
                                                                { 7.291, 8.716, 16.786}, // idx=2
                                                                { 7.291, 8.619, 16.787}, // idx=3 // problem 1: linear dependent, wenn z gleich wäre...
                                                                { 7.291, 4.281, 16.717}, // idx=4
                                                                {10.726, 4.281, 16.717}  // idx=5
                                                              });
static const QVector<LtsUtils::QPoint3D> polyBldg3101_608_387_Z_Gleichgezogen({
                                                                {10.726, 8.619, 16.786}, // idx=0
                                                                {10.726, 8.716, 16.786}, // idx=1
                                                                { 7.291, 8.716, 16.786}, // idx=2
                                                                { 7.291, 8.619, 16.786}, // idx=3 // problem 1: linear dependent, wenn z gleich wäre...
                                                                { 7.291, 4.281, 16.786}, // idx=4
                                                                {10.726, 4.281, 16.786}  // idx=5
                                                              });

static const QVector<LtsUtils::QPoint3D> polyBldg3101_608_387_Z_ROUNDED({
                                                                {10.73, 8.62, 16.79}, // idx=0
                                                                {10.73, 8.72, 16.79}, // idx=1
                                                                { 7.30, 8.72, 16.79}, // idx=2
                                                                { 7.39, 8.62, 16.79}, // idx=3 // problem 1: linear dependent, wenn z gleich wäre...
                                                                { 7.39, 4.28, 16.79}, // idx=4
                                                                {10.73, 4.28, 16.79}  // idx=5
                                                              });


static const QVector<LtsUtils::QPoint3D> polyBldg1103_205_5_ORG({
                                                              {11.639, 6.026, 5.412}, // idx=0
                                                              {11.639, 2.607, 5.513}, // idx=1 // problem: linear dependent -> wird erkannt
                                                              {11.639, 1.690, 5.540}, // idx=2
                                                              {14.659, 1.694, 5.539}, // idx=3
                                                              {14.659, 6.026, 5.412}  // idx=4
                                                            });
static const QVector<LtsUtils::QPoint3D> polyBldg1103_205_5_ROUNDED({
                                                              {11.64, 6.03, 5.41}, // idx=0
                                                              {11.64, 2.61, 5.51}, // idx=1    // problem: linear dependent -> wird erkannt
                                                              {11.64, 1.69, 5.54}, // idx=2
                                                              {14.66, 1.69, 5.54}, // idx=3
                                                              {14.66, 6.03, 5.41}  // idx=4
                                                            });

// 2021-10:
static const QVector<LtsUtils::QPoint3D> polyBldg1131_201_3({ // sollte eigentlich waagerechte Wand darstellen
                                                              {0.000, 5.987, 2.689}, // idx=0
                                                              {0.000, 5.791, 2.695}, // idx=1 // idx=1 "stört"
                                                              {0.000, 0.170, 2.859}, // idx=2
                                                              {3.098, 0.170, 2.859}, // idx=3
                                                              {3.098, 5.791, 2.695}, // idx=4
                                                              {2.928, 5.987, 2.689}  // idx=5
                                                            });

const LtsUtils::QPoint3D pIn(10.6477, 6.13499, 10000);


void VwmMathUnitTest::testIsLinearlyDependent()
{
  //
  // Pay attention to 'epsilon' defined at lib: (like #define epsilon 0.002)
  //
  bool bVecIsLinearlyDependent = false;

  {
    // first 2 values are identical -> lamdba at vecIsLinearlyDependent could not be determined
    // check return value of that case:
    /*
     double lambda;
      ...
     else if (fabs(v1.z()) > epsilon)
       lambda = v2.z() / v1.z();
     else
      return true; // dependent <- check this one
     */
    const QVector<LtsUtils::QPoint3D> poly({{0.000,  0.000, 0.},  // dependent
                                            {0.000,  0.000, 0.},
                                            {1.000,  2.000, 0.}
                                           });
    const LtsUtils::QPoint3D &v1 = poly.at(1) - poly.at(0);
    const LtsUtils::QPoint3D &v2 = poly.at(2) - poly.at(0);
    bVecIsLinearlyDependent = BuildingControlUnit::VWM::Math::vecIsLinearlyDependent(v1, v2);
    QCOMPARE(bVecIsLinearlyDependent, true);
  }

  {
    const QVector<LtsUtils::QPoint3D> poly({{0.000,  0.000, 0.},  // dependent
                                            {0.000,  1.000, 0.},
                                            {0.000,  2.000, 0.}
                                           });
    const LtsUtils::QPoint3D &v1 = poly.at(1) - poly.at(0);
    const LtsUtils::QPoint3D &v2 = poly.at(2) - poly.at(0);
    bVecIsLinearlyDependent = BuildingControlUnit::VWM::Math::vecIsLinearlyDependent(v1, v2);
    QCOMPARE(bVecIsLinearlyDependent, true);
  }
  {
    const QVector<LtsUtils::QPoint3D> poly({{0.000,  0.000, 0.},  // dependent
                                            {0.000,  1.000, 0.},
                                            {0.000,  2.000, 0.},
                                            {0.000,  3.000, 0.},
                                            {0.000,  4.000, 0.}
                                           });
    const LtsUtils::QPoint3D &v1 = poly.at(1) - poly.at(0);
    const LtsUtils::QPoint3D &v2 = poly.at(2) - poly.at(0);
    bVecIsLinearlyDependent = BuildingControlUnit::VWM::Math::vecIsLinearlyDependent(v1, v2);
    QCOMPARE(bVecIsLinearlyDependent, true);
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1104_401_52;
    const LtsUtils::QPoint3D &v1 = polyIN.at(1) - polyIN.at(0);
    const LtsUtils::QPoint3D &v2 = polyIN.at(2) - polyIN.at(0);
    bVecIsLinearlyDependent = BuildingControlUnit::VWM::Math::vecIsLinearlyDependent(v1, v2);
    QCOMPARE(bVecIsLinearlyDependent, true);
  }

  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1104_401_52_adapted01;
    const LtsUtils::QPoint3D &v1 = polyIN.at(1) - polyIN.at(0);
    const LtsUtils::QPoint3D &v2 = polyIN.at(2) - polyIN.at(0);
    bVecIsLinearlyDependent = BuildingControlUnit::VWM::Math::vecIsLinearlyDependent(v1, v2);
    QCOMPARE(bVecIsLinearlyDependent, true);
  }

  {
    const QVector<LtsUtils::QPoint3D> poly({{0.000,  0.000, 0.},  // _in_dependent
                                            {0.000,  1.000, 0.},
                                            {1.000,  1.000, 0.},
                                            {1.000,  2.000, 0.},
                                            {2.000,  2.000, 0.}
                                           });
    const LtsUtils::QPoint3D &v1 = poly.at(1) - poly.at(0);
    const LtsUtils::QPoint3D &v2 = poly.at(2) - poly.at(0);
    bVecIsLinearlyDependent = BuildingControlUnit::VWM::Math::vecIsLinearlyDependent(v1, v2);
    QCOMPARE(bVecIsLinearlyDependent, false);
  }
}

void printPoly(QVector<LtsUtils::QPoint3D> value)
{
  std::ostringstream stream;
  stream << "QVector<QPoint3D>(" << value.size() << "): ";
  for (LtsUtils::QPoint3D i : value)
    stream << i << ";";

  std::cout << stream.str() << std::endl;
}

void VwmMathUnitTest::getCoordsForPlane()
{
  {
    const BuildingControlUnit::VWM::WallElement we({{0.000,  0.000, 0.}, // dependent
                                                    {0.000,  1.000, 0.},
                                                    {0.000,  2.000, 0.}
                                                   });
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(we);
    QCOMPARE(planeCoords.size(), 2); // 2 since it´s dependent
  }
  {
    const BuildingControlUnit::VWM::WallElement we({{0.000,  0.000, 0.}, // _in_dependent
                                                    {0.000,  1.000, 0.},
                                                    {1.000,  1.000, 0.}
                                                   });
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(we);
    QCOMPARE(planeCoords.size(), 3);
  }
  {
    const BuildingControlUnit::VWM::WallElement we({{0.000,  0.000, 0.}, // _in_dependent
                                                    {0.000,  1.000, 0.},
                                                    {1.000,  1.000, 0.},
                                                    {1.000,  0.000, 0.}
                                                   });
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(we);
    QCOMPARE(planeCoords.size(), 4);
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1104_401_52;
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 5); // using epsilon 0.002
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1104_401_52_adapted01;
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 5); // using epsilon 0.002
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg577_201_1;
    // printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    // printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 3); // using epsilon 0.002
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg577_201_5;
    // printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    // printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 5); // using epsilon 0.002
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1116_206_8;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 4); // using epsilon 0.002
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1116_206_9;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 3); // using epsilon 0.002
  }

  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1116_206_X;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 5); // using epsilon 0.002
  }


  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg3101_306_149;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 4); // using epsilon 0.002
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg3101_506_276;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 4); // using epsilon 0.002
  }

  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg3102_402_210;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 4); // using epsilon 0.002
  }

  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldgUBahn_303_333;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 4); // using epsilon 0.002
  }

  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1108_103_X;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 6); // using epsilon 0.002
  }

  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg3101_510_309;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 4); // using epsilon 0.002
  }

  // polyBldg3101
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg3101_608_387_Org;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 6); // using epsilon 0.002
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg3101_608_387_Z_Gleichgezogen;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 5); // using epsilon 0.002
  }


  // Aufgrund des Rundens auf cm kann scheinbar nicht mehr die lineare Abhängigkeit entfernt werden :-(
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1103_205_5_ORG;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 4); // using epsilon 0.002
  }
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1103_205_5_ROUNDED;
    printPoly(polyIN);
    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(polyIN);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 5); // using epsilon 0.002
  }

  //
  // 2021-11-03: MakePlanar is introduced
  //

  // 2021-11-03:
  {
    const QVector<LtsUtils::QPoint3D> &polyIN = polyBldg1131_201_3;
    printPoly(polyIN);

    // add makePlanar (is typically done at Building::cleanupPolygons()
    BuildingControlUnit::VWM::MakePlanar makePlanar(polyIN);
    QVector<LtsUtils::QPoint3D> planarPoly = makePlanar.calculate();

    QVector<LtsUtils::QPoint3D> planeCoords = BuildingControlUnit::VWM::Math::getCoordsForPlane(planarPoly);
    printPoly(planeCoords);
    std::cout << " IN: " << polyIN.size() << " RESULT: " << planeCoords.size() << std::endl;
    QCOMPARE(planeCoords.size(), 5); // using epsilon 0.002
  }
}


void VwmMathUnitTest::testPlane()
{
  QVector<LtsUtils::QPoint3D> poly({{3.629, 0, 2.53},
                                    {    0, 0, 2.53},
                                    {    0, 0, 0},
                                    {3.627, 0, 0}});

  const LtsUtils::QPoint3D pIn(1, 0, 1);
  const LtsUtils::QPoint3D pOut(1, 0.2, 1);
  QVERIFY2(BuildingControlUnit::VWM::Math::isPointInPolygon(poly, pIn), "Point is not in polygon!");
  QVERIFY2(!BuildingControlUnit::VWM::Math::isPointInPolygon(poly, pOut), "Point is in polygon!");


  LtsUtils::QPoint3D p1(100, 100, 3);
  LtsUtils::QPoint3D p2(  2,   3, 4);
  LtsUtils::QPoint3D p3(  3,   4, 5);
  BuildingControlUnit::VWM::Math::VwmPlane plane(p1, p2, p3);
  QVERIFY2(plane.getPlaneEquation(), "Can not determine plane equation ...");
  QCOMPARE(plane.det(), LtsUtils::QPoint3D(-98,99,-1));
  QCOMPARE(plane.planeC(), 97.);

  LtsUtils::QPoint3D vec(1, 0, 2);
  LtsUtils::QPoint3D pos(1, 3, 7);
  QVERIFY2(plane.isPlaneLineIntersection(vec), "no intersection - error!");
  QCOMPARE(plane.getPlaneLineSkalar(pos, vec), 0.95);
  QCOMPARE(plane.getPlaneLineIntersection(pos, vec), LtsUtils::QPoint3D(1.95,3,8.9));
}

void VwmMathUnitTest::testBsmHit()
{
  // what went wrong until now:
  // rotation direction of wall polygon affected distance calculation (direction of hit/bsm was not taken into account)
  // rotation direction of wall polygon affected point in polygon calculation (plane vectors were not moved to 0/0/0)

  // clockwise
  const BuildingControlUnit::VWM::WallElement we1({{4.486, -0.003, 0.111},
                                                   {0.000,  0.000, 0.111},
                                                   {0.000,  0.000, 2.910},
                                                   {4.486, -0.003, 2.910}});

  // counter-clockwise
  const BuildingControlUnit::VWM::WallElement we2({{0.000,  0.000, 0.111},
                                                   {4.486, -0.003, 0.111},
                                                   {4.486, -0.003, 2.910},
                                                   {0.000,  0.000, 2.910}});

  BuildingControlUnit::VWM::OuterWall ow1;
  ow1.elements().push_back(we1);
  BuildingControlUnit::VWM::OuterWall ow2;
  ow2.elements().push_back(we2);

  BuildingControlUnit::VWM::Room r1;
  r1.setId(41);
  r1.outerWalls().push_back(ow1);
  BuildingControlUnit::VWM::Room r2;
  r2.setId(42);
  r2.outerWalls().push_back(ow2);

  const QMap<int, BuildingControlUnit::VWM::Room> rooms1({{r1.id(), r1}});
  const QMap<int, BuildingControlUnit::VWM::Room> rooms2({{r2.id(), r2}});

  const LtsUtils::QPoint3D bsmPos1(2.344, +0.018, 2.799);
  const LtsUtils::QPoint3D bsmPos2(2.344, -0.018, 2.799);
  const LtsUtils::QPoint3D bsmDir1(0, +1, 0);
  const LtsUtils::QPoint3D bsmDir2(0, -1, 0);

  // clockwise polygon: rooms1
  // counter-clockwise polygon: rooms2
  for (const auto &rooms : std::vector<QMap<int, BuildingControlUnit::VWM::Room>>({rooms1, rooms2}))
  {
    {
      // ok
      BuildingControlUnit::VWM::HitPosition hp1 = BuildingControlUnit::VWM::Math::checkBsmPosition(bsmPos1, bsmDir1, rooms);
      std::cout << hp1 << std::endl;
      QCOMPARE(hp1.state, BuildingControlUnit::VWM::HitPosition::State::OuterWall);
      QVERIFY2(hp1.distance > 0, "Distance must be greater zero");
      // does not hit since direction is wrong
      BuildingControlUnit::VWM::HitPosition hp2 = BuildingControlUnit::VWM::Math::checkBsmPosition(bsmPos1, bsmDir2, rooms);
      std::cout << hp2 << std::endl;
      QCOMPARE(hp2.state, BuildingControlUnit::VWM::HitPosition::State::OuterWall);
      QVERIFY2(hp2.distance < 0, "Distance must be smaller zero");
      // does not hit since position (y) behind wall
      BuildingControlUnit::VWM::HitPosition hp3 = BuildingControlUnit::VWM::Math::checkBsmPosition(bsmPos2, bsmDir1, rooms);
      std::cout << hp3 << std::endl;
      QCOMPARE(hp3.state, BuildingControlUnit::VWM::HitPosition::State::OuterWall);
      QVERIFY2(hp3.distance < 0, "Distance must be smaller zero");
      // does hit since direction and position is changed
      BuildingControlUnit::VWM::HitPosition hp4 = BuildingControlUnit::VWM::Math::checkBsmPosition(bsmPos2, bsmDir2, rooms);
      std::cout << hp4 << std::endl;
      QCOMPARE(hp4.state, BuildingControlUnit::VWM::HitPosition::State::OuterWall);
      QVERIFY2(hp4.distance > 0, "Distance must be greater zero");
    }
    {
      // hitDirection = bsmDirection * -1
      // ok
      BuildingControlUnit::VWM::HitPosition hp1 = BuildingControlUnit::VWM::Math::getHitPosition(bsmPos1, bsmDir1 * -1, rooms);
      std::cout << hp1 << std::endl;
      QCOMPARE(hp1.state, BuildingControlUnit::VWM::HitPosition::State::OuterWall);
      QVERIFY2(hp1.distance > 0, "Distance must be greater zero");
      // does not hit since direction is wrong
      BuildingControlUnit::VWM::HitPosition hp2 = BuildingControlUnit::VWM::Math::getHitPosition(bsmPos1, bsmDir2 * -1, rooms);
      std::cout << hp2 << std::endl;
      QCOMPARE(hp2.state, BuildingControlUnit::VWM::HitPosition::State::NoHit);
      // does not hit since position (y) behind wall
      BuildingControlUnit::VWM::HitPosition hp3 = BuildingControlUnit::VWM::Math::getHitPosition(bsmPos2, bsmDir1 * -1, rooms);
      std::cout << hp3 << std::endl;
      QCOMPARE(hp3.state, BuildingControlUnit::VWM::HitPosition::State::NoHit);
      // does hit since direction and position is changed
      BuildingControlUnit::VWM::HitPosition hp4 = BuildingControlUnit::VWM::Math::getHitPosition(bsmPos2, bsmDir2 * -1, rooms);
      std::cout << hp4 << std::endl;
      QCOMPARE(hp4.state, BuildingControlUnit::VWM::HitPosition::State::OuterWall);
      QVERIFY2(hp1.distance > 0, "Distance must be greater zero");
    }
  }
}

void VwmMathUnitTest::testSpecialCase()
{
  // special: is 10.6477,6.13499 in polygon polyBldg3101_510_309???

  LtsUtils::QPoint3D pIn(10.6477, 6.13499, 10000);

  // [GUEZE-605] / [GUEZE-707]
  static const QVector<LtsUtils::QPoint3D> polyBldg3101_608_387_Simple({
                                                                       {10.726, 8.716, 16.786}, // idx=1
                                                                       { 7.291, 8.716, 16.786}, // idx=2
                                                                       { 7.291, 4.281, 16.786}, // idx=4
                                                                       {10.726, 4.281, 16.786}  // idx=5
                                                                     });

//  const QVector<LtsUtils::QPoint3D> &polygon = polyBldg3101_608_387_Simple;
//  const QVector<LtsUtils::QPoint3D> &polygon = polyBldg3101_608_387_Z_Gleichgezogen;
//  const QVector<LtsUtils::QPoint3D> &polygon = polyBldg3101_608_387_Org;
  const QVector<LtsUtils::QPoint3D> &polygon = polyBldg3101_608_387_Z_ROUNDED;


  pIn.setZ(16.786);
  QVERIFY2(BuildingControlUnit::VWM::Math::isPointInPolygon(polygon, pIn), "Point is not in polygon!");

  QVERIFY2(BuildingControlUnit::VWM::Math::isPointInPolygon(polyBldg3101_608_387_Org, pIn), "Point is not in polygon!");


  // Searching nearest room for bsm position...
  const LtsUtils::QPoint3D position(10.6477, 6.13499, 10000);
  const LtsUtils::QPoint3D direction(0, 0, -1);


  BuildingControlUnit::VWM::Math::VwmPlane plane(polygon[0], polygon[1], polygon[2]);
  QVERIFY2(plane.getPlaneEquation(), "Can not determine plane equation ...");

  QVERIFY2(plane.isPlaneLineIntersection(direction), "no intersection - error!");

//  QVERIFY2(plane.isPlaneLineOrthogonal(direction), "not Orthogonal - error!");


  const LtsUtils::QPoint3D posOW = plane.getPlaneLineIntersection(position, direction);
  std::cout  << "   point to check: " << posOW.x() << std::endl;
  std::cout  << "   point to check: " << posOW.y() << std::endl;
  std::cout  << "   point to check: " << posOW.z() << std::endl;

  QVERIFY2(BuildingControlUnit::VWM::Math::isPointInPolygon(polygon, posOW), "Point is not in polygon!");
}

void VwmMathUnitTest::testSpecialCase2()
{
    // special: is 10.6477,6.13499 in polygon polyBldg3101_510_309???

    LtsUtils::QPoint3D pIn(10.6477, 6.13499, 10000);

    QVector<LtsUtils::QPoint3D> polygon = polyBldg3101_608_387_Org;

    pIn.setZ(16.786);
    QVERIFY2(BuildingControlUnit::VWM::Math::isPointInPolygon(polygon, pIn), "Point is not in polygon!");

    QVERIFY2(BuildingControlUnit::VWM::Math::isPointInPolygon(polyBldg3101_608_387_Org, pIn), "Point is not in polygon!");


    // Searching nearest room for bsm position...
    const LtsUtils::QPoint3D position(10.6477, 6.13499, 10000);
    const LtsUtils::QPoint3D direction(0, 0, -1);

    // Now the magic :
    BuildingControlUnit::VWM::MakePlanar makePlanar(polygon);
    polygon = makePlanar.calculate();  // now planar
    // : Now the magic

    BuildingControlUnit::VWM::Math::VwmPlane plane(polygon[0], polygon[1], polygon[2]);
    QVERIFY2(plane.getPlaneEquation(), "Can not determine plane equation ...");

    QVERIFY2(plane.isPlaneLineIntersection(direction), "no intersection - error!");

  //  QVERIFY2(plane.isPlaneLineOrthogonal(direction), "not Orthogonal - error!");


    const LtsUtils::QPoint3D posOW = plane.getPlaneLineIntersection(position, direction);
    std::cout  << "   point to check: " << posOW.x() << std::endl;
    std::cout  << "   point to check: " << posOW.y() << std::endl;
    std::cout  << "   point to check: " << posOW.z() << std::endl;

    QVERIFY2(BuildingControlUnit::VWM::Math::isPointInPolygon(polygon, posOW), "Point is not in polygon!");
}

//bool isLinearDependendCandiate
bool isLinearDependend_QVector3D(const QVector3D &v1,
                                 const QVector3D &v2,
                                 const QVector3D &v3)
{

  float distance1 = v2.distanceToPoint(v1);
  float distance2 = v2.distanceToPoint(v3);
  float distanceAll = v1.distanceToPoint(v3);
  float abweichung = (distance1+distance2) - distanceAll;
  std::cout  << "abweichung: " << abweichung << " - qFuzzyIsNull = " << std::boolalpha << qFuzzyIsNull(abweichung) << std::endl;

  return qFuzzyIsNull(
        (v2.distanceToPoint(v1) + v2.distanceToPoint(v3)) - v1.distanceToPoint(v3) // distanceToPoint returns 'float'
        );
}



void VwmMathUnitTest::testQVector3D()
{
  QVector3D v1(1, 1, 0.0);
  QVector3D v2(1, 2, 0.003); // 0.003 -> ..IsNull=true ; 0.0035 -> ..IsNull=false
  QVector3D v3(1, 3, 0.0);

  QCOMPARE(isLinearDependend_QVector3D(v1, v2, v3), true);

  //
  v2.setZ(0.0035);
  QCOMPARE(isLinearDependend_QVector3D(v1, v2, v3), false);
}