// SPDX-FileCopyrightText: 2006 Durk Talsma
// SPDX-License-Identifier: GPL-2.0-or-later

#include <config.h>

#include <algorithm>
#include <cstdio>
#include <random>

#include <osg/Geode>
#include <osg/Geometry>
#include <osg/MatrixTransform>
#include <osg/Shape>

#include <simgear/scene/material/EffectGeode.hxx>
#include <simgear/scene/material/mat.hxx>
#include <simgear/scene/material/matlib.hxx>
#include <simgear/scene/util/OsgMath.hxx>
#include <simgear/timing/sg_time.hxx>

#include <Scenery/scenery.hxx>

#include "atc_mgr.hxx"
#include "trafficcontrol.hxx"
#include <AIModel/AIAircraft.hxx>
#include <AIModel/AIFlightPlan.hxx>
#include <AIModel/performancedata.hxx>
#include <Airports/airport.hxx>
#include <Airports/dynamics.hxx>
#include <Airports/groundnetwork.hxx>
#include <Radio/radio.hxx>
#include <Traffic/TrafficMgr.hxx>
#include <signal.h>

#include <ATC/ATCController.hxx>
#include <ATC/StartupController.hxx>
#include <ATC/atc_mgr.hxx>
#include <ATC/trafficcontrol.hxx>

using std::sort;
using std::string;

/***************************************************************************
 * class FGStartupController
 * subclass of FGATCController
 **************************************************************************/
FGStartupController::FGStartupController(FGAirportDynamics* par) : FGATCController()
{
    parent = par;
}

FGStartupController::~FGStartupController()
{
}

void FGStartupController::announcePosition(int id,
                                           FGAIFlightPlan* intendedRoute,
                                           int currentPosition, double lat,
                                           double lon, double heading,
                                           double speed, double alt,
                                           double radius, int leg,
                                           FGAIAircraft* ref)
{
    init();
    // Search activeTraffic for a record matching our id
    TrafficVectorIterator i = FGATCController::searchActiveTraffic(id);

    // Add a new TrafficRecord if no one exists for this aircraft.
    if (i == activeTraffic.end() || activeTraffic.empty()) {
        FGTrafficRecord* rec = new FGTrafficRecord();
        rec->setId(id);

        rec->setPositionAndHeading(lat, lon, heading, speed, alt, leg);
        rec->setRunway(intendedRoute->getRunway());
        rec->setLeg(leg);
        rec->setPositionAndIntentions(currentPosition, intendedRoute);
        rec->setCallsign(ref->getCallSign());
        rec->setAircraft(ref);
        rec->setHoldPosition(true);
        SGSharedPtr<FGTrafficRecord> sharedRec = static_cast<FGTrafficRecord*>(rec);
        activeTraffic.push_back(sharedRec);
        SG_LOG(SG_ATC, SG_DEBUG,
               "Added " << rec->getCallsign() << "(" << rec->getId() << ") " << rec << " to " << rec->getRunway());
        if (airportGroundRadar->add(sharedRec)) {
            SG_LOG(SG_ATC, SG_DEBUG,
                   "Added " << (sharedRec)->getCallsign() << "(" << (sharedRec)->getId() << ") " << sharedRec);

        } else {
            SG_LOG(SG_ATC, SG_DEV_WARN,
                   "Not Added " << (sharedRec)->getCallsign() << "(" << (sharedRec)->getId() << ") " << sharedRec);
        }
    } else {
        bool moved = airportGroundRadar->move(SGRect<double>(lat, lon), *i);
        if (!moved) {
            SG_LOG(SG_ATC, SG_DEV_WARN,
                   "Not moved " << (*i)->getCallsign() << "");
        }
        (*i)->setRunway(intendedRoute->getRunway());
        (*i)->setPositionAndIntentions(currentPosition, intendedRoute);
        (*i)->setPositionAndHeading(lat, lon, heading, speed, alt, leg);
    }
}

