// flarm.cxx -- Flarm protocol class
//
// Written by Thorsten Brehm, started November 2017.
//
// Copyright (C) 2017 Thorsten Brehm - brehmt (at) gmail com
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
// License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
//
// $Id$

#include "config.h"

#include <cstdlib>
#include <cstring>
#include <cstdio>

#include <simgear/debug/logstream.hxx>
#include <simgear/constants.h>
#include <simgear/io/iochannel.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/sg_inlines.h>

#include <FDM/flightProperties.hxx>
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
#include <flightgearBuildId.h>

#include "flarm.hxx"

using namespace NMEA;

// #define FLARM_DEBUGGING

#ifdef FLARM_DEBUGGING
#warning Flarm debugging is enabled!
#endif

/*
 * The Flarm protocol emulation reports multi-player and AI aircraft within the
 * configured range using NMEA-style messages. The module supports bidirectional
 * communication, i.e. its capable of sending messages, and also of receiving
 * (and replying to) configuration commands. The emulation should be good enough
 * to convince standard NAV/moving map clients (tablet APPs like skydemon, skymap,
 * xcsoar etc) that they're connected to a Flarm device. The clients receive the
 * GPS position and traffic information from the flight simulator.
 *
 * By default, the emulation reports all moving aircraft as targets of the lowest
 * alert level ("traffic info only"). Parked aircraft are not reported.
 *
 * Higher alert levels (low-level/import/urgent alert) are only triggered
 * when the FlightGear TCAS instrument is installed in the aircraft model (yes,
 * Flarm is not TCAS, but reusing the TCAS threat-level for the emulation
 * should be good enough :-) ). The TCAS instrument classifies all AI and MP
 * aircraft into 4 threat levels (from invisible to high alert) - similar to
 * the actual alert levels normally provided by a Flarm device.
 *
 * Supported NMEA messages:
 *      $GPRMC: own position information
 *
 * Supported Garmin proprietary messages:
 *      $PGRMZ: own barometric altitude information in feet
 *
 * Supported Flarm proprietary messages:
 *      $PFLAU: status and intruder data
 *      $PFLAA: data on targets within range
 *      $PFLAV: version information
 *      $PFLAE: device status information
 *      $PFLAC: configuration request
 *      $PFLAS: debug information
 *
 * Module properties:
 *   All Flarm configuration properties are mirrored to /sim/flarm/config.
 *   Useful properties:
 *   /sim/flarm/config/RANGE    Range in meters when considering traffic targets
 *   /sim/flarm/config/NMEAOUT  NMEA message mode (0=off, 1=all, 2=Garmin/NMEA messages only, 3=Flarm messages only)
 *   /sim/flarm/config/ACFT     Aircraft type (1=glider, 3=helicopter, 8=motor aircraft, 9=jet)
 *
 */

FGFlarm::FGFlarm() :
    FGGarmin(),
    mFlarmMessages(FLARM::SET),
    mFlarmConfig(fgGetNode("/sim/flarm/config", true)),
    mLastUpdate(0)
{
    // Flarm (and Garmin) devices normally report barometric altitude in feet
    mMetric = false;
    // disable all Garmin messages, except PGRMZ
    mGarminMessages = GARMIN::PGRMZ;
    // Allow processing more message lines per cycle than with FG's standard NMEA protocol,
    // otherwise we won't reply quickly when a remote sends multiple configuration requests.
    mMaxReceiveLines = 20;
    // allow bidirectional communication (we're sending data and accepting requests)
    mBiDirectionalSupport = true;

#ifdef FLARM_DEBUGGING
    // show I/O debug messages
    sglog().set_log_classes(SG_IO);
    sglog().set_log_priority(SG_DEBUG);
#endif

    // some default configuration data, to please XCSoar and other apps
    const unsigned int zero=0;
    setDefaultConfigValue("ACFT",       9);
    setDefaultConfigValue("ADDWP",      "");
    setDefaultConfigValue("BAUD",       5);
    setDefaultConfigValue("CFLAGS",     zero);
    setDefaultConfigValue("COMPID",     "");
    setDefaultConfigValue("COMPCLASS",  "");
    setDefaultConfigValue("COPIL",      "");
    setDefaultConfigValue("GLIDERID",   "");
    setDefaultConfigValue("GLIDERTYPE", "");
    setDefaultConfigValue("ID",         "0");
    setDefaultConfigValue("LOGINT",     2);
    setDefaultConfigValue("NEWTASK",    "");
    setDefaultConfigValue("NMEAOUT",    1); // all messages enabled
    setDefaultConfigValue("NOTRACK",    zero); // disabled
    setDefaultConfigValue("PILOT",      "Curt"); // :-)))
    setDefaultConfigValue("PRIV",       zero);
    setDefaultConfigValue("RANGE",      25500);
    setDefaultConfigValue("THRE",       2);
    setDefaultConfigValue("UI",         zero);
}


