/*
 * Service module for swift<->FG connection
 * SPDX-FileCopyrightText: (C) 2019-2022 swift Project Community / Contributors (https://swift-project.org/)
 * SPDX-FileCopyrightText: (C) 2019-2022 Lars Toenning <dev@ltoenning.de>
 * SPDX-License-Identifier: GPL-2.0-or-later
 */

#include "service.h"
#include <Main/fg_props.hxx>
#include <iostream>
#include <simgear/constants.h>
#include <simgear/debug/logstream.hxx>
#include <simgear/structure/commands.hxx>

#define FGSWIFTBUS_API_VERSION 3;

namespace flightgear::swift {

static const std::string k_fgswiftbus_service_interfacename = "org.swift_project.fgswiftbus.service";
static const std::string k_fgswiftbus_service_objectpath = "/fgswiftbus/service";

CService::CService()
{
    // Initialize node pointers
    m_textMessageNode = fgGetNode("/sim/messages/copilot", true);
    m_aircraftModelPathNode = fgGetNode("/sim/aircraft-dir", true);
    m_aircraftDescriptionNode = fgGetNode("/sim/description", true);
    m_isPausedNode = fgGetNode("/sim/freeze/master", true);
    m_latitudeNode = fgGetNode("/position/latitude-deg", true);
    m_longitudeNode = fgGetNode("/position/longitude-deg", true);
    m_altitudeMSLNode = fgGetNode("/position/altitude-ft", true);
    m_heightAGLNode = fgGetNode("/position/altitude-agl-ft", true);
    m_groundSpeedNode = fgGetNode("/velocities/groundspeed-kt", true);
    m_pitchNode = fgGetNode("/orientation/pitch-deg", true);
    m_rollNode = fgGetNode("/orientation/roll-deg", true);
    m_trueHeadingNode = fgGetNode("/orientation/heading-deg", true);
    m_wheelsOnGroundNode = fgGetNode("/gear/gear/wow", true);
    m_com1ActiveNode = fgGetNode("/instrumentation/comm/frequencies/selected-mhz", true);
    m_com1StandbyNode = fgGetNode("/instrumentation/comm/frequencies/standby-mhz", true);
    m_com2ActiveNode = fgGetNode("/instrumentation/comm[1]/frequencies/selected-mhz", true);
    m_com2StandbyNode = fgGetNode("/instrumentation/comm[1]/frequencies/standby-mhz", true);
    m_transponderCodeNode = fgGetNode("/instrumentation/transponder/id-code", true);
    m_transponderModeNode = fgGetNode("/instrumentation/transponder/inputs/knob-mode", true);
    m_transponderIdentNode = fgGetNode("/instrumentation/transponder/ident", true);
    m_beaconLightsNode = fgGetNode("/controls/lighting/beacon", true);
    m_landingLightsNode = fgGetNode("/controls/lighting/landing-lights", true);
    m_navLightsNode = fgGetNode("/controls/lighting/nav-lights", true);
    m_strobeLightsNode = fgGetNode("/controls/lighting/strobe", true);
    m_taxiLightsNode = fgGetNode("/controls/lighting/taxi-light", true);
    m_altimeterServiceableNode = fgGetNode("/instrumentation/altimeter/serviceable", true);
    m_pressAltitudeFtNode = fgGetNode("/instrumentation/altimeter/pressure-alt-ft", true);
    m_flapsDeployRatioNode = fgGetNode("/surface-positions/flap-pos-norm", true);
    m_gearDeployRatioNode = fgGetNode("/gear/gear/position-norm", true);
    m_speedBrakeDeployRatioNode = fgGetNode("/surface-positions/speedbrake-pos-norm", true);
    m_aircraftNameNode = fgGetNode("/sim/aircraft", true);
    m_groundElevationNode = fgGetNode("/position/ground-elev-m", true);
    m_velocityXNode = fgGetNode("/velocities/speed-east-fps", true);
    m_velocityYNode = fgGetNode("/velocities/speed-down-fps", true);
    m_velocityZNode = fgGetNode("/velocities/speed-north-fps", true);
    m_rollRateNode = fgGetNode("/orientation/roll-rate-degps", true);
    m_pichRateNode = fgGetNode("/orientation/pitch-rate-degps", true);
    m_yawRateNode = fgGetNode("/orientation/yaw-rate-degps", true);
    m_com1VolumeNode = fgGetNode("/instrumentation/comm/volume", true);
    m_com2VolumeNode = fgGetNode("/instrumentation/comm[1]/volume", true);

    SG_LOG(SG_NETWORK, SG_INFO, "FGSwiftBus Service initialized");
}

const std::string& CService::InterfaceName()
{
    return k_fgswiftbus_service_interfacename;
}

const std::string& CService::ObjectPath()
{
    return k_fgswiftbus_service_objectpath;
}

// Static method
int CService::getVersionNumber()
{
    return FGSWIFTBUS_API_VERSION;
}

void CService::addTextMessage(const std::string& text)
{
    if (text.empty()) { return; }
    m_textMessageNode->setStringValue(text);
}

std::string CService::getAircraftModelPath() const
{
    return m_aircraftModelPathNode->getStringValue();
}

std::string CService::getAircraftLivery() const
{
    return "";
}

std::string CService::getAircraftIcaoCode() const
{
    return "";
}

std::string CService::getAircraftDescription() const
{
    return m_aircraftDescriptionNode->getStringValue();
}

bool CService::isPaused() const
{
    return m_isPausedNode->getBoolValue();
}

double CService::getLatitude() const
{
    return m_latitudeNode->getDoubleValue();
}

double CService::getLongitude() const
{
    return m_longitudeNode->getDoubleValue();
}

double CService::getAltitudeMSL() const
{
    return m_altitudeMSLNode->getDoubleValue();
}

double CService::getHeightAGL() const
{
    return m_heightAGLNode->getDoubleValue();
}

double CService::getGroundSpeed() const
{
    return m_groundSpeedNode->getDoubleValue();
}

double CService::getPitch() const
{
    return m_pitchNode->getDoubleValue();
}

double CService::getRoll() const
{
    return m_rollNode->getDoubleValue();
}

double CService::getTrueHeading() const
{
    return m_trueHeadingNode->getDoubleValue();
}

bool CService::getAllWheelsOnGround() const
{
    return m_wheelsOnGroundNode->getBoolValue();
}

int CService::getCom1Active() const
{
    return (int)(m_com1ActiveNode->getDoubleValue() * 1000);
}

int CService::getCom1Standby() const
{
    return (int)(m_com1StandbyNode->getDoubleValue() * 1000);
}

int CService::getCom2Active() const
{
    return (int)(m_com2ActiveNode->getDoubleValue() * 1000);
}

int CService::getCom2Standby() const
{
    return (int)(m_com2StandbyNode->getDoubleValue() * 1000);
}

int CService::getTransponderCode() const
{
    return m_transponderCodeNode->getIntValue();
}

int CService::getTransponderMode() const
{
    return m_transponderModeNode->getIntValue();
}

bool CService::getTransponderIdent() const
{
    return m_transponderIdentNode->getBoolValue();
}

bool CService::getBeaconLightsOn() const
{
    return m_beaconLightsNode->getBoolValue();
}

bool CService::getLandingLightsOn() const
{
    return m_landingLightsNode->getBoolValue();
}

bool CService::getNavLightsOn() const
{
    return m_navLightsNode->getBoolValue();
}


bool CService::getStrobeLightsOn() const
{
    return m_strobeLightsNode->getBoolValue();
}

bool CService::getTaxiLightsOn() const
{
    return m_taxiLightsNode->getBoolValue();
}

double CService::getPressAlt() const
{
    if (m_altimeterServiceableNode->getBoolValue()) {
        return m_pressAltitudeFtNode->getDoubleValue();
    } else {
        return m_altitudeMSLNode->getDoubleValue();
    }
}

void CService::setCom1Active(int freq)
{
    m_com1ActiveNode->setDoubleValue(freq / (double)1000);
}

void CService::setCom1Standby(int freq)
{
    m_com1StandbyNode->setDoubleValue(freq / (double)1000);
}

void CService::setCom2Active(int freq)
{
    m_com2ActiveNode->setDoubleValue(freq / (double)1000);
}

void CService::setCom2Standby(int freq)
{
    m_com2StandbyNode->setDoubleValue(freq / (double)1000);
}

void CService::setTransponderCode(int code)
{
    m_transponderCodeNode->setIntValue(code);
}

void CService::setTransponderMode(int mode)
{
    m_transponderModeNode->setIntValue(mode);
}

double CService::getFlapsDeployRatio() const
{
    return m_flapsDeployRatioNode->getFloatValue();
}

double CService::getGearDeployRatio() const
{
    return m_gearDeployRatioNode->getFloatValue();
}

int CService::getNumberOfEngines() const
{
    // TODO Use correct property
    return 2;
}

std::vector<double> CService::getEngineN1Percentage() const
{
    // TODO use correct engine numbers
    std::vector<double> list;
    const auto number = static_cast<unsigned int>(getNumberOfEngines());
    list.reserve(number);
    for (unsigned int engineNumber = 0; engineNumber < number; ++engineNumber) {
        list.push_back(fgGetDouble("/engine/engine/n1"));
    }
    return list;
}

double CService::getSpeedBrakeRatio() const
{
    return m_speedBrakeDeployRatioNode->getFloatValue();
}

double CService::getGroundElevation() const
{
    return m_groundElevationNode->getDoubleValue();
}

std::string CService::getAircraftModelFilename() const
{
    std::string modelFileName = getAircraftName();
    modelFileName.append("-set.xml");
    return modelFileName;
}

std::string CService::getAircraftModelString() const
{
    std::string modelName = getAircraftName();
    std::string modelString = "FG " + modelName;
    return modelString;
}

std::string CService::getAircraftName() const
{
    return m_aircraftNameNode->getStringValue();
}

double CService::getVelocityX() const
{
    return m_velocityXNode->getDoubleValue() * SG_FEET_TO_METER;
}

double CService::getVelocityY() const
{
    return m_velocityYNode->getDoubleValue() * SG_FEET_TO_METER * -1; // + (up), - (down)
}

double CService::getVelocityZ() const
{
    return m_velocityZNode->getDoubleValue() * SG_FEET_TO_METER;
}

double CService::getRollRate() const
{
    return m_rollRateNode->getDoubleValue() * SG_DEGREES_TO_RADIANS;
}
double CService::getPitchRate() const
{
    return m_pichRateNode->getDoubleValue() * SG_DEGREES_TO_RADIANS;
}

double CService::getYawRate() const
{
    return m_yawRateNode->getDoubleValue() * SG_DEGREES_TO_RADIANS;
}

double CService::getCom1Volume() const
{
    return m_com1VolumeNode->getDoubleValue();
}

double CService::getCom2Volume() const
{
    return m_com2VolumeNode->getDoubleValue();
}


static const char* introspection_service = DBUS_INTROSPECT_1_0_XML_DOCTYPE_DECL_NODE;

DBusHandlerResult CService::dbusMessageHandler(const CDBusMessage& message_)
{
    CDBusMessage message(message_);
    const std::string sender = message.getSender();
    const dbus_uint32_t serial = message.getSerial();
    const bool wantsReply = message.wantsReply();

    if (message.getInterfaceName() == DBUS_INTERFACE_INTROSPECTABLE) {
        if (message.getMethodName() == "Introspect") {
            sendDBusReply(sender, serial, introspection_service);
        }
    } else if (message.getInterfaceName() == k_fgswiftbus_service_interfacename) {
        if (message.getMethodName() == "addTextMessage") {
            maybeSendEmptyDBusReply(wantsReply, sender, serial);
            std::string text;
            message.beginArgumentRead();
            message.getArgument(text);

            queueDBusCall([this, text]() {
                addTextMessage(text);
            });
        } else if (message.getMethodName() == "getOwnAircraftSituationData") {
            queueDBusCall([this, sender, serial]() {
                double lat = getLatitude();
                double lon = getLongitude();
                double alt = getAltitudeMSL();
                double gs = getGroundSpeed();
                double pitch = getPitch();
                double roll = getRoll();
                double trueHeading = getTrueHeading();
                double pressAlt = getPressAlt();
                CDBusMessage reply = CDBusMessage::createReply(sender, serial);
                reply.beginArgumentWrite();
                reply.appendArgument(lat);
                reply.appendArgument(lon);
                reply.appendArgument(alt);
                reply.appendArgument(gs);
                reply.appendArgument(pitch);
                reply.appendArgument(roll);
                reply.appendArgument(trueHeading);
                reply.appendArgument(pressAlt);
                sendDBusMessage(reply);
            });
        } else if (message.getMethodName() == "getOwnAircraftVelocityData") {
            queueDBusCall([this, sender, serial]() {
                double velocityX = getVelocityX();
                double velocityY = getVelocityY();
                double velocityZ = getVelocityZ();
                double pitchVelocity = getPitchRate();
                double rollVelocity = getRollRate();
                double yawVelocity = getYawRate();
                CDBusMessage reply = CDBusMessage::createReply(sender, serial);
                reply.beginArgumentWrite();
                reply.appendArgument(velocityX);
                reply.appendArgument(velocityY);
                reply.appendArgument(velocityZ);
                reply.appendArgument(pitchVelocity);
                reply.appendArgument(rollVelocity);
                reply.appendArgument(yawVelocity);
                sendDBusMessage(reply);
            });
        } else if (message.getMethodName() == "getVersionNumber") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getVersionNumber());
            });
        } else if (message.getMethodName() == "getAircraftModelPath") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getAircraftModelPath());
            });
        } else if (message.getMethodName() == "getAircraftModelFilename") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getAircraftModelFilename());
            });
        } else if (message.getMethodName() == "getAircraftModelString") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getAircraftModelString());
            });
        } else if (message.getMethodName() == "getAircraftName") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getAircraftName());
            });
        } else if (message.getMethodName() == "getAircraftLivery") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getAircraftLivery());
            });
        } else if (message.getMethodName() == "getAircraftIcaoCode") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getAircraftIcaoCode());
            });
        } else if (message.getMethodName() == "getAircraftDescription") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getAircraftDescription());
            });
        } else if (message.getMethodName() == "isPaused") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, isPaused());
            });
        } else if (message.getMethodName() == "getLatitudeDeg") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getLatitude());
            });
        } else if (message.getMethodName() == "getLongitudeDeg") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getLongitude());
            });
        } else if (message.getMethodName() == "getAltitudeMslFt") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getAltitudeMSL());
            });
        } else if (message.getMethodName() == "getHeightAglFt") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getHeightAGL());
            });
        } else if (message.getMethodName() == "getGroundSpeedKts") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getGroundSpeed());
            });
        } else if (message.getMethodName() == "getPitchDeg") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getPitch());
            });
        } else if (message.getMethodName() == "getRollDeg") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getRoll());
            });
        } else if (message.getMethodName() == "getAllWheelsOnGround") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getAllWheelsOnGround());
            });
        } else if (message.getMethodName() == "getCom1ActiveKhz") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getCom1Active());
            });
        } else if (message.getMethodName() == "getCom1StandbyKhz") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getCom1Standby());
            });
        } else if (message.getMethodName() == "getCom2ActiveKhz") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getCom2Active());
            });
        } else if (message.getMethodName() == "getCom2StandbyKhz") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getCom2Standby());
            });
        } else if (message.getMethodName() == "getTransponderCode") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getTransponderCode());
            });
        } else if (message.getMethodName() == "getTransponderMode") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getTransponderMode());
            });
        } else if (message.getMethodName() == "getTransponderIdent") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getTransponderIdent());
            });
        } else if (message.getMethodName() == "getBeaconLightsOn") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getBeaconLightsOn());
            });
        } else if (message.getMethodName() == "getLandingLightsOn") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getLandingLightsOn());
            });
        } else if (message.getMethodName() == "getNavLightsOn") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getNavLightsOn());
            });
        } else if (message.getMethodName() == "getStrobeLightsOn") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getStrobeLightsOn());
            });
        } else if (message.getMethodName() == "getTaxiLightsOn") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getTaxiLightsOn());
            });
        } else if (message.getMethodName() == "getPressAlt") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getPressAlt());
            });
        } else if (message.getMethodName() == "getGroundElevation") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getGroundElevation());
            });
        } else if (message.getMethodName() == "setCom1ActiveKhz") {
            maybeSendEmptyDBusReply(wantsReply, sender, serial);
            int frequency = 0;
            message.beginArgumentRead();
            message.getArgument(frequency);
            queueDBusCall([this, frequency]() {
                setCom1Active(frequency);
            });
        } else if (message.getMethodName() == "setCom1StandbyKhz") {
            maybeSendEmptyDBusReply(wantsReply, sender, serial);
            int frequency = 0;
            message.beginArgumentRead();
            message.getArgument(frequency);
            queueDBusCall([this, frequency]() {
                setCom1Standby(frequency);
            });
        } else if (message.getMethodName() == "setCom2ActiveKhz") {
            maybeSendEmptyDBusReply(wantsReply, sender, serial);
            int frequency = 0;
            message.beginArgumentRead();
            message.getArgument(frequency);
            queueDBusCall([this, frequency]() {
                setCom2Active(frequency);
            });
        } else if (message.getMethodName() == "setCom2StandbyKhz") {
            maybeSendEmptyDBusReply(wantsReply, sender, serial);
            int frequency = 0;
            message.beginArgumentRead();
            message.getArgument(frequency);
            queueDBusCall([this, frequency]() {
                setCom2Standby(frequency);
            });
        } else if (message.getMethodName() == "setTransponderCode") {
            maybeSendEmptyDBusReply(wantsReply, sender, serial);
            int code = 0;
            message.beginArgumentRead();
            message.getArgument(code);
            queueDBusCall([this, code]() {
                setTransponderCode(code);
            });
        } else if (message.getMethodName() == "setTransponderMode") {
            maybeSendEmptyDBusReply(wantsReply, sender, serial);
            int mode = 0;
            message.beginArgumentRead();
            message.getArgument(mode);
            queueDBusCall([this, mode]() {
                setTransponderMode(mode);
            });
        } else if (message.getMethodName() == "getFlapsDeployRatio") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getFlapsDeployRatio());
            });
        } else if (message.getMethodName() == "getGearDeployRatio") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getGearDeployRatio());
            });
        } else if (message.getMethodName() == "getEngineN1Percentage") {
            queueDBusCall([this, sender, serial]() {
                std::vector<double> array = getEngineN1Percentage();
                sendDBusReply(sender, serial, array);
            });
        } else if (message.getMethodName() == "getSpeedBrakeRatio") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getSpeedBrakeRatio());
            });
        } else if (message.getMethodName() == "getCom1Volume") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getCom1Volume());
            });
        } else if (message.getMethodName() == "getCom2Volume") {
            queueDBusCall([this, sender, serial]() {
                sendDBusReply(sender, serial, getCom2Volume());
            });
        } else {
            // Unknown message. Tell DBus that we cannot handle it
            return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
        }
    }
    return DBUS_HANDLER_RESULT_HANDLED;
}


int CService::process()
{
    invokeQueuedDBusCalls();

    return 1;
}

} // namespace flightgear::swift