void FGStartupController::updateAircraftInformation(int id, SGGeod geod, double heading, double speed, double alt,
                                                    double dt)
{
    // Search activeTraffic for a record matching our id
    TrafficVectorIterator i = FGATCController::searchActiveTraffic(id);
    TrafficVectorIterator current;

    if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
        SG_LOG(SG_ATC, SG_DEV_WARN,
               "AI error: updating aircraft without traffic record at " << SG_ORIGIN);
        return;
    } else {
        (*i)->setPositionAndHeading(geod.getLatitudeDeg(), geod.getLongitudeDeg(), heading, speed, alt, AILeg::UNKNOWN);
        current = i;
    }
    setDt(getDt() + dt);

    int state = (*i)->getState();

    // Sentry FLIGHTGEAR-2Q : don't crash on null TrafficRef
    // Sentry FLIGHTGEAR-129: don't crash on null aircraft
    if (!(*i)->getAircraft() || !(*i)->getAircraft()->getTrafficRef()) {
        SG_LOG(SG_ATC, SG_DEV_WARN, "AI traffic: updating aircraft without traffic ref");
        return;
    }

    // The user controlled aircraft should have crashed here, because it doesn't have a traffic reference.
    // NOTE: if we create a traffic schedule for the user aircraft, we can use this to plan a flight.
    time_t startTime = (*i)->getAircraft()->getTrafficRef()->getDepartureTime();
    time_t now = globals->get_time_params()->get_cur_time();


    if (((startTime - now) > 60 && (startTime - now) % 60 == 0) ||
        ((startTime - now) < 60 && (startTime - now) > 0)) {
        SG_LOG(SG_ATC, SG_BULK, (*i)->getAircraft()->getTrafficRef()->getCallSign() << " is scheduled to depart in " << startTime - now << " seconds. Available = " << available << " at parking " << getGateName((*i)->getAircraft()));
    }

    if ((now - lastTransmission) > 3 + (rand() % 15)) {
        available = true;
    }

    //FIXME These messages can become interleaved and shouldn't be
    if (now > (startTime + 0)) {
        checkTransmissionState(ATCMessageState::NORMAL, ATCMessageState::NORMAL, i, now, MSG_ANNOUNCE_ENGINE_START, ATC_AIR_TO_GROUND);
    }
    if (now > (startTime + 60)) {
        checkTransmissionState(ATCMessageState::ACK_HOLD, ATCMessageState::ACK_HOLD, i, now, MSG_REQUEST_ENGINE_START, ATC_AIR_TO_GROUND);
    }
    if (now > (startTime + 80)) {
        checkTransmissionState(ATCMessageState::ACK_RESUME_TAXI, ATCMessageState::ACK_RESUME_TAXI, i, now, MSG_PERMIT_ENGINE_START, ATC_GROUND_TO_AIR);
    }
    if (now > (startTime + 100)) {
        checkTransmissionState(ATCMessageState::TAXI_CLEARED, ATCMessageState::TAXI_CLEARED, i, now, MSG_ACKNOWLEDGE_ENGINE_START, ATC_AIR_TO_GROUND);
    }
    if (now > (startTime + 130)) {
        checkTransmissionState(ATCMessageState::ACK_TAXI_CLEARED, ATCMessageState::ACK_TAXI_CLEARED, i, now, MSG_ACKNOWLEDGE_SWITCH_GROUND_FREQUENCY, ATC_AIR_TO_GROUND);
    }
    if (now > (startTime + 140)) {
        checkTransmissionState(ATCMessageState::START_TAXI, ATCMessageState::START_TAXI, i, now, MSG_INITIATE_CONTACT, ATC_AIR_TO_GROUND);
    }
    if (now > (startTime + 150)) {
        checkTransmissionState(ATCMessageState::REPORT_RUNWAY, ATCMessageState::REPORT_RUNWAY, i, now, MSG_ACKNOWLEDGE_INITIATE_CONTACT, ATC_GROUND_TO_AIR);
    }
    if (now > (startTime + 180)) {
        checkTransmissionState(ATCMessageState::ACK_REPORT_RUNWAY, ATCMessageState::ACK_REPORT_RUNWAY, i, now, MSG_REQUEST_PUSHBACK_CLEARANCE, ATC_AIR_TO_GROUND);
    }
    if ((state == ATCMessageState::SWITCH_GROUND_TOWER) && available) {
        bool pushbackBlocked = airportGroundRadar->isBlockedForPushback(*i);
        if (now > startTime + 200) {
            if ((*i)->pushBackAllowed() && !pushbackBlocked) {
                (*i)->allowRepeatedTransmissions();
                transmit((*i), &(*parent), MSG_PERMIT_PUSHBACK_CLEARANCE,
                         ATC_GROUND_TO_AIR, true);
                (*i)->updateState();
            } else {
                if ((*i)->allowTransmissions()) {
                    transmit((*i), &(*parent), MSG_HOLD_PUSHBACK_CLEARANCE,
                             ATC_GROUND_TO_AIR, true);
                    (*i)->suppressRepeatedTransmissions();
                }
            }
            lastTransmission = now;
            available = false;
        }
    }
    if ((state == ATCMessageState::ACK_SWITCH_GROUND_TOWER) && available) {
        (*i)->setHoldPosition(false);
    }
}

