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

// manual class headers
#include "GeoPairingImplFiredBullet.hpp"

// from cpp_includes
#include <GBL_Types/Angle3.hpp>
#include <GBL_Types/Coord3.hpp>
#include <GBL_Types/GeoCoord.hpp>
#include <GBL_ConfigSystem/geoconfig.h>
#include <limits>
#include <LtsCoordTrans/lts_coord_trans.h>
#include <QPointF>

#ifdef WIN32
#define RAND48_SEED_0   (0x330e)
#define RAND48_SEED_1 (0xabcd)
#define RAND48_SEED_2 (0x1234)
#define RAND48_MULT_0 (0xe66d)
#define RAND48_MULT_1 (0xdeec)
#define RAND48_MULT_2 (0x0005)
#define RAND48_ADD (0x000b)

unsigned short _rand48_seed[3] = {
        RAND48_SEED_0,
         RAND48_SEED_1,
         RAND48_SEED_2
};
unsigned short _rand48_mult[3] = {
         RAND48_MULT_0,
         RAND48_MULT_1,
         RAND48_MULT_2
};
unsigned short _rand48_add = RAND48_ADD;

void
_dorand48(unsigned short xseed[3])
{
  unsigned long accu;
  unsigned short temp[2];

  accu = (unsigned long)_rand48_mult[0] * (unsigned long)xseed[0] +
    (unsigned long)_rand48_add;
  temp[0] = (unsigned short)accu;        /* lower 16 bits */
  accu >>= sizeof(unsigned short) * 8;
  accu += (unsigned long)_rand48_mult[0] * (unsigned long)xseed[1] +
    (unsigned long)_rand48_mult[1] * (unsigned long)xseed[0];
  temp[1] = (unsigned short)accu;        /* middle 16 bits */
  accu >>= sizeof(unsigned short) * 8;
  accu += _rand48_mult[0] * xseed[2] + _rand48_mult[1] * xseed[1] + _rand48_mult[2] * xseed[0];
  xseed[0] = temp[0];
  xseed[1] = temp[1];
  xseed[2] = (unsigned short)accu;
}

double erand48(unsigned short xseed[3])
{
  _dorand48(xseed);
  return ldexp((double)xseed[0], -48) +
    ldexp((double)xseed[1], -32) +
    ldexp((double)xseed[2], -16);
}

double drand48() {
  return erand48(_rand48_seed);
}

#endif

