// **************************************************************************
//
//  (C) Copyright 2009   Swiss SIMTEC AG
//
// **************************************************************************
//
//  $HeadURL$
//  $LastChangedBy$
//  $LastChangedDate$
//  $LastChangedRevision$
//
// **************************************************************************

// manual class headers
#include "GeoPairingImplFiredBulletBalistic.hpp"
#include <GBL_ConfigSystem/geoconfig.h>

// from cpp_includes
#include <FW_Ballistic/ITrajectory.hpp>
#include <FW_Ballistic/IFireControl.hpp>
#include <FW_Ballistic/BallisticService.hpp>
#include <GBL_Types/Angle3.hpp>
#include "geo_settings.h"

namespace N_GeoPairingImpl
{

// ---------------------------------------------------------------
// Constructur taking the SensorData as input values
// ---------------------------------------------------------------
GeoPairingImplFiredBulletBalistic::GeoPairingImplFiredBulletBalistic(const GeoPairingImplSensorData &a_sensorData, const char *loggerName)
  : GeoPairingImplFiredBullet(a_sensorData, loggerName ? loggerName : "GeoPairingImplFiredBulletBalistic")
{
  m_UtmSensor.setLlPositionDegree(m_SensorData.WeaponPositionLat,
                                  m_SensorData.WeaponPositionLong,
                                  m_SensorData.WeaponPositionHeight);
  m_ballisticService = std::make_shared<N_Ballistic::BallisticService>();

  m_ballisticService->init(N_GeoPairingImpl::Settings::cesGeopairingIniPath(),
                           N_GeoPairingImpl::Settings::cesGeopairingIniFile(),
                           N_GeoPairingImpl::Settings::cesAmmunitionIniFile());
  m_SensorData.IgnitionType = a_sensorData.IgnitionType;

}

// ---------------------------------------------------------------
// Performs the calculation of the trajectory and also returns
// the result of calculation
// ---------------------------------------------------------------
GeoPairingImplFiredBullet::E_CalcResult GeoPairingImplFiredBulletBalistic::calculate(const std::shared_ptr<N_Ballistic::BallisticService> &ballisticService)
{
  m_ballisticService = ballisticService;
  E_CalcResult l_cr = e_NONE;
  N_ConfigSystem::GeoConfig *l_config = N_ConfigSystem::GeoConfig::getInstance();

  QMap<int, QString> ammuMap = l_config->getAmmuMap();
  QMap<int, QVector<double> > chargeMap = l_config->getChargeMap();

  INFO_LOG(m_logger) << "----------- Calculating Impact -----------";

  N_Types::Coord3 l_sensorPos(m_UtmSensor.utmEasting(),
                              m_UtmSensor.utmNorthing(),
                              m_UtmSensor.height());
  INFO_LOG(m_logger) << "l_sensorPos : " << l_sensorPos;

  N_Math::GeoCoord l_checkUtm;
  l_checkUtm.setTotalUtmPosition(l_sensorPos.m_X, l_sensorPos.m_Y, l_sensorPos.m_Z,
                                 m_UtmSensor.utmZone(), m_UtmSensor.northP());

  N_Types::Coord3 l_checkPos(l_checkUtm.latDeg(), l_checkUtm.lonDeg(), l_checkUtm.height());
  INFO_LOG(m_logger) << "o_checkPos : " << l_checkPos;
  INFO_LOG(m_logger) << "m_SensorData.AmmunitionType : " << m_SensorData.AmmunitionType;
  QString l_ammunition = ammuMap[m_SensorData.AmmunitionType];
  INFO_LOG(m_logger) << "l_ammunition=" << l_ammunition;


  // ---------------- Preparing Calculation
  N_Ballistic::IFireControl *lp_fireControl = nullptr;
  if (!m_ballisticService)
  {
    ERROR_LOG(m_logger) << "!m_ballisticService";
    return l_cr;
  }
  bool ok = m_ballisticService->createInterfaceOf((N_Ballistic::IUnknown **)&lp_fireControl, N_Ballistic::BallisticService::e_IFireControl, l_ammunition);
  if (!ok)
  {
    ERROR_LOG(m_logger) << "couldn't create interface ";
    return l_cr;
  }
  if (!lp_fireControl)
  {
    ERROR_LOG(m_logger) << "couldn't initialize fireControl for " << l_ammunition;
    return l_cr;
  }

  double l_density = lp_fireControl->getAirDensity(m_MeteoData.AirPressure, m_MeteoData.Temperature);
  INFO_LOG(m_logger) << "l_density = " << l_density;

  INFO_LOG ( m_logger) << "getInitialAmmuVelocity=" << m_SensorData.WeaponTemperature;
  double l_initialspeed = lp_fireControl->getInitialAmmuVelocity(m_SensorData.WeaponTemperature);
  INFO_LOG ( m_logger) << "l_initialspeed=" << l_initialspeed;


  QMap<int, QVector<double> >::iterator chargeIt = chargeMap.find(m_SensorData.AmmunitionType);
  if (chargeIt != chargeMap.end())
  {
    QVector<double> l_charges = chargeMap[m_SensorData.AmmunitionType];
    if (m_SensorData.ChargeType >= 0 && m_SensorData.ChargeType < l_charges.size())
    {
      l_initialspeed = l_charges[m_SensorData.ChargeType];
      INFO_LOG(m_logger) << "Charge chosen for " << l_ammunition << " : " << l_initialspeed;
    }
  }
  else
    INFO_LOG(m_logger) << "l_initialspeed = " << l_initialspeed;

  GeoPairingImplTrajectoryData::TrajectoryPoint tpSensor;
  tpSensor.m_H = l_sensorPos.m_Z;
  tpSensor.m_Vel = l_initialspeed;
  tpSensor.m_Rho = l_density;
  m_TrajectoryData.m_Points.push_back(tpSensor);

  N_Types::Radian64 l_windDir;
  l_windDir.setDegree(m_MeteoData.WindDirection);
  N_Types::Coord3 l_windAngle(0, m_MeteoData.WindSpeed, 0);
  l_windAngle = l_windAngle.rotateZ(l_windDir);
  INFO_LOG(m_logger) << "o_windAngle = " << l_windAngle;
  double l_crossWind = l_windAngle.rotateZ(-m_SensorData.WeaponAzimuth).getX();
  INFO_LOG(m_logger) << "l_crossWind = " << l_crossWind;

  // ------------------ Prepare Trajectory
  N_Ballistic::ITrajectory *lp_trajectory = nullptr;
  //if (!BallisticService::createInterfaceOf((IUnknown **)&lp_trajectory, BallisticService::e_ITrajectory, l_ammunition))
  //m_ballisticService->createInterfaceOf((IUnknown **)&lp_fireControl, BallisticService::e_IFireControl, l_ammunition);
  m_ballisticService->createInterfaceOf((N_Ballistic::IUnknown **)&lp_trajectory, N_Ballistic::BallisticService::e_ITrajectory, l_ammunition);

  if (!lp_trajectory)
  {
    ERROR_LOG(m_logger) << "Couldn't create Trajectory object for " << l_ammunition;
    return l_cr;
  }

  N_Types::Angle3 l_sensorAngle(m_SensorData.WeaponAzimuth,
                                m_SensorData.WeaponElevation,
                                0);
  INFO_LOG(m_logger) << "o_sensorAngle : " << l_sensorAngle;

  if (!lp_trajectory->init(l_sensorPos, l_sensorAngle, l_crossWind, l_density, l_initialspeed))
  {
    lp_trajectory->release();
    ERROR_LOG(m_logger) << "Couldn't initialize Trajectory";
    return l_cr;
  }


  // ------------------- Loop Trajectory
  double l_time = 0;
  QString s_Precision = QStringLiteral("Precision");
  QString s_DeltaTime = QStringLiteral("DeltaTime");

  double l_timeDelta = l_config->getFloat(s_Precision, s_DeltaTime, 0.05);
  INFO_LOG(m_logger) << "Used Precision DeltaTime = " << l_timeDelta;

  N_Types::Coord3 l_pos;

  while (lp_trajectory->isActive())
  {
    l_time += l_timeDelta;
    lp_trajectory->move(l_timeDelta);

    if (m_SensorData.IgnitionType == 0 && !lp_trajectory->isActive())
    {
      INFO_LOG(m_logger) << "Timeout of Bullet! IgnitionType=" << m_SensorData.IgnitionType;
      l_cr = e_TIMEOUT;
      break;
    }

    l_pos = lp_trajectory->getPosition();

    N_Math::GeoCoord l_checkUtmShadow;
    l_checkUtmShadow.setTotalUtmPosition(l_pos.m_X, l_pos.m_Y, l_pos.m_Z,
                                   m_UtmSensor.utmZone(), m_UtmSensor.northP());

    N_Types::Coord3 l_checkPosShadow(l_checkUtmShadow.lonDeg(), l_checkUtmShadow.latDeg(), l_checkUtmShadow.height());

    INFO_LOG(m_logger) << "l_pos "
                       << " X=" << l_pos.getX()
                       << " Y=" << l_pos.getY()
                       << " Z=" << l_pos.getZ()
                       << " m_X=" << l_pos.m_X
                       << " m_Y=" << l_pos.m_Y
                       << " m_Z=" << l_pos.m_Z
                       << " l_checkUtm.height=" << l_checkUtmShadow.height()
                       << " l_checkPos.z=" << l_checkPosShadow.getZ();

    double l_height = 0;
    if (checkTerrainHit(l_checkPosShadow, l_height))
    {
      INFO_LOG(m_logger) << "deactivate";
      lp_trajectory->deactivate();
      l_cr = e_OK;
    }
    else
    {
      if (m_SensorData.IgnitionType > 0 && l_time > m_SensorData.IgnitionType)
      {
        INFO_LOG(m_logger) << "deactivate";
        lp_trajectory->deactivate();
        l_cr = e_OK;
        INFO_LOG(m_logger) << "Detonation by Timer : " << m_SensorData.IgnitionType;
      }
    }

    GeoPairingImplTrajectoryData::TrajectoryPoint tpImpact;
    tpImpact.m_X = l_pos.m_X - l_sensorPos.m_X;
    tpImpact.m_Y = l_pos.m_Y - l_sensorPos.m_Y;
    tpImpact.m_Z = l_pos.m_Z - l_sensorPos.m_Z;
    tpImpact.m_H = l_height;
    tpImpact.m_Vel = lp_trajectory->getVelocity().magnitude();
    tpImpact.m_Time = l_time;
    m_TrajectoryData.m_Points.push_back(tpImpact);

  }

  l_pos = lp_trajectory->getPosition();

//  INFO_LOG(m_logger) << "Total Distance = " << l_pos.length(l_sensorPos);
  INFO_LOG(m_logger) << "Total Time     = " << l_time;

  lp_trajectory->release();
  lp_fireControl->release();

  INFO_LOG(m_logger) << "l_pos : " << l_pos;

  double l_Distance = l_pos.length(l_sensorPos);
  INFO_LOG(m_logger) << "l_Distance = " << l_Distance;

  N_Math::GeoCoord l_impactUtm;
  l_impactUtm.setTotalUtmPosition(l_pos.m_X, l_pos.m_Y, l_pos.m_Z,
                                  m_UtmSensor.utmZone(), m_UtmSensor.northP());

  INFO_LOG(m_logger) << "l_impactUtm (lat/lon/h) : " << l_impactUtm.latDeg()
                     << "/" << l_impactUtm.lonDeg() << "/" << l_impactUtm.height()
                     << " zone=" << m_UtmSensor.utmZone()
                     << " northP=" << m_UtmSensor.northP();


  m_ImpactData.AmmunitionType       = m_SensorData.AmmunitionType;
  m_ImpactData.ImpactTime           = m_SensorData.FireTime + l_time;
  m_ImpactData.ImpactPositionLat    = l_impactUtm.latDeg();
  m_ImpactData.ImpactPositionLong   = l_impactUtm.lonDeg();
  m_ImpactData.ImpactPositionHeight = l_impactUtm.height();

  return l_cr;
}


// ---------------------------------------------------------------
bool GeoPairingImplFiredBulletBalistic::checkTerrainHit(const N_Types::Coord3 &a_pos, double &a_height)
{
  a_height = 0;
  INFO_LOG(m_logger) << "checkTerrainHit"
                     << " a_pos.m_Z=" << a_pos.m_Z
                     << " m_UtmSensor.height()=" << m_UtmSensor.height();
  return a_pos.m_Z < m_UtmSensor.height();
}

} // namespace N_GeoPairingImpl