/**
  * @RDE_HEADER@
  * NGSConnector class for using DIS protocol
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Hannes Baeuerle, SLSE, 2016-01-28
  */

#include "ngs_connector_dis.h"

#include <IPlayerSOM.h>

#include <IncompleteDISPDUError.h>

// ngs player interfaces
#include <IPlayerHumanObjInstance.h>
#include <IPlayerPhysicalEntityObjInstance.h>
#include <IPlayerAggregateEntityObjInstance.h>
#include <IPlayerGroundVehicleObjInstance.h>
#include <IPlayerAggregateEntityObjInstance.h>

#include <IPlayerHumanObjClass.h>
#include <IPlayerGroundVehicleObjClass.h>
#include <IPlayerAggregateEntityObjClass.h>

#include <SpatialStruct.h>
#include <DimensionStruct.h>
#include <RTIObjectIdStruct.h>
#include <RTIObjectIdArrayStruct.h>
#include <MarkingStruct.h>

#include <WeaponStateEnum32.h>
#include <StanceCodeEnum32.h>
#include <CamouflageEnum32.h>
#include <ForceIdentifierEnum8.h>
#include <TrailingEffectsCodeEnum32.h>
#include <HatchStateEnum32.h>
#include <AggregateStateEnum8.h>
#include <FormationEnum32.h>

#include <ArrayOfArticulatedParameterStructDynamicHLAunpaddedLengthlessVarArray.h>
#include <ArrayOfPropulsionSystemDataStructDynamicHLAlengthlessVarArray.h>
#include <ArrayOfVectoringNozzleSystemDataStructDynamicHLAlengthlessVarArray.h>
#include <ArrayOfSilentAggregateStructDynamicHLAlengthlessVarArray.h>
#include <ArrayOfSilentEntityStructDynamicHLAlengthlessVarArray.h>
#include <ArrayOfVariableDatumStructDynamicHLAlengthlessVarArray.h>

#include "logger.h"