FGFlarm::~FGFlarm() {
}


void FGFlarm::setDefaultConfigValue(const char* ConfigKey, const char* Value)
{
    if (!mFlarmConfig->hasValue(ConfigKey))
        mFlarmConfig->setStringValue(ConfigKey, Value);
}


void FGFlarm::setDefaultConfigValue(const char* ConfigKey, unsigned int Value)
{
    if (!mFlarmConfig->hasValue(ConfigKey))
        mFlarmConfig->setIntValue(ConfigKey, Value);
}


// generate Flarm NMEA messages
bool FGFlarm::gen_message()
{
    // generate generic messages first
    FGGarmin::gen_message();

    // traffic updates once per second only, independent of the normal protocol frequency
    if ((get_count()-mLastUpdate)/get_hz() < 1.0)
    {
        return true;
    }
    mLastUpdate = get_count();

    char nmea[256];
    int TargetCount=0;
    double ClosestDistanceM2 = 99e9;
    double ClosestLond=0.0, ClosestLatd=0.0;
    int ClosestRelVerticalM=0;
    int ClosestThreatLevel=-1;

    // obtain own position
    double latd = mFdm.get_Latitude() * SGD_RADIANS_TO_DEGREES;
    double lond = mFdm.get_Longitude() * SGD_RADIANS_TO_DEGREES;

    // PFLAA (Flarm proprietary)
    if (mFlarmMessages & FLARM::PFLAA)
    {
        double altitude_ft = mFdm.get_Altitude();

        // check all AI/MP aircraft
        SGPropertyNode* pAi = fgGetNode("/ai/models", true);
        simgear::PropertyList aircraftList = pAi->getChildren("aircraft");
        for (simgear::PropertyList::iterator i = aircraftList.begin(); i != aircraftList.end(); ++i)
        {
            SGPropertyNode* pModel = *i;
            if ((pModel)&&(pModel->nChildren()))
            {
                double GroundSpeedKt = pModel->getDoubleValue("velocities/true-airspeed-kt", 0.0);
                int threatLevel = pModel->getIntValue("tcas/threat-level", -99);
                // threatLevel is undefined (-99) when no TCAS is installed
                if (threatLevel == -99)
                {
                    // set threat level to 0 (traffic info) when a/c is moving. Otherwise -1 (invisible).
                    threatLevel = (GroundSpeedKt>1) ? 0 : -1;
                }

                // report traffic, unless considered "invisible"
                if (threatLevel >= 0)
                {
                    // position data of current intruder
                    double targetLatd = pModel->getDoubleValue("position/latitude-deg", 0.0);
                    double targetLond = pModel->getDoubleValue("position/longitude-deg", 0.0);

                    // calculate the relative North and relative East distances in meters, as
                    // required by the Flarm protocol
                    double RelNorthAngleDeg = targetLatd - latd;
                    double RelNorth = ((2*SG_PI*SG_POLAR_RADIUS_M) / 360.0) * RelNorthAngleDeg;

                    double RelEastAngleDeg  = targetLond - lond;
                    double RelEast  = ((2*SG_PI*SG_EQUATORIAL_RADIUS_M) / 360.0) *
                                        abs(cos(latd*SGD_DEGREES_TO_RADIANS)) * RelEastAngleDeg;

#ifdef FLARM_DEBUGGING
                    {
                        double distanceM = sqrt(RelNorth*RelNorth+RelEast*RelEast);
                        pModel->setDoubleValue("flarm/distance", distanceM);
                        pModel->setIntValue("flarm/alive", pModel->getIntValue("flarm/alive",0)+1);
                    }
#endif

                    // do not consider targets beyond 100km (1e5 meters)
                    double FlarmRangeM = mFlarmConfig->getIntValue("RANGE", 25500);
                    double DistanceM2 = RelNorth*RelNorth+RelEast*RelEast;
                    if (DistanceM2 < FlarmRangeM*FlarmRangeM)
                    {
                        TargetCount++;
#if 0//def FLARM_DEBUGGING
                        {
                            double distanceM = sqrt(RelNorth*RelNorth+RelEast*RelEast);
                            printf("%3u: id %3u, %s, distance: %.1fkm, North: %.1f, East: %.1f, speed: %.1f kt\n",
                                    pModel->getIndex(),
                                    pModel->getIntValue("id"),
                                    pModel->getStringValue("callsign", "<no name>"),
                                    distanceM/1000.0, RelNorth/1e3, RelEast/1e3, GroundSpeedKt);
                        }
#endif

                        int RelVerticalM = (pModel->getDoubleValue("position/altitude-ft", 0.0)-altitude_ft)* SG_FEET_TO_METER;
                        int Track = pModel->getDoubleValue("orientation/true-heading-deg", 0.0);
                        int ClimbRateMs = pModel->getDoubleValue("velocities/vertical-speed-fps", 0.0) * (SG_FPS_TO_KT * SG_KT_TO_MPS);
                        int AcftType = 9; // report as jet aircraft for now
                        // generate some fake 6-digit hex code
                        unsigned int ID = pModel->getIntValue("id") & 0x00FFFFFF;
                        //$PFLAA,AlarmLevel,RelNorth,RelEast,RelVertical,IDType,ID,Track,TurnRate,GroundSpeed,ClimbRate,AcftType
                        snprintf( nmea, 256, "$PFLAA,%u,%i,%i,%i,2,%06X,%u,,%i,%i,%u",
                                 threatLevel, (int)RelNorth, (int)RelEast, RelVerticalM, ID,
                                 Track, (int) (GroundSpeedKt*SG_KT_TO_MPS), ClimbRateMs, AcftType);
                        add_with_checksum(nmea, 256);

                        if (DistanceM2 < ClosestDistanceM2)
                        {
                            ClosestDistanceM2 = DistanceM2;
                            ClosestThreatLevel = threatLevel;
                            ClosestRelVerticalM = RelVerticalM;
                            ClosestLatd = targetLatd;
                            ClosestLond = targetLond;
                        }
                    }
                }
            }
        }
    }

    // PFLAU (Flarm proprietary)
    if (mFlarmMessages & FLARM::PFLAU)
    {
        if (ClosestThreatLevel < 0)
        {
            // no threats, but maybe some targets
            snprintf( nmea, 256, "$PFLAU,%u,1,1,1,0,,0,,", TargetCount);
        }
        else
        {
            // calculate the bearing and range of the closest target
            double az2, bearing, distanceM;
            geo_inverse_wgs_84(latd, lond, ClosestLatd, ClosestLond, &bearing, &az2, &distanceM);

            // calculate relative bearing
            double heading = mFdm.get_Psi_deg();
            bearing -= heading;
            SG_NORMALIZE_RANGE(bearing, -180.0, 180.0);

            // set alert mode, depending on TCAS threat level
            int AlertMode = 0; // no alert
            if (ClosestThreatLevel >= 2) // TCAS RA alert
                AlertMode = 2; // alarm!
            else
            if (ClosestThreatLevel == 1) // TCAS proximity alert
                AlertMode = 1; // warning
            snprintf( nmea, 256, "$PFLAU,%u,1,1,1,%u,%.0f,%u,%u,%u",
                    TargetCount, ClosestThreatLevel, bearing, AlertMode, ClosestRelVerticalM, (unsigned int) distanceM);
        }
        add_with_checksum(nmea, 256);
    }

    return true;
}

