/**
  * @RDE_HEADER@
  * Manage and combine several ElevationMap
  * $Id$
  * $HeadURL$
  * $Author$
  */


#include "elevation_map_manager.h"
#include "elevation_map.h"

// Qt
#include <QPointer>
#include <QFileInfo>
#include <QRectF>
#include <QPolygonF>

// system
#include <vector>

namespace SituationMap
{

static long s_counter{0};

//------------------------------------------------------------------------------------------------------------
// ElevationMapManager::Private
//------------------------------------------------------------------------------------------------------------

class ElevationMapManager::Private
{
public:
  // fine to rough
  std::vector<ElevationMap *> m_elevationMapCont;
  QRectF                      m_simpleExtent;
};


//------------------------------------------------------------------------------------------------------------
// ElevationMapManager
//------------------------------------------------------------------------------------------------------------
ElevationMapManager::ElevationMapManager()
  : BaseClass()
  , d(new Private())
  , m_logger(getLogger("ElevationMapManager"))
{
  ++s_counter;
  TRACE_LOG(m_logger) << LOGFUNCTION << " START, s_counter=" << s_counter << " this=" << this;
  TRACE_LOG(m_logger) << LOGFUNCTION << " END, s_counter=" << s_counter << " this=" << this;
}

ElevationMapManager::~ElevationMapManager()
{
  --s_counter;
  TRACE_LOG(m_logger) << LOGFUNCTION << " START, s_counter=" << s_counter << " this=" << this;
  cleanupCurrentContent();
  delete d;
  d = nullptr;
  TRACE_LOG(m_logger) << LOGFUNCTION << " END, s_counter=" << s_counter << " this=" << this;
}
int ElevationMapManager::init(const QStringList &filePaths_rough_to_fine)
{
  NOTICE_LOG(m_logger) << LOGFUNCTION << " START" << " filePaths_rough_to_fine=" << filePaths_rough_to_fine.join(QStringLiteral(" ; "));

  cleanupCurrentContent();

  d->m_elevationMapCont.reserve(filePaths_rough_to_fine.size());
  int cnt = 0;
  for (const QString &filePath : filePaths_rough_to_fine)
  {
    QFileInfo fileInfo(filePath);

    const QString &fileName = fileInfo.absoluteFilePath();

    if (!fileInfo.exists())
    {
      ERROR_LOG(m_logger) << LOGFUNCTION << " elevation data file \'" << fileName << "\' doesn't exists.";
      continue;
    }

    if (!fileInfo.isReadable())
    {
      ERROR_LOG(m_logger) << LOGFUNCTION << " elevation data file \'" << fileName << "\' isn't readable.";
      continue;
    }

    // ctor will spawn thread via QtConcurrent and read file async:
    d->m_elevationMapCont.insert(d->m_elevationMapCont.begin(), new ElevationMap(fileInfo));
    ++cnt;
  } // for

  NOTICE_LOG(m_logger) << LOGFUNCTION << " END" << " filePaths_rough_to_fine=" << filePaths_rough_to_fine.join(QStringLiteral(" ; "))
                       << ", following number of files are going to be loaded: " << cnt;
  return cnt;
}

void ElevationMapManager::cleanupCurrentContent()
{
  qDeleteAll(d->m_elevationMapCont);
  d->m_elevationMapCont.clear();
}




//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------



bool ElevationMapManager::waitForInitialization()
{
  for (ElevationMap *elevationMap : d->m_elevationMapCont)
    elevationMap->waitForInitialization();
  return isValid();
}

bool ElevationMapManager::isValid() const
{
  bool allAreValid = true;
  for (const ElevationMap *elevationMap : d->m_elevationMapCont)
    allAreValid = allAreValid && elevationMap->isValid();

//  FATAL_LOG(m_logger) << LOGFUNCTION << " allAreValid = " << allAreValid;
  return allAreValid;
}

double ElevationMapManager::elevation(double lon_rad, double lat_rad, bool *bIsCovered) const
{
  // assume, that all children are ready
  for (const ElevationMap *elevationMap : d->m_elevationMapCont)
  {
//    qCritical() << " EMM::elevation:  "
//                << " lon_rad=" << lon_rad
//                << " lat_rad=" << lat_rad
//                << " (try: " << elevationMap->filename() << ")...";

    double singleResult = elevationMap->elevation(lon_rad, lat_rad);

    if (singleResult > -300) // -300 -> ausserhalb der Höhenkarte !!
    {
      // qCritical() << " EMM::elevation: elevation= " << singleResult << " (taken from: " << elevationMap->filename() << ")";
      if (bIsCovered)
        *bIsCovered = true;
      return singleResult;
    }
  }

//  DEBUG_LOG(m_logger) << LOGFUNCTION << " invalid= "
//                      << " lon_rad=" << lon_rad
//                      << " lat_rad=" << lat_rad;
  if (bIsCovered)
    *bIsCovered = false;
  return std::numeric_limits<double>::min();
}

double ElevationMapManager::elevation(double lon_rad, double lat_rad, double defaultValue, bool *bIsCovered) const
{
  bool bIsCoveredLocal = false;
  const double elevationValue = elevation(lon_rad, lat_rad, &bIsCoveredLocal);

  // in any case: if provided, transfer isCovered back to caller
  if (bIsCovered)
    *bIsCovered = bIsCoveredLocal;

  // which value to be returned?
  if (!bIsCoveredLocal)
    return defaultValue;

  return elevationValue;
}


QRectF ElevationMapManager::simpleUnitedExtent() const
{
  if (d->m_simpleExtent.isNull())
  {
    bool bFirst = true;
    for (const ElevationMap *elevationMap : d->m_elevationMapCont)
    {
      const QRectF &singleExtent = elevationMap->extent();
      if (bFirst)
      {
        bFirst = false;
        d->m_simpleExtent = singleExtent;
//        qCritical() << " bFirst.left " << singleExtent.left() << " bFirst.right " << singleExtent.right();
//        qCritical() << " bFirst.top " << singleExtent.top() << " bFirst.bottom " << singleExtent.bottom();

      } else {
        // prefer following instead QRectF::united (top vs. bottom):
//        qCritical() << " next.left " << singleExtent.left() << " secnextright " << singleExtent.right();
//        qCritical() << " next.top " << singleExtent.top()   << " next.bottom " << singleExtent.bottom();
        d->m_simpleExtent.setLeft  (std::min(d->m_simpleExtent.left(),   singleExtent.left()));
        d->m_simpleExtent.setTop   (std::max(d->m_simpleExtent.top(),    singleExtent.top()));        // top - >use max !!!
        d->m_simpleExtent.setBottom(std::min(d->m_simpleExtent.bottom(), singleExtent.bottom()));     // bottom -> use min
        d->m_simpleExtent.setRight (std::max(d->m_simpleExtent.right(),  singleExtent.right()));
      }
    } // for
  }

  return d->m_simpleExtent;
}

bool ElevationMapManager::contains(const QPointF &point) const
{
  for (const ElevationMap *elevationMap : d->m_elevationMapCont)
  {
    if (elevationMap->contains(point))
      return true;
  }
  return false;
}

void ElevationMapManager::LOSProfile(const QPointF &start, const QPointF &end, LOSVector &losProfile)
{
  TRACE_LOG(m_logger) << LOGFUNCTION << " START";

  if (d->m_elevationMapCont.empty())
  {
    ERROR_LOG(m_logger) << LOGFUNCTION << " no elevationMap";
    return;
  }

  waitForInitialization();

  QRectF simpleExtentLocal = simpleUnitedExtent();

//  qCritical() << " simpleExtent.left " << simpleExtentLocal.left() << " simpleExtent.right " << simpleExtentLocal.right();
//  qCritical() << " simpleExtent.top " << simpleExtentLocal.top() << " simpleExtent.bottom " << simpleExtentLocal.bottom();

  if (!simpleExtentLocal.contains(start))
  {
    ERROR_LOG(m_logger) << LOGFUNCTION << " start coordinate not within area"
                        << ", start: x=" << start.x() << " ; y=" << start.y()
                        << ", area: "
                        << " left " << simpleExtentLocal.left() << ", right " << simpleExtentLocal.right()
                        << " top " << simpleExtentLocal.top() << ", bottom " << simpleExtentLocal.bottom()
                           ;
    return;  // ausserhalb der Höhenkarte !!
  }

  bool clipped = false;
  double totalLng;
  QPointF startG = start;
  QPointF endG = end;

  if (!simpleExtentLocal.contains(end))  // Clippen
  {
    clipped = true;
    totalLng = length(endG - startG);
    if (endG.x() < simpleExtentLocal.left())
    {
      double m = (endG.y() - startG.y()) / (endG.x() - startG.x());
      double x = simpleExtentLocal.left();
      endG.setX(x);
      endG.setY(startG.y() + m * (x - startG.x()));
    }
    else if (endG.x() > simpleExtentLocal.right())
    {
      double m = (endG.y() - startG.y()) / (endG.x() - startG.x());
      double x = simpleExtentLocal.right();
      endG.setX(x);
      endG.setY(startG.y() + m * (x - startG.x()));
    }
    if (endG.y() < simpleExtentLocal.bottom())
    {
      double m = (endG.x() - startG.x()) / (endG.y() - startG.y());
      double y = simpleExtentLocal.bottom();
      endG.setY(y);
      endG.setX(startG.x() + m * (y - startG.y()));
    }
    else if (endG.y() > simpleExtentLocal.top())
    {
      double m = (endG.x() - startG.x()) / (endG.y() - startG.y());
      double y = simpleExtentLocal.top();
      endG.setY(y);
      endG.setX(startG.x() + m * (y - startG.y()));
    }
  }


  // Korrektursteigung aufgrund WGS84 Erdellipsoid, Lineare Approximation, Fehler bis 200 km: < 1.0m
  double startRadius = radiusWGS84(startG.y());
  double corrM = (radiusWGS84(endG.y()) - startRadius) / length(endG - startG);

  //
  // getSamplePoints(start, end, elevProfile);
  // find best elevationMap for equested path {startG, endG}
  //

  // 1st find maps covering the desired pathG:
  const QPolygonF pathG( QVector<QPointF>{startG, endG} );

  std::vector<ElevationMap *> coveredElevationMaps;
  coveredElevationMaps.reserve(d->m_elevationMapCont.size());
  for (auto curItem : qAsConst(d->m_elevationMapCont))
  {
    if (curItem && curItem->intersects(pathG))
      coveredElevationMaps.push_back(curItem);
  } // for

  if (m_logger.isDebugEnabled())
  {
    DEBUG_LOG(m_logger) << " coveredElevationMap.size() = " << coveredElevationMaps.size();
    for (auto curItem : qAsConst(coveredElevationMaps))
      DEBUG_LOG(m_logger) << " coveredElevationMap = " << curItem->filename();
  }

  // if no maps covers the desired path, stay on old approach: use most bottom elevation map:
  if (coveredElevationMaps.empty())
    coveredElevationMaps = d->m_elevationMapCont;

  // 2nd: find elevation map with highest resolution to achieve best results:
  using ELE_VEC_IT = std::vector<ElevationMap*>::iterator;
  ELE_VEC_IT eleIt = coveredElevationMaps.begin();
  for (ELE_VEC_IT curItem = coveredElevationMaps.begin()+1; curItem != coveredElevationMaps.end(); ++curItem)
  {
    ElevationMap *elePtr = *curItem;
    if (!elePtr)
      continue;

    if (elePtr->elexelSize().first < (*eleIt)->elexelSize().first)
      eleIt = curItem;
  } // for

  Q_ASSERT_X((*eleIt), "ElevationMapManager::LOSProfile", "eleIt failed");

  Vec3dVector elevProfile;
  (*eleIt)->getSamplePoints(start, end, elevProfile);

  INFO_LOG(m_logger) << "Using ElevationMap '" << (*eleIt)->filename() << "'"
                     << " (of size = " << d->m_elevationMapCont.size() << ")"
                     << " with_elexelSize " << (*eleIt)->elexelSize().first
                     << " -> # SamplePoints = " << elevProfile.size();

  for (Vec3dVector::iterator itr = elevProfile.begin(); itr != elevProfile.end(); ++itr)
  {
    double dist  = length(distance_in_meter_WGS84(start, itr->toPointF()));
    double height = elevation(itr->x(), itr->y());                   // use elevation over all seperate elevationMaps
    height += corrM * dist; // WGS84 Erdellipsoid Korrektur
    height -= (startRadius * (1 - cos(atan(dist / startRadius))));        // Einbeziehen der Erdkruemmung
    // alternativ als Naeherung: height -= ( dist * dist) / (startRadius * 2.0);
    losProfile.push_back(LOSentry(dist, height));
  }

  if (clipped)
    losProfile.push_back(LOSentry(totalLng, -NAN));

  TRACE_LOG(m_logger) << LOGFUNCTION << " END";
}

ElevationMapManager::LineOfSight ElevationMapManager::lineOfSightCalculation(const LOSVector &losProfile,
                                                                             double centerHat,
                                                                             double objHat) const
{
  // Analog zu SightFanImpl::calculateSightBeam


  if (losProfile.size() < 2) // Startpunkt undefiniert oder ausserhalb der Karte
  {
    return LineOfSight::LOS_ERROR;
  }

  LOSVector::const_iterator itr = losProfile.begin();
  const double centerHeight = itr->height() + centerHat;

  double d1 = 0.0;
  double height1 = itr->height();

//  // überspringen bis _innerRadius erreicht ist !
//  while( ++itr != losProfile.end())
//  {
//    double  d2 = itr->dist(); // *EarthRadius;
//    double  height2 = itr->height();
//    if( d2 >= m_internal->m_innerRadius)
//    {
//      height1 = height1 + (m_internal->m_innerRadius - d1) * ( height2 - height1)/(d2 - d1);
//      d1 = m_internal->m_innerRadius;
//      --itr;
//      break;
//    }
//    d1 = d2;
//    height1 = height2;
//  }

//    bool visible = true;
//  SightMode mode = START;

  d1 = itr->dist();
  double curM = (height1 - centerHeight ) / d1;     // bestimmende Steigung

//  sBeam.add( SightEntry( d1, START) );

  while (++itr != losProfile.end())
  {
    double  d2      = itr->dist(); // *EarthRadius;
    double  height2 = itr->height();

    if (std::isnan(height2)) // Undefined
    {
      return LineOfSight::LOS_UNDEFINED;
    }

    double hObj = (height2 + objHat - centerHeight );
    double objM =  hObj / d2;          // Steigung bezogen auf Objekte( Objekte verdecken nicht !)

    LineOfSight newMode = ((objM >= curM)) ? LineOfSight::LOS_VISIBLE : LineOfSight::LOS_INVISIBLE;

    if (newMode == LineOfSight::LOS_INVISIBLE)
      return LineOfSight::LOS_INVISIBLE;


//    if (newMode != mode)
//    {
//      if ( (mode != LineOfSight::LOS_UNDEFINED) && (d2 != d1) )
//      {
//        // Korrekturwert für Distanz berechnen, unter Berücksichtigung der Objekthoehe !!
//        double localM = (height2 - height1) /(d2 - d1);
//        // localM == curM kann nicht auftreten, da newMode != mode
//        double dCorr = (hObj - (curM * d2)) / (localM - curM);

//        if (dCorr < 0.0)
//          dCorr = 0.0;
//        dCorr = d2 - dCorr;
//        if (dCorr < d1)
//          dCorr = d1;

//        sBeam.add( SightEntry( dCorr, mode) );
//      }
//      mode = newMode;
//    }

    double terrM = (height2 - centerHeight) / d2;      // Steigung bezogen auf Gelaende
    if (terrM >= curM)
    {
      curM = terrM;  // neue bestimmende Steigung
    }

//    height1 = height2;
//    d1 = d2;
  } // while

  return LineOfSight::LOS_VISIBLE;
}

ElevationMapManager::LineOfSight ElevationMapManager::lineOfSightCalculation(const LtsUtils::QPoint3D &startPointGeo_absZ,
                                                                             const LtsUtils::QPoint3D &endPointGeo_absZ)
{
  TRACE_LOG(m_logger) << LOGFUNCTION << " START" << ", startPointGeo_absZ = " << startPointGeo_absZ << ", endPointGeo_absZ = " << endPointGeo_absZ;

  if (!isValid())
  {
    ERROR_LOG(m_logger) << LOGFUNCTION << " Error, LtsElevationManager not valid";
    return LineOfSight::LOS_ERROR;
  }

  // 1st: a) centerHat / startHat:
  const double startHat = startPointGeo_absZ.z() - this->elevation(startPointGeo_absZ.x(), startPointGeo_absZ.y(), -10000);
  if (startHat < 0)
  {
    ERROR_LOG(m_logger) << LOGFUNCTION << " Error, startHat is negativ";
    return LineOfSight::LOS_ERROR;
  }
  // 1st: b) endHat / objHat:
  const double endHat = endPointGeo_absZ.z() - this->elevation(endPointGeo_absZ.x(), endPointGeo_absZ.y(), -10000);
  if (endHat < 0)
  {
    ERROR_LOG(m_logger) << LOGFUNCTION << " Error, objHat is negativ"
                        << ", startPointGeo_absZ=" << startPointGeo_absZ
                        << ", endPointGeo_absZ=" << endPointGeo_absZ;
    return LineOfSight::LOS_ERROR;
  }

  SituationMap::ElevationMapManager::LineOfSight eleMapMgrResult = lineOfSightCalculation(startPointGeo_absZ.toPointF(), startHat,
                                                                                          endPointGeo_absZ.toPointF(), endHat);

  TRACE_LOG(m_logger) << LOGFUNCTION << " END" << ", startPointGeo = " << startPointGeo_absZ << ", endPointGeo = " << endPointGeo_absZ
                      << " -> LineOfSight " << static_cast<int>(eleMapMgrResult);
  return eleMapMgrResult;
}

ElevationMapManager::LineOfSight ElevationMapManager::lineOfSightCalculation(const QPointF &startPointGeo, double startHat,
                                                                             const QPointF &endPointGeo, double endHat)
{
  TRACE_LOG(m_logger) << LOGFUNCTION << " START"
                      << ", startPointGeo = " << startPointGeo << ", startHat = " << startHat
                      << ", endPointGeo = " << endPointGeo << ", endHat = " << endHat ;
  if (!isValid())
  {
    ERROR_LOG(m_logger) << LOGFUNCTION << " Error, LtsElevationManager not valid";
    return LineOfSight::LOS_ERROR;
  }

  if (startHat < 0)
  {
    ERROR_LOG(m_logger) << LOGFUNCTION << " Error, startHat is negativ";
    return LineOfSight::LOS_ERROR;
  }
  if (endHat < 0)
  {
    ERROR_LOG(m_logger) << LOGFUNCTION << " Error, endHat is negativ";
    return LineOfSight::LOS_ERROR;
  }

  // 1st: create LOSVector:
  SituationMap::LOSVector losProfile;
  LOSProfile(startPointGeo, endPointGeo, losProfile);
  if (losProfile.size() < 2)
  {
    ERROR_LOG(m_logger) << LOGFUNCTION << " Error, losProfile not valid"
                        << ", losProfile.size = " << losProfile.size();
    return LineOfSight::LOS_ERROR;
  }

  // 2nd: lineOfSightCalculation
  SituationMap::ElevationMapManager::LineOfSight eleMapMgrResult = this->lineOfSightCalculation(losProfile, startHat, endHat);

  TRACE_LOG(m_logger) << LOGFUNCTION << " END"
                      << ", startPointGeo = " << startPointGeo << ", startHat = " << startHat
                      << ", endPointGeo = " << endPointGeo << ", endPointHat = " << endHat
                      << " -> LineOfSight " << static_cast<int>(eleMapMgrResult);
  return eleMapMgrResult;
}




} // SituationMap