namespace N_GeoPairingImpl
{

// ---------------------------------------------------------------
// Constructur taking the SensorData as input values
// ---------------------------------------------------------------
GeoPairingImplFiredBullet::GeoPairingImplFiredBullet(const GeoPairingImplSensorData &a_sensorData,
                                                     const char *loggerName)
  : m_SensorData(a_sensorData)
  , m_logger(CesGeoPairing::getLogger(loggerName ? loggerName : "Plugin.GeoPairing.GeoPairingImplFiredBullet"))
{
  INFO_LOG(m_logger) <<  "---------- Setting Sensor Data ------------";
  INFO_LOG(m_logger) <<  "WeaponPositionLat    : " << m_SensorData.WeaponPositionLat;
  INFO_LOG(m_logger) <<  "WeaponPositionLong   : " << m_SensorData.WeaponPositionLong;
  INFO_LOG(m_logger) <<  "WeaponPositionHeight : " << m_SensorData.WeaponPositionHeight;
  INFO_LOG(m_logger) <<  "WeaponAzimuth        : " << m_SensorData.WeaponAzimuth << " (" << m_SensorData.WeaponAzimuth.getDegree() << ")" ;
  INFO_LOG(m_logger) <<  "WeaponAzimuthMin     : " << m_SensorData.WeaponAzimuthMin << " (" << m_SensorData.WeaponAzimuthMin.getDegree() << ")" ;
  INFO_LOG(m_logger) <<  "WeaponAzimuthMax     : " << m_SensorData.WeaponAzimuthMax << " (" << m_SensorData.WeaponAzimuthMax.getDegree() << ")" ;
  INFO_LOG(m_logger) <<  "WeaponElevation      : " << m_SensorData.WeaponElevation << " (" << m_SensorData.WeaponElevation.getDegree() << ")" ;
  INFO_LOG(m_logger) <<  "WeaponElevationMin   : " << m_SensorData.WeaponElevationMin << " (" << m_SensorData.WeaponElevationMin.getDegree() << ")" ;
  INFO_LOG(m_logger) <<  "WeaponElevationMax   : " << m_SensorData.WeaponElevationMax << " (" << m_SensorData.WeaponElevationMax.getDegree() << ")" ;
  INFO_LOG(m_logger) <<  "WeaponTemperature    : " << m_SensorData.WeaponTemperature;
  INFO_LOG(m_logger) <<  "AmmunitionType       : " << m_SensorData.AmmunitionType;
  INFO_LOG(m_logger) <<  "ChargeType           : " << m_SensorData.ChargeType;
  INFO_LOG(m_logger) <<  "IgnitionType         : " << m_SensorData.IgnitionType;
  INFO_LOG(m_logger) <<  "FireTime             : " << m_SensorData.FireTime;
}

// ---------------------------------------------------------------
// Performs the calculation of the trajectory and also returns
// the result of calculation
// ---------------------------------------------------------------
GeoPairingImplFiredBullet::E_CalcResult GeoPairingImplFiredBullet::calculate(const std::shared_ptr<N_Ballistic::BallisticService> &ballisticService)
{
  Q_UNUSED(ballisticService)  // why?

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

  N_Math::GeoCoord o_sensorUtm;

  o_sensorUtm.setLlPositionDegree(m_SensorData.WeaponPositionLat,
                                  m_SensorData.WeaponPositionLong,
                                  m_SensorData.WeaponPositionHeight);

  N_Types::Coord3 o_sensorPos(o_sensorUtm.utmEasting(),
                              o_sensorUtm.utmNorthing(),
                              o_sensorUtm.height());
  INFO_LOG(m_logger) << "o_sensorPos : " << o_sensorPos;

  GeoPairingImplTrajectoryData::TrajectoryPoint tpSensor;
  tpSensor.m_X = o_sensorPos.m_X;
  tpSensor.m_Y = o_sensorPos.m_Y;
  tpSensor.m_Z = o_sensorPos.m_Z;
  tpSensor.m_H = 0;
  m_TrajectoryData.m_Points.push_back(tpSensor);

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

  // calculate a faked shot
  Float64 d_shootingDistance = 500.0 + (m_SensorData.AmmunitionType * 200) ;
  INFO_LOG(m_logger) << "d_shootingDistance : " << d_shootingDistance;

  N_Types::Coord3 o_shot(0, d_shootingDistance, 0);
  INFO_LOG(m_logger) << "o_shot : " << o_shot;
  o_shot = o_shot.rotate(o_sensorAngle);
  o_sensorPos += o_shot;
  INFO_LOG(m_logger) << "o_shot : " << o_shot;
  INFO_LOG(m_logger) << "impact : " << o_sensorPos;

  N_Math::GeoCoord o_impactUtm;
  o_impactUtm.setTotalUtmPosition(o_sensorPos.m_X, o_sensorPos.m_Y, o_sensorPos.m_Z,
                                  o_sensorUtm.utmZone(), o_sensorUtm.northP());

  GeoPairingImplTrajectoryData::TrajectoryPoint tpImpact;
  tpImpact.m_X = o_sensorPos.m_X;
  tpImpact.m_Y = o_sensorPos.m_Y;
  tpImpact.m_Z = o_sensorPos.m_Z;
  tpImpact.m_H = 0;
  m_TrajectoryData.m_Points.push_back(tpImpact);

  if (m_logger.isInfoEnabled())
  {
    for (const auto &tp : m_TrajectoryData.m_Points)
      INFO_LOG(m_logger) << tp.m_X << " " << tp.m_Y << " " << tp.m_Z << " " << tp.m_H;
  }

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

  return e_OK;
}

GeoPairingImplFiredBullet::E_CalcResult GeoPairingImplFiredBullet::calculateG5G6(double entfernung,
                                                                                 double drift,
                                                                                 double flugzeit)
{
  INFO_LOG(m_logger) << "----------- Calculating Impact -----------";


  Float64 xOutMgrs = 0.0;
  Float64 yOutMgrs = 0.0;
  Float64 xOutLon  = 0.0;
  Float64 yOutLat  = 0.0;
  Float64 zOutAlt  = 0.0;

  //LtsCoordTrans::LonLat2MGRS(m_SensorData.WeaponPositionLong / 180 * M_PI,
  //                        m_SensorData.WeaponPositionLat / 180 * M_PI,
  //                        xOutMgrs,
  //                        yOutMgrs);
  const int zoneToUse = -1;
  auto utm = LtsCoordTrans::UtmCoordinate::fromGeographic(m_SensorData.WeaponPositionLong / 180 * M_PI,// TODO ueberpruefen!!!
                                                          m_SensorData.WeaponPositionLat / 180 * M_PI, zoneToUse);
  xOutMgrs = utm.x;
  yOutMgrs = utm.y;

  Float64 x_mem = entfernung;
  Float64 xOut = x_mem * cos(m_SensorData.WeaponAzimuth.getItsValue() + M_PI_2 - drift);
  Float64 yOut = x_mem * sin(m_SensorData.WeaponAzimuth.getItsValue() + M_PI_2 - drift);

  xOutMgrs += xOut;
  yOutMgrs += yOut;
  LtsCoordTrans::UtmCoordinate tm2(utm.zone, xOutMgrs, yOutMgrs );
  INFO_LOG(m_logger) << "NEW POS "  << tm2.geographic().x() << "   " << tm2.geographic().y() ;

  //LtsCoordTrans::MGRS2LonLat(xOutMgrs, yOutMgrs, xOutLon, yOutLat);
  LtsCoordTrans::geocentric2LonLatAlt(xOutMgrs, yOutMgrs, 0.0, xOutLon, yOutLat, zOutAlt);

  m_ImpactData.AmmunitionType       = m_SensorData.AmmunitionType;
  m_ImpactData.ImpactTime           = m_SensorData.FireTime + lrint(flugzeit);
  m_ImpactData.ImpactPositionLat    = tm2.geographic().y() * (180 / M_PI);
  m_ImpactData.ImpactPositionLong   = tm2.geographic().x() * (180 / M_PI);
  m_ImpactData.ImpactPositionHeight = 0;

  INFO_LOG(m_logger) << "m_SensorData.WeaponPositionLong=" << m_SensorData.WeaponPositionLong
                       << " m_SensorData.WeaponPositionLat=" << m_SensorData.WeaponPositionLat
                       << "\n entfernung=" << entfernung
                       << "\n utm.x=" << utm.x
                       << " utm.y=" << utm.y
                       << "\n xOut=" << xOut
                       << " yOut=" << yOut
                       << "\n xOutMgrs=" << xOutMgrs
                       << " yOutMgrs=" << yOutMgrs
                       << "\n xOutLon=" << xOutLon
                       << " yOutLat=" << yOutLat
                       << "\n ImpactPositionLat=" << m_ImpactData.ImpactPositionLat
                       << " ImpactPositionLong=" << m_ImpactData.ImpactPositionLong;

  return e_OK;
}


// ---------------------------------------------------------------
// Performs the calculation of multiple trajectories and also
// returns the result of calculation
// ---------------------------------------------------------------
GeoPairingImplFiredBullet::E_CalcResult GeoPairingImplFiredBullet::calculateSalve(const std::shared_ptr<N_Ballistic::BallisticService> &ballisticService,
                                                                                  int a_nbrRounds, double &a_widthField,
                                                                                  double &a_lengthField, double &a_duration,
                                                                                  double &a_heading)
{
  INFO_LOG(m_logger) << "----------- Calculating Salve -----------";

  if (a_nbrRounds < 1)
    return GeoPairingImplFiredBullet::e_OUTOFRANGE;

  N_Math::GeoCoord l_sensorUtm;

  l_sensorUtm.setLlPositionDegree(m_SensorData.WeaponPositionLat,
                                  m_SensorData.WeaponPositionLong,
                                  m_SensorData.WeaponPositionHeight);

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

  Float64 l_TopPos = 0, l_BottomPos = 0, l_LeftPos = 0, l_RightPos = 0, l_BelowPos = 0, l_AbovePos = 0;

  N_Types::Coord3 l_avgImpactPos;
  int l_avgImpactCount = 0;

  Float64 l_avgImpactTime = 0.0;

  Float64 l_AzimuthReferenceAngle = m_SensorData.WeaponAzimuth;

  for (int i = 0; i < a_nbrRounds; i++)
  {
    INFO_LOG(m_logger) << "========== Calculating Round " << i + 1 << " of " << a_nbrRounds << " =============";
    prepareSensorData();

    if (calculate( ballisticService) != e_OK)
      continue;

    N_Math::GeoCoord l_impactUtm;
    l_impactUtm.setLlPositionDegree(m_ImpactData.ImpactPositionLat,
                                    m_ImpactData.ImpactPositionLong,
                                    m_ImpactData.ImpactPositionHeight);

    N_Types::Coord3 l_impactPos(l_impactUtm.utmEasting(),
                                l_impactUtm.utmNorthing(),
                                l_impactUtm.height());

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

    l_avgImpactPos += l_impactPos;
    l_avgImpactCount++;

    l_avgImpactTime += m_ImpactData.ImpactTime;

    N_Types::Coord3 l_RotImpactPos = l_impactPos - l_sensorPos;

    l_RotImpactPos = l_RotImpactPos.rotateZ(-l_AzimuthReferenceAngle); // rotate back to north position
    if (i == 0)
    {
      l_TopPos = l_BottomPos = l_RotImpactPos.m_Y;
      l_LeftPos = l_RightPos = l_RotImpactPos.m_X;
      l_BelowPos = l_AbovePos = l_RotImpactPos.m_Z;
    }
    else
    {
      l_TopPos    = std::max(l_RotImpactPos.m_Y, l_TopPos);
      l_BottomPos = std::min(l_RotImpactPos.m_Y, l_BottomPos);
      l_RightPos  = std::max(l_RotImpactPos.m_X, l_RightPos);
      l_LeftPos   = std::min(l_RotImpactPos.m_X, l_LeftPos);
      l_AbovePos  = std::max(l_RotImpactPos.m_Z, l_AbovePos);
      l_BelowPos  = std::min(l_RotImpactPos.m_Z, l_BelowPos);
    }
  }

  INFO_LOG(m_logger) << "========== Summarize Impact Data ==========";

  a_widthField  = l_RightPos - l_LeftPos;
  a_lengthField = l_TopPos - l_BottomPos;

  INFO_LOG(m_logger) << "a_widthField  = " << a_widthField;
  INFO_LOG(m_logger) << "a_lengthField = " << a_lengthField;

  // calculate average impact pos for this salve
  N_Types::Coord3 l_ImpactPos = l_avgImpactPos / std::max(1, l_avgImpactCount);

  INFO_LOG(m_logger) << "l_ImpactPos (Avg) : " << l_ImpactPos;

  N_Math::GeoCoord l_impactUtm;
  l_impactUtm.setTotalUtmPosition(l_ImpactPos.m_X, l_ImpactPos.m_Y, l_ImpactPos.m_Z,
                                  l_sensorUtm.utmZone(), l_sensorUtm.northP());

  m_ImpactData.ImpactPositionLat    = l_impactUtm.latDeg();
  m_ImpactData.ImpactPositionLong   = l_impactUtm.lonDeg();
  m_ImpactData.ImpactPositionHeight = l_impactUtm.height();

  m_ImpactData.ImpactTime = ceil(l_avgImpactTime / std::max(1, l_avgImpactCount));

  auto l_config = N_ConfigSystem::GeoConfig::getInstance();

  QString s_MK19 = QStringLiteral("MK19");
  QString s_RoundsPerMinute = QStringLiteral("RoundsPerMinute");
  Float64 l_RoundsPerMinute = l_config->getFloat(s_MK19, s_RoundsPerMinute, 350);

  a_duration = 60 * a_nbrRounds / l_RoundsPerMinute;
  INFO_LOG(m_logger) << "a_duration = " << a_duration;

  Float64 l_Distance = l_ImpactPos.length(l_sensorPos);
  INFO_LOG(m_logger) << "l_Distance = " << l_Distance;

  N_Types::Radian64 l_AvgHeading = m_SensorData.WeaponAzimuthMax - m_SensorData.WeaponAzimuthMin;
  l_AvgHeading /= 2.0;
  l_AvgHeading += m_SensorData.WeaponAzimuthMin;
  l_AvgHeading += M_PI_2; // rotate 90deg from east

  a_heading = l_AvgHeading.getZeroTo360();
  INFO_LOG(m_logger) << "a_heading [deg ] = " << a_heading;


  return e_OK;
}


// ---------------------------------------------------------------
// retrieves the Impact Data
// ---------------------------------------------------------------
GeoPairingImplImpactData *GeoPairingImplFiredBullet::getImpactData()
{
  if (m_ImpactData.ImpactTime == 0)
  {
    ERROR_LOG(m_logger) << "Impact Data not yet calculated";
    return nullptr;
  }

  INFO_LOG(m_logger) <<  "---------- Getting Impact Data ------------";
  INFO_LOG(m_logger) <<  "ImpactPositionLat    : " << m_ImpactData.ImpactPositionLat;
  INFO_LOG(m_logger) <<  "ImpactPositionLong   : " << m_ImpactData.ImpactPositionLong;
  INFO_LOG(m_logger) <<  "ImpactPositionHeight : " << m_ImpactData.ImpactPositionHeight;
  INFO_LOG(m_logger) <<  "ImpactTime           : " << m_ImpactData.ImpactTime;
  INFO_LOG(m_logger)  << "AmmunitionType       : " << m_ImpactData.AmmunitionType;

  return &m_ImpactData;
}


// ---------------------------------------------------------------
// retrieves the Trajectory Data
// ---------------------------------------------------------------
GeoPairingImplTrajectoryData *GeoPairingImplFiredBullet::getTrajectoryData()
{
  if (m_ImpactData.ImpactTime == 0)
  {
    ERROR_LOG(m_logger) << "Impact Data not yet calculated";
    return nullptr;
  }

  return &m_TrajectoryData;
}


// ---------------------------------------------------------------
// Sets the meteo data that are considered for this bullet
// ---------------------------------------------------------------
void GeoPairingImplFiredBullet::setMeteoData(const GeoPairingImplMeteoData &a_meteoData)
{
  m_MeteoData = a_meteoData;

  INFO_LOG(m_logger) <<  "---------- Setting Meteo Data ------------";
  INFO_LOG(m_logger) <<  "AirPressure   : " << m_MeteoData.AirPressure;
  INFO_LOG(m_logger) <<  "Temperature   : " << m_MeteoData.Temperature;
  INFO_LOG(m_logger) <<  "WindDirection : " << m_MeteoData.WindDirection;
  INFO_LOG(m_logger) <<  "WindSpeed     : " << m_MeteoData.WindSpeed;
}

// ---------------------------------------------------------------
// spreads the sensor heading and pitch in the min/max field,
// using random values
// ---------------------------------------------------------------
void GeoPairingImplFiredBullet::prepareSensorData()
{
  if (!m_SensorData.UseMinMax)
    return;

  INFO_LOG(m_logger) << "----------- Prepare Sensor Angles -----------";

  N_Types::Radian64 l_AzimuthMin = m_SensorData.WeaponAzimuthMin;
  N_Types::Radian64 l_AzimuthMax = m_SensorData.WeaponAzimuthMax;

  INFO_LOG(m_logger) << "WeaponAzimuthMin   [deg] = " << m_SensorData.WeaponAzimuthMin.getDegree();
  INFO_LOG(m_logger) << "WeaponAzimuthMax   [deg] = " << m_SensorData.WeaponAzimuthMax.getDegree();
  INFO_LOG(m_logger) << "WeaponElevationMin [deg] = " << m_SensorData.WeaponElevationMin.getDegree();
  INFO_LOG(m_logger) << "WeaponElevationMax [deg] = " << m_SensorData.WeaponElevationMax.getDegree();

  N_Types::Radian64 l_AzimuthDelta = fabs(l_AzimuthMax - l_AzimuthMin);

  // test the +/- 180 deg limit case
  if (l_AzimuthDelta > M_PI)
  {
    INFO_LOG(m_logger) << "Take short circle path";
    l_AzimuthDelta -= M_PI;
  }

  N_Types::Radian64 l_DeltaElevation = m_SensorData.WeaponElevationMax - m_SensorData.WeaponElevationMin;
  N_Types::Radian64 l_DeltaAzimuth   = l_AzimuthDelta;

  m_SensorData.WeaponElevation = m_SensorData.WeaponElevationMin + (drand48() * l_DeltaElevation);
  m_SensorData.WeaponAzimuth   = m_SensorData.WeaponAzimuthMin + (drand48() * l_DeltaAzimuth);

  INFO_LOG(m_logger) <<  "l_DeltaElevation   : " << l_DeltaElevation << " (" << l_DeltaElevation.getDegree() << ")";
  INFO_LOG(m_logger) <<  "l_DeltaAzimuth     : " << l_DeltaAzimuth << " (" << l_DeltaAzimuth.getDegree() << ")";
  INFO_LOG(m_logger) <<  "WeaponElevation    : " << m_SensorData.WeaponElevation << " (" << m_SensorData.WeaponElevation.getDegree() << ")" ;
  INFO_LOG(m_logger) <<  "WeaponAzimuth      : " << m_SensorData.WeaponAzimuth << " (" << m_SensorData.WeaponAzimuth.getDegree() << ")";

}


} // namespace N_GeoPairingImpl