namespace HighLevelArchitecture
{

NgsConnectorDis::NgsConnectorDis(const QString &fedName, MainHandler *mainHandler)
  : NgsConnector(fedName, mainHandler)
{}

//bool NgsConnectorDis::addAggregateEntity(const ParticipantInformation &playerData,
//                                         const dds::core::vector<int32_t> &children)
//{
//  ERROR_LOG(m_logger) << "AggregateEntities (Unit typeclass) not yet supported with DIS.";
//  // TODO: Has to be implemented!
//  Q_UNUSED(playerData)
//  Q_UNUSED(children)

//  return false;
//}

bool NgsConnectorDis::addGroundVehicle(const ParticipantInformation &playerData,
                                       const rpr2::TacticalStatusStruct &state)
{
  if (m_participantMap.contains(playerData.id))
    return false;

  DEBUG_LOG(m_logger) << "addGroundVehicle("
                      << playerData.id << ", "
                      << playerData.partyId << ", "
                      << playerData.entityTypeString << ", "
                      << state.TacticalStatus << ")";

  rpr2::IPlayerGroundVehicleObjClass &player = m_somPlayer->getPlayerGroundVehicleObjClass();
  rpr2::IPlayerGroundVehicleObjInstance *playerInstance = &player.newGroundVehicleObjInstance();

  if (!addBaseEntityAttributes(playerInstance, playerData))
    return false;
  addPhysicalEntityAttributes(playerInstance, playerData, state);
  addPlatformEntityAttributes(playerInstance);

  m_participantMap.insert(playerData.id, playerInstance);
  playerInstance->publish();

  return true;
}

bool NgsConnectorDis::addHuman(const ParticipantInformation &playerData,
                               const rpr2::TacticalStatusStruct &state)
{
  if (m_participantMap.contains(playerData.id))
    return false;

  DEBUG_LOG(m_logger) << "addHuman("
                      << playerData.id << ", "
                      << playerData.partyId << ", "
                      << playerData.entityTypeString << ", "
                      << state.TacticalStatus << ")";

  rpr2::IPlayerHumanObjClass &player = m_somPlayer->getPlayerHumanObjClass();
  rpr2::IPlayerHumanObjInstance *playerInstance = &player.newHumanObjInstance();

  if (!addBaseEntityAttributes(playerInstance, playerData))
    return false;
  addPhysicalEntityAttributes(playerInstance, playerData, state);
  addLifeFormEntityAttributes(playerInstance);

  m_participantMap.insert(playerData.id, playerInstance);
  playerInstance->publish();

  return true;
}

bool NgsConnectorDis::addAircraft(const ParticipantInformation &playerData,
                                  const rpr2::TacticalStatusStruct &state)
{
  ERROR_LOG(m_logger) << "Aircrafts not yet supported with DIS.";
  // TODO: Has to be implemented!
  Q_UNUSED(playerData)
  Q_UNUSED(state)

  return false;
}

//bool NgsConnectorDis::addVessel(const ParticipantInformation &playerData,
//                                const rpr2::TacticalStatusStruct &state)
//{
//  ERROR_LOG(m_logger) << "Vessels not yet supported with DIS.";
//  // TODO: Has to be implemented!
//  Q_UNUSED(playerData)
//  Q_UNUSED(state)

//  return false;
//}

bool NgsConnectorDis::addBaseEntityAttributes(rpr2::IPlayerBaseEntityObjInstance *playerInstance,
                                              const ParticipantInformation &partData)
{
  IdentifierStruct is = IdentifierStruct::fromInt(partData.externalId);
  if (!is.isOwnElement())
  {
    playerInstance->deleteObjectInstance();
    return false;
  }

  playerInstance->setEntityIdentifier(is.toEIS());

  setParticipantPartyId(*playerInstance, partData.id, partData.partyId);

  //set world location
  rpr2::SpatialStruct ss;
  ss.DeadReckoningAlgorithm__A__Alternatives.discriminant(rpr2::DeadReckoningAlgorithmEnum8::Static);
  ss.DeadReckoningAlgorithm__A__Alternatives.SpatialStatic().WorldLocation     =  partData.position.toWorldLocation();
  ss.DeadReckoningAlgorithm__A__Alternatives.SpatialStatic().Orientation.Phi   =  -2.504036;
  ss.DeadReckoningAlgorithm__A__Alternatives.SpatialStatic().Orientation.Psi   =  1.865257;
  ss.DeadReckoningAlgorithm__A__Alternatives.SpatialStatic().Orientation.Theta =  -0.103978;
  ss.DeadReckoningAlgorithm__A__Alternatives.SpatialStatic().IsFrozen          =  false;

  playerInstance->setSpatial(ss);

  rpr2::IPlayerBaseEntityObjInstance *parent = m_participantMap.value(partData.parentId);
  rpr2::IPlayerAggregateEntityObjInstance *parentAggregate = dynamic_cast<rpr2::IPlayerAggregateEntityObjInstance *>(parent);
  if (parentAggregate)
  {
    bool bChildFound = false;
    rpr2::RTIObjectIdStruct childObjectId;
    childObjectId.ID = rpr2::OMT13string(playerInstance->getObjectName());

    rpr2::RTIObjectIdArrayStruct children = parentAggregate->getEntityIdentifiers();
    for (unsigned int i = 0; i < children.NumberOfObjectIDs__A__ID.size(); ++i)
    {
      if (children.NumberOfObjectIDs__A__ID[i] == childObjectId)
      {
        bChildFound = true;
        break;
      }
    }
    if (!bChildFound)
    {
      addOneItemToArray(children.NumberOfObjectIDs__A__ID, childObjectId);
      parentAggregate->setEntityIdentifiers(children);
      parentAggregate->publish();
    }
  }
  return true;
}

void NgsConnectorDis::addPhysicalEntityAttributes(rpr2::IPlayerPhysicalEntityObjInstance *playerInstance,
                                                  const ParticipantInformation &partData,
                                                  const rpr2::TacticalStatusStruct &state)
{
  DamageModel dm(rpr2::EntityIdentifierStruct(), state);
  playerInstance->setDamageState(dm.damageState());
  playerInstance->setImmobilized(dm.immobilized());
  playerInstance->setFirePowerDisabled(dm.firePowerDisabled());

  rpr2::MarkingStruct ms;
  fillMarkingStruct(ms, partData.partyId, partData.id);
  playerInstance->setMarking(ms);

  rpr2::ForceIdentifierEnum8::ET forceId = partyIdToForceIdentifier(partData.partyId);
  playerInstance->setForceIdentifier(forceId);

  playerInstance->setAcousticSignatureIndex(0);
  playerInstance->setAlternateEntityType(rpr2::EntityTypeStruct());
  playerInstance->setArticulatedParametersArray(rpr2::ArrayOfArticulatedParameterStructDynamicHLAunpaddedLengthlessVarArray());
  playerInstance->setCamouflageType(rpr2::CamouflageEnum32::UniformPaintScheme);
  playerInstance->setEngineSmokeOn(false);
  playerInstance->setFlamesPresent(false);
  playerInstance->setForceIdentifier(rpr2::ForceIdentifierEnum8::Other);
  playerInstance->setHasAmmunitionSupplyCap(false);
  playerInstance->setHasFuelSupplyCap(false);
  playerInstance->setHasRecoveryCap(false);
  playerInstance->setHasRepairCap(false);
  playerInstance->setInfraredSignatureIndex(0);
  playerInstance->setIsConcealed(false);
  playerInstance->setLiveEntityMeasuredSpeed(0);
  playerInstance->setPowerPlantOn(false);
  playerInstance->setPropulsionSystemsData(rpr2::ArrayOfPropulsionSystemDataStructDynamicHLAlengthlessVarArray());
  playerInstance->setRadarCrossSectionSignatureIndex(0);
  playerInstance->setSmokePlumePresent(false);
  playerInstance->setTentDeployed(false);
  playerInstance->setTrailingEffectsCode(rpr2::TrailingEffectsCodeEnum32::NoTrail);
  playerInstance->setVectoringNozzleSystemData(rpr2::ArrayOfVectoringNozzleSystemDataStructDynamicHLAlengthlessVarArray());
}

void NgsConnectorDis::addLifeFormEntityAttributes(rpr2::IPlayerLifeformObjInstance *playerInstance)
{
  playerInstance->setComplianceState(rpr2::ComplianceStateEnum32::Other);
  playerInstance->setFlashLightsOn(false);
  playerInstance->setPrimaryWeaponState(rpr2::WeaponStateEnum32::Stowed);
  playerInstance->setSecondaryWeaponState(rpr2::WeaponStateEnum32::Stowed);
  playerInstance->setStanceCode(rpr2::StanceCodeEnum32::UprightStandingStill);
}

void NgsConnectorDis::addPlatformEntityAttributes(rpr2::IPlayerPlatformObjInstance *playerInstance)
{
  playerInstance->setAfterburnerOn(false);
  playerInstance->setAntiCollisionLightsOn(false);
  playerInstance->setBlackOutBrakeLightsOn(false);
  playerInstance->setBlackOutLightsOn(false);
  playerInstance->setBrakeLightsOn(false);
  playerInstance->setFormationLightsOn(false);
  playerInstance->setHatchState(rpr2::HatchStateEnum32::NotApplicable);
  playerInstance->setHeadLightsOn(false);
  playerInstance->setInteriorLightsOn(false);
  playerInstance->setLandingLightsOn(false);
  playerInstance->setLauncherRaised(false);
  playerInstance->setNavigationLightsOn(false);
  playerInstance->setRampDeployed(false);
  playerInstance->setRunningLightsOn(false);
  playerInstance->setSpotLightsOn(false);
  playerInstance->setTailLightsOn(false);
}

} // namespace HighLevelArchitecture