// process a Flarm sentence
void FGFlarm::parse_message(const std::vector<std::string>& tokens)
{
    char nmea[256];

    if ( tokens[0] == "PFLAE" )
    {
        if (tokens.size()<2)
            return;

        // #1: request
        const string& request = tokens[1];
        SG_LOG( SG_IO, SG_DEBUG, "  PFLAE request = " << request );
        if (request == "R")
        {
            // report "no errors"
            snprintf( nmea, 256, "$PFLAE,A,0,0");
            add_with_checksum(nmea, 256);
            SG_LOG( SG_IO, SG_DEBUG, "  PFLAE reply= " << nmea );
        }
    }
    else
    if ( tokens[0] == "PFLAV" )
    {
        if (tokens.size()<2)
            return;

        // #1: request
        const string& request = tokens[1];
        SG_LOG( SG_IO, SG_DEBUG, "  PFLAV version request = " << request );
        if (request == "R")
        {
            // report some fixed version to please the requesting device
            snprintf( nmea, 256, "$PFLAV,A,2.00,6.00,");
            add_with_checksum(nmea, 256);
            SG_LOG( SG_IO, SG_DEBUG, "  PFLAV reply= " << nmea );
        }
    }
    else
    if ( tokens[0] == "PFLAS" )
    {
        // status/debug information

        if (tokens.size()<2)
            return;

        // #1: request
        const string& request = tokens[1];
        SG_LOG( SG_IO, SG_DEBUG, "  PFLAS status/debug request = " << request );
        if (request == "R")
        {
            // Report some debug data to please the requesting device.
            // Apparently debug replies are not in NMEA format and contain plain text.
            const char* FlrmDebugReply = (
                    "------------------------------------------\r\n"
                    "FlightGear " FLIGHTGEAR_VERSION "\r\n"
                    "Revision   " REVISION "\r\n"
                    "------------------------------------------\r\n");
            mNmeaSentence += FlrmDebugReply;
            SG_LOG( SG_IO, SG_DEBUG, "  PFLAS reply = " << FlrmDebugReply );
        }
    }
    else
    if ( tokens[0] == "PFLAC" )
    {
        // configuration command

        if (tokens.size()<3)
            return;

        bool Error = true;

        // #1: request
        const string& request = tokens[1];

        SG_LOG( SG_IO, SG_DEBUG, "  PFLAC config request = " << request );

        // #2: keyword
        const string& keyword = tokens[2];
        SG_LOG( SG_IO, SG_DEBUG, "  PFLAC config request = " << keyword );

        // check if the config element is supported
        SGPropertyNode* configNode = mFlarmConfig->getChild(keyword,0,false);
        if (!configNode)
        {
            // unsupported configuration element
        }
        else
        if (request == "R")
        {
            // reply with config data
            snprintf( nmea, 256, "$PFLAC,A,%s,%s",
                    keyword.c_str(), configNode->getStringValue().c_str());
            add_with_checksum(nmea, 256);
            Error = false;
        }
        else
        if (request == "S")
        {
            if (tokens.size()<4)
                return;
            Error = false;

            // special handling for some parameters
            if (keyword == "NMEAOUT")
            {
                // #3: value
                int value = (int) atof(tokens[3].c_str());
                switch(value % 10)
                {
                    case 0:
                        // disable all periodic messages
                        mNmeaMessages   = 0;
                        mGarminMessages = 0;
                        mFlarmMessages  = 0;
                        break;
                    case 1:
                        // enable all periodic messages
                        mNmeaMessages   = NMEA::SET;
                        mGarminMessages = GARMIN::PGRMZ;
                        mFlarmMessages  = FLARM::SET;
                        break;
                    case 2:
                        // enable all, except periodic Flarm messages
                        mNmeaMessages   = NMEA::SET;
                        mGarminMessages = GARMIN::PGRMZ;
                        mFlarmMessages  = 0;
                        break;
                    case 3:
                        // enable all, except periodic NMEA messages
                        mNmeaMessages   = 0;
                        mGarminMessages = GARMIN::PGRMZ;
                        mFlarmMessages  = FLARM::SET;
                        break;
                    default:
                        Error = true;
                }
            }

            if (!Error)
            {
                // just store the config data as it is
                configNode->setStringValue(tokens[3]);

                // reply
                snprintf( nmea, 256, "$PFLAC,A,%s,%s",
                        keyword.c_str(), configNode->getStringValue().c_str());
                add_with_checksum(nmea, 256);
            }
        }

        if (Error)
        {
            // report error for unsupported requests
            snprintf( nmea, 256, "$PFLAC,A,ERROR");
            add_with_checksum(nmea, 256);
        }
    }
    else
    {
        // Invalid or unsupported message.
        // In flarm mode, we only accept flarm messages as input, but no other messages, i.e. no position updates.
        // Use the Garmin or NMEA basic protocols to feed position data into FG.
        SG_LOG( SG_IO, SG_DEBUG, "  Unsupported message = " << tokens[0] );
    }
}