// Note that this function is copied from simgear. for maintenance purposes, it's probably better to make a general function out of that.
static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
                            double lon, double elev, double hdg, double slope)
{
    SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
    obj_pos = makeZUpFrame(geod);
    // hdg is not a compass heading, but a counter-clockwise rotation
    // around the Z axis
    obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
                                        0.0, 0.0, 1.0));
    obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
                                        0.0, 1.0, 0.0));
}


void FGStartupController::render(bool visible)
{
    SGMaterialLib* matlib = globals->get_matlib();
    if (group) {
        //int nr = ;
        globals->get_scenery()->get_scene_graph()->removeChild(group);
        //while (group->getNumChildren()) {
        // SG_LOG(SG_ATC, SG_BULK, "Number of children: " << group->getNumChildren());
        //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
        //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
        //geode->releaseGLObjects();
        //group->removeChild(geode);
        //delete geode;
        group = 0;
    }
    if (visible) {
        SG_LOG(SG_ATC, SG_BULK, "Rendering startup controller");
        group = new osg::Group;
        FGScenery* local_scenery = globals->get_scenery();
        //double elevation_meters = 0.0;
        //double elevation_feet = 0.0;

        FGGroundNetwork* groundNet = parent->parent()->groundNetwork();

        //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
        double dx = 0;
        time_t now = globals->get_time_params()->get_cur_time();

        for (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); ++i) {
            if ((*i)->isActive(300)) {
                // Handle start point
                int pos = (*i)->getCurrentPosition();
                SG_LOG(SG_ATC, SG_BULK, "rendering for " << (*i)->getAircraft()->getCallSign() << "pos = " << pos);
                if (pos > 0) {
                    FGTaxiSegment* segment = groundNet->findSegment(pos);
                    SGGeod start = (*i)->getPos();
                    SGGeod end(segment->getEnd()->geod());

                    double length = SGGeodesy::distanceM(start, end);
                    //heading = SGGeodesy::headingDeg(start->geod(), end->geod());

                    double az2, heading; //, distanceM;
                    SGGeodesy::inverse(start, end, heading, az2, length);
                    double coveredDistance = length * 0.5;
                    SGGeod center;
                    SGGeodesy::direct(start, heading, coveredDistance, center, az2);
                    SG_LOG(SG_ATC, SG_BULK, "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading);
                    ///////////////////////////////////////////////////////////////////////////////
                    // Make a helper function out of this
                    osg::Matrix obj_pos;
                    osg::MatrixTransform* obj_trans = new osg::MatrixTransform;
                    obj_trans->setDataVariance(osg::Object::STATIC);
                    // Experimental: Calculate slope here, based on length, and the individual elevations
                    double elevationStart;
                    if (isUserAircraft((*i)->getAircraft())) {
                        elevationStart = fgGetDouble("/position/ground-elev-m");
                    } else {
                        elevationStart = ((*i)->getAircraft()->_getAltitude() * SG_FEET_TO_METER);
                    }
                    double elevationEnd = segment->getEnd()->getElevationM();
                    if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
                        SGGeod center2 = end;
                        center2.setElevationM(SG_MAX_ELEVATION_M);
                        if (local_scenery->get_elevation_m(center2, elevationEnd, NULL)) {
                            //elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
                            //elevation_meters += 0.5;
                        } else {
                            elevationEnd = parent->getElevation();
                        }
                        segment->getEnd()->setElevation(elevationEnd);
                    }

                    double elevationMean = (elevationStart + elevationEnd) / 2.0;
                    double elevDiff = elevationEnd - elevationStart;

                    double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;

                    SG_LOG(SG_ATC, SG_BULK, "1. Using mean elevation : " << elevationMean << " and " << slope);

                    WorldCoordinate(obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean + 0.5 + dx, -(heading), slope);
                    ;

                    obj_trans->setMatrix(obj_pos);
                    //osg::Vec3 center(0, 0, 0)

                    float width = length / 2.0;
                    osg::Vec3 corner(-width, 0, 0.25f);
                    osg::Vec3 widthVec(2 * width + 1, 0, 0);
                    osg::Vec3 heightVec(0, 1, 0);
                    osg::Geometry* geometry;
                    geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
                    simgear::EffectGeode* geode = new simgear::EffectGeode;
                    geode->setName("test");
                    geode->addDrawable(geometry);
                    //osg::Node *custom_obj;
                    SGMaterial* mat;
                    if (segment->hasBlock(now)) {
                        mat = matlib->find("UnidirectionalTaperRed", center);
                    } else {
                        mat = matlib->find("UnidirectionalTaperGreen", center);
                    }
                    if (mat)
                        geode->setEffect(mat->get_effect());
                    obj_trans->addChild(geode);
                    // wire as much of the scene graph together as we can
                    //->addChild( obj_trans );
                    group->addChild(obj_trans);
                    /////////////////////////////////////////////////////////////////////
                } else {
                    SG_LOG(SG_ATC, SG_DEBUG, "BIG FAT WARNING: current position is here : " << pos);
                }
                for (intVecIterator j = (*i)->getIntentions().begin(); j != (*i)->getIntentions().end(); ++j) {
                    osg::Matrix obj_pos;
                    int k = (*j);
                    if (k > 0) {
                        SG_LOG(SG_ATC, SG_BULK, "rendering for " << (*i)->getAircraft()->getCallSign() << "intention = " << k);
                        osg::MatrixTransform* obj_trans = new osg::MatrixTransform;
                        obj_trans->setDataVariance(osg::Object::STATIC);
                        FGTaxiSegment* segment = groundNet->findSegment(k);

                        double elevationStart = segment->getStart()->getElevationM();
                        double elevationEnd = segment->getEnd()->getElevationM();
                        if ((elevationStart == 0) || (elevationStart == parent->getElevation())) {
                            SGGeod center2 = segment->getStart()->geod();
                            center2.setElevationM(SG_MAX_ELEVATION_M);
                            if (local_scenery->get_elevation_m(center2, elevationStart, NULL)) {
                                //elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
                                //elevation_meters += 0.5;
                            } else {
                                elevationStart = parent->getElevation();
                            }
                            segment->getStart()->setElevation(elevationStart);
                        }
                        if ((elevationEnd == 0) || (elevationEnd == parent->getElevation())) {
                            SGGeod center2 = segment->getEnd()->geod();
                            center2.setElevationM(SG_MAX_ELEVATION_M);
                            if (local_scenery->get_elevation_m(center2, elevationEnd, NULL)) {
                                //elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
                                //elevation_meters += 0.5;
                            } else {
                                elevationEnd = parent->getElevation();
                            }
                            segment->getEnd()->setElevation(elevationEnd);
                        }

                        double elevationMean = (elevationStart + elevationEnd) / 2.0;
                        double elevDiff = elevationEnd - elevationStart;
                        double length = segment->getLength();
                        double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;

                        SG_LOG(SG_ATC, SG_BULK, "2. Using mean elevation : " << elevationMean << " and " << slope);

                        SGGeod segCenter(segment->getCenter());
                        WorldCoordinate(obj_pos, segCenter.getLatitudeDeg(),
                                        segCenter.getLongitudeDeg(), elevationMean + 0.5 + dx, -(segment->getHeading()), slope);

                        //WorldCoordinate( obj_pos, segment->getLatitude(), segment->getLongitude(), parent->getElevation()+8+dx, -(segment->getHeading()) );

                        obj_trans->setMatrix(obj_pos);
                        //osg::Vec3 center(0, 0, 0)

                        float width = segment->getLength() / 2.0;
                        osg::Vec3 corner(-width, 0, 0.25f);
                        osg::Vec3 widthVec(2 * width + 1, 0, 0);
                        osg::Vec3 heightVec(0, 1, 0);
                        osg::Geometry* geometry;
                        geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
                        simgear::EffectGeode* geode = new simgear::EffectGeode;
                        geode->setName("test");
                        geode->addDrawable(geometry);
                        //osg::Node *custom_obj;
                        SGMaterial* mat;
                        if (segment->hasBlock(now)) {
                            mat = matlib->find("UnidirectionalTaperRed", segCenter);
                        } else {
                            mat = matlib->find("UnidirectionalTaperGreen", segCenter);
                        }
                        if (mat)
                            geode->setEffect(mat->get_effect());
                        obj_trans->addChild(geode);
                        // wire as much of the scene graph together as we can
                        //->addChild( obj_trans );
                        group->addChild(obj_trans);
                    } else {
                        SG_LOG(SG_ATC, SG_DEBUG, "BIG FAT WARNING: k is here : " << pos);
                    }
                }
                dx += 0.2;
            }
        }
        globals->get_scenery()->get_scene_graph()->addChild(group);
    }
}

string FGStartupController::getName() const
{
    return string(parent->parent()->getName() + "-Startup");
}

void FGStartupController::update(double dt)
{
    FGATCController::eraseDeadTraffic();
}

int FGStartupController::getFrequency()
{
    int groundFreq = parent->getGroundFrequency(2);
    int towerFreq = parent->getTowerFrequency(2);
    return groundFreq > 0 ? groundFreq : towerFreq;
}
