text:
#include "AirTrafficScreen.hpp"
#include <iterator>
#include <boost/lexical_cast.hpp>
#include <boost/property_tree/json_parser.hpp>
#include "Math.hpp"
#include "GraphUtil.hpp"
#include "Collision.hpp"
#include <sstream>
#include <cctype>

#include <iostream>
using namespace std;

AirTrafficScreen::AirTrafficScreen(sf::RenderWindow &NewApp) : Screen(NewApp), Background(NULL)
{
    LoadResources();

    ScoreText.setFont(Font);
    ScoreText.setCharacterSize(40);
    ScoreText.setPosition(sf::Vector2f(10.f, 10.f));
    ScoreText.setColor(sf::Color::White);

    HighScoreText.setFont(Font);
    HighScoreText.setCharacterSize(24);
    HighScoreText.setPosition(sf::Vector2f(10.f, 50.f));
    HighScoreText.setColor(sf::Color::White);

    ChatText.setFont(Font);
    ChatText.setCharacterSize(14);
    ChatLines.assign(6, sf::String());

    AlarmSound.setLoop(true);
    AlarmSound.setRelativeToListener(true);

    // not needed?
    //Reset();
}

AirTrafficScreen::~AirTrafficScreen()
{
    if (Background)
    {
        delete Background;
    }
}

AirTrafficScreen::ScreenType AirTrafficScreen::Run(const ScreenType &OldScreen)
{
    Pause(false);

    FrameTimer.restart();
    Running = true;
    while (Running)
    {
        HandleEvents();
        if (Net.IsActive())
            HandleNet();
        Step();
        App.clear();
        Draw();
        App.display();
    }

    Pause(true);

    return MenuType;
}

void AirTrafficScreen::StepNet()
{
    if (Net.IsActive())
    {
        HandleNet();
        Step();
    }
}

void AirTrafficScreen::Reset()
{
    UserChatLine.clear();
    ChatLines.assign(6, sf::String());

    Pathing = NULL;
    Score = HighScore = 0;

    Wind = sf::Vector2f(0.f, 1.f);
    Wind = Rotate(Wind, Random(0.f, 360.f));
    Wind = Scale(Wind, Random(3.f, 10.f));

    Spawner = 0.f;
    PlayTime = 0.f;
    WindTime = 0.f;
    NetUpdateTime = 0.f;

    if (Net.IsServer())
    {
        for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
        {
            Net.SendTcp(sf::Packet() << 0 << PacketTypes::AircraftDestroy << it->first);
        }
    }
    Aircrafts.clear();
    AidNext = 0;
    Runways.clear();
    Explosions.clear();
    Sceneries.clear();

    PickSurface();

    for (int n = 0; n < 3; n++)
    {
        SpawnRunway();
    }
    CalculateHull();

    for (int n = 0; n < 5; n++)
    {
        SpawnScenery();
    }

    if (Net.IsServer())
    {
        SendGameData();
    }
}

void AirTrafficScreen::LoadResources()
{
    Sounds.insert(make_pair("", sf::SoundBuffer())); // empty sound for special cases

    Font.loadFromFile("res/Play-Regular.ttf");

    boost::property_tree::json_parser::read_json("data.json", Data);

    boost::property_tree::ptree &DataRunways = Data.get_child("runways");
    boost::property_tree::ptree &DataAircrafts = Data.get_child("aircrafts");
    boost::property_tree::ptree &DataExplosions = Data.get_child("explosions");
    boost::property_tree::ptree &DataSceneries = Data.get_child("sceneries");
    boost::property_tree::ptree &DataSurfaces = Data.get_child("surfaces");

    for (boost::property_tree::ptree::iterator it = DataRunways.begin(); it != DataRunways.end(); ++it)
    {
        boost::property_tree::ptree &Cur = it->second;
        RunwayTemplate Temp;
        Temp.Name = Cur.get<string>("name");
        Temp.Res = Cur.get<string>("res");
        LoadTexture(Temp.Res);
        Temp.Center = sf::Vector2f(Cur.get<float>("centerx"), Cur.get<float>("centery"));
        Temp.Radius = Cur.get<float>("radius");
        Temp.LandAngle = Cur.get<float>("landangle");
        Temp.Length = Cur.get<float>("length");
        Temp.Directional = Cur.get<bool>("directional");

        boost::property_tree::ptree &CurSurfaces = Cur.get_child("surfaces");
        for (boost::property_tree::ptree::iterator it2 = CurSurfaces.begin(); it2 != CurSurfaces.end(); ++it2)
        {
            Temp.Surfaces.push_back(it2->second.get_value<string>());
        }

        RunwayTemplates.insert(make_pair(Temp.Name, Temp));
    }

    for (boost::property_tree::ptree::iterator it = DataAircrafts.begin(); it != DataAircrafts.end(); ++it)
    {
        boost::property_tree::ptree &Cur = it->second;
        AircraftTemplate Temp;
        Temp.Name = Cur.get<string>("name");
        Temp.Res = Cur.get<string>("res");
        LoadTexture(Temp.Res);
        Temp.TakeoffRes = Cur.get("takeoffres", "");
        LoadSound(Temp.TakeoffRes);
        Temp.FlyRes = Cur.get("flyres", "");
        LoadSound(Temp.FlyRes);
        Temp.LandingRes = Cur.get("landingres", "");
        LoadSound(Temp.LandingRes);
        Temp.FrameSize = sf::Vector2i(Cur.get("framew", -1), Cur.get("frameh", -1));
        Temp.FrameRate = Cur.get("framerate", 0.f);
        Temp.Speed = Cur.get<float>("speed");
        Temp.Radius = Cur.get<float>("radius");
        Temp.Turn = Cur.get<float>("turn");
        Temp.Value = Cur.get<int>("value");

        boost::property_tree::ptree &CurRunways = Cur.get_child("runways");
        for (boost::property_tree::ptree::iterator it2 = CurRunways.begin(); it2 != CurRunways.end(); ++it2)
        {
            Temp.Runways.push_back(it2->second.get_value<string>());
        }

        AircraftTemplates.insert(make_pair(Temp.Name, Temp));
    }

    for (boost::property_tree::ptree::iterator it = DataExplosions.begin(); it != DataExplosions.end(); ++it)
    {
        boost::property_tree::ptree &Cur = it->second;
        ExplosionTemplate Temp;
        Temp.Name = Cur.get<string>("name");
        Temp.Res = Cur.get<string>("res");
        LoadTexture(Temp.Res);
        Temp.FrameSize = sf::Vector2i(Cur.get("framew", -1), Cur.get("frameh", -1));
        Temp.FrameRate = Cur.get("framerate", 0.f);
        Temp.SoundRes = Cur.get("soundres", "");
        LoadSound(Temp.SoundRes);
        Temp.Radius = Cur.get<float>("radius");
        Temp.Time = Cur.get("time", 0.f);

        ExplosionTemplates.insert(make_pair(Temp.Name, Temp));
    }

    for (boost::property_tree::ptree::iterator it = DataSceneries.begin(); it != DataSceneries.end(); ++it)
    {
        boost::property_tree::ptree &Cur = it->second;
        SceneryTemplate Temp;
        Temp.Name = Cur.get<string>("name");
        Temp.Res = Cur.get<string>("res");
        LoadTexture(Temp.Res);
        Temp.FrameSize = sf::Vector2i(Cur.get("framew", -1), Cur.get("frameh", -1));
        Temp.FrameRate = Cur.get("framerate", 0.f);
        Temp.Airport = Cur.get<string>("airport") == "true";
        Temp.Shadow = Cur.get("shadow", 0.f);

        boost::property_tree::ptree &CurSurfaces = Cur.get_child("surfaces");
        for (boost::property_tree::ptree::iterator it2 = CurSurfaces.begin(); it2 != CurSurfaces.end(); ++it2)
        {
            Temp.Surfaces.push_back(it2->second.get_value<string>());
        }

        SceneryTemplates.insert(make_pair(Temp.Name, Temp));
    }

    for (boost::property_tree::ptree::iterator it = DataSurfaces.begin(); it != DataSurfaces.end(); ++it)
    {
        boost::property_tree::ptree &Cur = it->second;
        SurfaceTemplate Temp;
        Temp.Name = Cur.get<string>("name");
        Temp.Res = Cur.get<string>("res");
        LoadTexture(Temp.Res);
        Temp.AreaRes = Cur.get("areares", "Concrete.png");
        LoadTexture(Temp.AreaRes);

        SurfaceTemplates.insert(make_pair(Temp.Name, Temp));
    }

    LoadTexture("Pointer.png");
    LoadTexture("Concrete.png");

    LoadSound("alarm.wav");
    AlarmSound.setBuffer(Sounds["alarm.wav"]);
}

bool AirTrafficScreen::SetupClient(const string &Host)
{
    if (!Net.SetupClient(Host))
        return false;

    Reset(); // not fully needed
    return true;
}

bool AirTrafficScreen::SetupServer()
{
    if (!Net.SetupServer())
        return false;

    Reset();
    return true;
}

void AirTrafficScreen::KillNet()
{
    Net.Kill();
}

void AirTrafficScreen::HandleNet()
{
    sf::Packet Packet;
    Networker::ReceiveStatus ReceiveStatus;

    do
    {
        switch (ReceiveStatus = Net.Receive(Packet))
        {
            case Networker::Connected:
            {
                sf::Uint32 Id;
                Packet >> Id;
                ostringstream ss;
                ss << "[ " << Id << " connected ]";
                AddChatLine(ss.str());
                if (Net.IsServer())
                {
                    Net.SendTcp(sf::Packet() << Id << PacketTypes::ClientConnect);
                    SendGameData(Id);
                }
                break;
            }
            case Networker::Disconnected:
            {
                sf::Uint32 Id;
                Packet >> Id;
                ostringstream ss;
                ss << "[ " << Id;
                if (Id == 0)
                    ss << "(server)";
                ss << " disconnected ]";
                AddChatLine(ss.str());

                if (Net.IsServer())
                {
                    Net.SendTcp(sf::Packet() << Id << PacketTypes::ClientDisconnect);
                    Pointers.erase(Id);
                }
                else
                {
                    Pointers.clear();
                    KillNet();
                }
                break;
            }
            case Networker::NewPacket:
            {
                sf::Uint32 SourceId;
                PacketType Type;
                Packet >> SourceId >> Type;
                //cerr << "Packet (Id " << SourceId << " Type " << Type << ")" << endl;
                switch (Type)
                {
                    case PacketTypes::ConnectionResponse:
                    {
                        if (Net.IsServer())
                            break;

                        sf::Uint32 Id;
                        Packet >> Id;
                        ostringstream ss;
                        ss << "[ connected as " << Id << " ]";
                        AddChatLine(ss.str());
                        break;
                    }
                    case PacketTypes::SurfaceUpdate:
                    {
                        if (Net.IsServer())
                            break;
                        string Name;
                        Packet >> Name;

                        map<string, SurfaceTemplate>::iterator it = SurfaceTemplates.find(Name);

                        if (it != SurfaceTemplates.end())
                        {
                            if (Background)
                                delete Background;

                            Background = new Surface(it->second, Textures);
                        }
                        break;
                    }
                    case PacketTypes::SceneryUpdate:
                    {
                        if (Net.IsServer())
                            break;
                        Sceneries.clear();
                        while (!Packet.endOfPacket())
                        {
                            string Name;
                            float X, Y, Angle;
                            Packet >> Name >> X >> Y >> Angle;

                            map<string, SceneryTemplate>::iterator it = SceneryTemplates.find(Name);

                            if (it != SceneryTemplates.end())
                                Sceneries.push_back(new Scenery(it->second, Textures, sf::Vector2f(X, Y), Angle));
                        }
                        break;
                    }
                    case PacketTypes::RunwayUpdate:
                    {
                        if (Net.IsServer())
                            break;
                        Runways.clear();
                        while (!Packet.endOfPacket())
                        {
                            string Name;
                            float X, Y, Angle;
                            Packet >> Name >> X >> Y >> Angle;

                            map<string, RunwayTemplate>::iterator it = RunwayTemplates.find(Name);

                            if (it != RunwayTemplates.end())
                                Runways.push_back(new Runway(it->second, Textures, sf::Vector2f(X, Y), Angle));
                        }
                        CalculateHull();
                        break;
                    }
                    case PacketTypes::ScoreUpdate:
                    {
                        if (Net.IsServer())
                            break;
                        Packet >> Score >> HighScore;
                        break;
                    }
                    case PacketTypes::WindUpdate:
                    {
                        if (Net.IsServer())
                            break;
                        Packet >> Wind.x >> Wind.y;
                        break;
                    }
                    case PacketTypes::PointerUpdate:
                    {
                        sf::Vector2i PointerPos;
                        Packet >> PointerPos.x >> PointerPos.y;
                        Pointers[SourceId] = PointerPos;

                        if (Net.IsServer())
                        {
                            Net.SendUdp(sf::Packet() << SourceId << PacketTypes::PointerUpdate << PointerPos.x << PointerPos.y);
                        }
                        break;
                    }
                    case PacketTypes::AircraftSpawn:
                    case PacketTypes::AircraftCreateOut:
                    case PacketTypes::AircraftCreateIn:
                    {
                        if (Net.IsServer())
                            break;

                        sf::Uint32 Aid;
                        string Name;
                        sf::Vector2f Pos;
                        float Angle;
                        sf::Int32 Rw;
                        Packet >> Aid >> Name >> Pos.x >> Pos.y >> Angle >> Rw;

                        map<string, AircraftTemplate>::iterator it = AircraftTemplates.find(Name);

                        if (it != AircraftTemplates.end())
                        {
                            Runway *Land = 0;
                            if (Rw >= 0)
                            {
                                boost::ptr_list<Runway>::iterator it2 = Runways.begin();
                                advance(it2, Rw);
                                Land = &*it2;
                            }

                            Aircraft *Ac;

                            switch (Type)
                            {
                                case PacketTypes::AircraftSpawn:
                                {
                                    if (Packet.endOfPacket()) // in
                                    {
                                        Ac = new Aircraft(it->second, Textures, Sounds, Pos, Angle);
                                    }
                                    else // out
                                    {
                                        sf::Uint16 OutDirection;
                                        Packet >> OutDirection;
                                        Ac = new Aircraft(it->second, Textures, Sounds, Pos, Angle, Land, static_cast<Aircraft::OutDirections>(OutDirection));
                                    }

                                    break;
                                }
                                case PacketTypes::AircraftCreateOut:
                                {
                                    sf::Uint16 State;
                                    sf::Uint16 OutDirection;
                                    Packet >> State >> OutDirection;
                                    Ac = new Aircraft(it->second, Textures, Sounds, Pos, Angle, Land, static_cast<Aircraft::States>(State), static_cast<Aircraft::OutDirections>(OutDirection));
                                    break;
                                }
                                case PacketTypes::AircraftCreateIn:
                                {
                                    sf::Uint16 State;
                                    sf::Vector2f LandPoint;
                                    Packet >> State >> LandPoint.x >> LandPoint.y;
                                    Ac = new Aircraft(it->second, Textures, Sounds, Pos, Angle, Land, static_cast<Aircraft::States>(State), LandPoint);
                                    break;
                                }
                                default:
                                    break;
                            }

                            Aircrafts.insert(Aid, Ac);
                        }
                        break;
                    }
                    case PacketTypes::AircraftUpdate:
                    {
                        if (Net.IsServer())
                            break;

                        sf::Uint32 Aid;
                        sf::Vector2f Pos;
                        float Angle;
                        sf::Int32 Rw;
                        Packet >> Aid >> Pos.x >> Pos.y >> Angle >> Rw;

                        Runway *Land = 0;
                        if (Rw >= 0)
                        {
                            boost::ptr_list<Runway>::iterator it2 = Runways.begin();
                            advance(it2, Rw);
                            Land = &*it2;
                        }

                        boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.find(Aid);
                        if (it != Aircrafts.end())
                        {
                            Aircraft *Ac = it->second;
                            Ac->SetPos(Pos);
                            Ac->SetAngle(Angle);
                            Ac->SetRunway(Land);
                        }
                        break;
                    }
                    case PacketTypes::AircraftDestroy:
                    {
                        if (Net.IsServer())
                            break;

                        sf::Uint32 Aid;
                        Packet >> Aid;

                        boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.find(Aid);
                        if (it != Aircrafts.end())
                        {
                            if (Pathing == it->second)
                            {
                                Pathing = NULL;
                            }

                            Aircrafts.erase(it);
                        }
                        break;
                    }
                    case PacketTypes::ExplosionUpdate:
                    {
                        if (Net.IsServer())
                            break;

                        while (!Packet.endOfPacket())
                        {
                            string Name;
                            sf::Vector2f Pos;
                            float Angle, Time;
                            Packet >> Name >> Pos.x >> Pos.y >> Angle >> Time;

                            map<string, ExplosionTemplate>::iterator it = ExplosionTemplates.find(Name);

                            if (it != ExplosionTemplates.end())
                                Explosions.push_back(new Explosion(it->second, Textures, Sounds, Pos, Angle, Time));
                        }
                        break;
                    }
                    case PacketTypes::PathUpdate:
                    {
                        sf::Uint32 Aid;
                        sf::Vector2f Pos;
                        Packet >> Aid >> Pos.x >> Pos.y;

                        boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.find(Aid);
                        if (it != Aircrafts.end() && it->second->Pathable())
                        {
                            it->second->GetPath().TryAddPoint(Pos, 0.f);
                            PathingFinish(it->second);
                        }

                        if (Net.IsServer())
                        {
                            Net.SendTcp(sf::Packet() << SourceId << PacketTypes::PathUpdate << Aid << Pos.x << Pos.y);
                        }
                        break;
                    }
                    case PacketTypes::PathClear:
                    {
                        sf::Uint32 Aid;
                        Packet >> Aid;

                        boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.find(Aid);
                        if (it != Aircrafts.end() && it->second->Pathable())
                        {
                            it->second->SetRunway(NULL);
                            it->second->GetPath().Clear();
                        }

                        if (Net.IsServer())
                        {
                            Net.SendTcp(sf::Packet() << SourceId << PacketTypes::PathClear << Aid);
                        }
                        break;
                    }
                    case PacketTypes::ChatMessage:
                    {
                        sf::String ChatLine;
                        Packet >> ChatLine;

                        ostringstream ss;
                        ss << SourceId;
                        if (SourceId == 0)
                            ss << "(server)";
                        ss << ": ";
                        AddChatLine(ss.str() + ChatLine);

                        if (Net.IsServer())
                        {
                            Net.SendTcp(sf::Packet() << SourceId << PacketTypes::ChatMessage << ChatLine);
                        }
                        break;
                    }
                    case PacketTypes::ClientConnect:
                    {
                        ostringstream ss;
                        ss << "[ " << SourceId << " connected ]";
                        AddChatLine(ss.str());
                        break;
                    }
                    case PacketTypes::ClientDisconnect:
                    {
                        ostringstream ss;
                        ss << "[ " << SourceId << " disconnected ]";
                        AddChatLine(ss.str());
                        Pointers.erase(SourceId);
                        break;
                    }
                }
                break;
            }
            case Networker::NoPacket:
            {
                break;
            }
        }
    }
    while (ReceiveStatus != Networker::NoPacket);

    if (Net.IsServer() && NetUpdateTime > 0.1f)
    {
        for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
        {
            sf::Packet Packet;
            Packet << 0 << PacketTypes::AircraftUpdate << it->first << it->second->GetPos().x << it->second->GetPos().y << it->second->GetAngle();
            if (it->second->GetLand() == NULL)
                Packet << -1;
            else
            {
                boost::ptr_list<Runway>::iterator it2 = Runways.begin();
                for (; it2 != Runways.end(); ++it2)
                {
                    if (&*it2 == it->second->GetLand())
                        break;
                }
                sf::Int32 Rw = distance(Runways.begin(), it2);
                Packet << Rw;
            }

            Net.SendUdp(Packet);
        }
        NetUpdateTime = 0.f;
    }
}

void AirTrafficScreen::SendGameData(const sf::Uint32 Id)
{
    Net.SendTcp(sf::Packet() << 0 << PacketTypes::ScoreUpdate << Score << HighScore, Id); // score
    Net.SendTcp(sf::Packet() << 0 << PacketTypes::WindUpdate << Wind.x << Wind.y, Id); // wind
    Net.SendTcp(sf::Packet() << 0 << PacketTypes::SurfaceUpdate << Background->GetTemplate().Name, Id); // surface

    { // Runways
        sf::Packet Packet;
        Packet << 0 << PacketTypes::RunwayUpdate;
        for (boost::ptr_list<Runway>::iterator it = Runways.begin(); it != Runways.end(); ++it)
            Packet << it->GetTemplate().Name << it->GetPos().x << it->GetPos().y << it->GetAngle();
        Net.SendTcp(Packet, Id);
    }

    { // Scenery
        sf::Packet Packet;
        Packet << 0 << PacketTypes::SceneryUpdate;
        for (boost::ptr_list<Scenery>::iterator it = Sceneries.begin(); it != Sceneries.end(); ++it)
            Packet << it->GetTemplate().Name << it->GetPos().x << it->GetPos().y << it->GetAngle();
        Net.SendTcp(Packet, Id);
    }

    // Aircrafts
    for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
    {
        sf::Packet Packet;
        Packet << 0 << (it->second->GetDirection() == Aircraft::In ? PacketTypes::AircraftCreateIn : PacketTypes::AircraftCreateOut);
        Packet << it->first << it->second->GetTemplate().Name << it->second->GetPos().x << it->second->GetPos().y << it->second->GetAngle();
        if (it->second->GetLand() == NULL)
            Packet << -1;
        else
        {
            boost::ptr_list<Runway>::iterator it2 = Runways.begin();
            for (; it2 != Runways.end(); ++it2)
            {
                if (&*it2 == it->second->GetLand())
                    break;
            }
            sf::Int32 Rw = distance(Runways.begin(), it2);
            Packet << Rw;
        }
        Packet << static_cast<sf::Uint16>(it->second->GetState());

        if (it->second->GetDirection() == Aircraft::In)
        {
            const sf::Vector2f &LandPoint = it->second->GetLandPoint();
            Packet << LandPoint.x << LandPoint.y;
        }
        else
        {
            Packet << static_cast<sf::Uint16>(it->second->GetOutDirection());
        }

        Net.SendTcp(Packet, Id);
    }

    // Explosions
    {
        sf::Packet Packet;
        Packet << 0 << PacketTypes::ExplosionUpdate;

        for (boost::ptr_list<Explosion>::iterator it = Explosions.begin(); it != Explosions.end(); ++it)
        {
            const sf::Vector2f &Pos = it->GetPos();
            Packet << it->GetTemplate().Name << Pos.x << Pos.y << it->GetAngle() << it->GetTime();
        }
        Net.SendTcp(Packet, Id);
    }
}

void AirTrafficScreen::HandleEvents()
{
    const sf::Vector2f MousePos(sf::Mouse::getPosition(App).x, sf::Mouse::getPosition(App).y);

    sf::Event Event;
    while (App.pollEvent(Event))
    {
        if (Event.type == sf::Event::Closed ||
            (Event.type == sf::Event::KeyPressed && Event.key.code == sf::Keyboard::Escape) /*||
            Event.Type == sf::Event::LostFocus*/)
        {
            Running = false;
        }

        if (Event.type == sf::Event::MouseButtonPressed &&
            Event.mouseButton.button == sf::Mouse::Left)
        {
            for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
            {
                Aircraft &Ac = *it->second;
                if (Ac.Pathable() && Ac.OnMe(MousePos))
                {
                    Pathing = &Ac;
                    PathingAid = it->first;
                    Pathing->SetRunway(NULL);
                    Pathing->GetPath().Clear();
                    Pathing->GetPath().TryAddPoint(MousePos);

                    if (Net.IsActive())
                    {
                        Net.SendTcp(sf::Packet() << Net.GetId() << PacketTypes::PathClear << PathingAid);
                        Net.SendTcp(sf::Packet() << Net.GetId() << PacketTypes::PathUpdate << PathingAid << MousePos.x << MousePos.y);
                    }

                    break;
                }
            }
        }
        else if (Event.type == sf::Event::MouseButtonReleased &&
                 Event.mouseButton.button == sf::Mouse::Left &&
                 Pathing != NULL)
        {
            Path &P = Pathing->GetPath();
            if (P.TryAddPoint(MousePos, 10.f, true))
            {
                if (Net.IsActive())
                {
                    Net.SendTcp(sf::Packet() << Net.GetId() << PacketTypes::PathUpdate << PathingAid << MousePos.x << MousePos.y);
                }
                PathingFinish(Pathing);
            }

            Pathing = NULL;
        }
        else if (Event.type == sf::Event::MouseMoved)
        {
            if (Net.IsActive())
            {
                Net.SendUdp(sf::Packet() << Net.GetId() << PacketTypes::PointerUpdate << Event.mouseMove.x << Event.mouseMove.y);
            }

            if (Pathing != NULL)
            {
                Path &P = Pathing->GetPath();
                if (P.TryAddPoint(MousePos))
                {
                    if (Net.IsActive())
                    {
                        Net.SendTcp(sf::Packet() << Net.GetId() << PacketTypes::PathUpdate << PathingAid << MousePos.x << MousePos.y);
                    }
                    PathingFinish(Pathing);
                }
            }

        }
        else if (Net.IsActive())
        {
            if (Event.type == sf::Event::KeyPressed)
            {
                switch (Event.key.code)
                {
                    case sf::Keyboard::Return:
                    {
                        ostringstream ss;
                        ss << Net.GetId() << "(me): ";
                        AddChatLine(ss.str() + UserChatLine);
                        if (Net.IsActive())
                        {
                            Net.SendTcp(sf::Packet() << Net.GetId() << PacketTypes::ChatMessage << UserChatLine);
                        }
                        UserChatLine.clear();
                        break;
                    }

                    case sf::Keyboard::Escape:
                        UserChatLine.clear();
                        break;

                    case sf::Keyboard::BackSpace:
                        if (UserChatLine.getSize() > 0)
                            UserChatLine.erase(UserChatLine.getSize() - 1);
                        break;

                    default:
                        break;
                }
            }
            else if (Event.type == sf::Event::TextEntered && iswprint(Event.text.unicode))
            {
                UserChatLine += Event.text.unicode;
            }
        }
    }

	if (Pathing != NULL)
	{
		if (!Pathing->Pathable() || (Pathing->GetPath().NumPoints() == 0 && !InRange(Pathing->GetPos(), MousePos, 25)))
			Pathing = NULL;
	}
}

void AirTrafficScreen::Step()
{
    const float FT = FrameTimer.restart().asSeconds();
    Spawner += FT;
    PlayTime += FT;
    WindTime += FT;
    NetUpdateTime += FT;

    sf::Vector2i MousePos = sf::Mouse::getPosition(App);
    sf::Listener::setPosition(MousePos.x, MousePos.y, 100.f);
    //sf::Listener::SetDirection(0.f, 0.f, -100.f);

    float SpawnTime = 5 * pow(0.99995, Score);

    if ((!Net.IsActive() || Net.IsServer()) && Spawner > SpawnTime)
    {
        SpawnAircraft();
        Spawner = 0.f;
    }

    if ((!Net.IsActive() || Net.IsServer()) && WindTime > 0.5f) // may need changing
    {
        Wind = Rotate(Wind, Random(-5.f, 5.f));
        Wind *= Random(0.95f, 1.05f);
        WindTime = 0.f;

        Net.SendTcp(sf::Packet() << 0 << PacketTypes::WindUpdate << Wind.x << Wind.y);
    }

    for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); )
    {
        Aircraft &Ac = *it->second;

        if (!Net.IsActive() || Net.IsServer())
        {
            boost::ptr_map<sf::Uint32, Aircraft>::iterator it2 = it;
            for (++it2; it2 != Aircrafts.end(); ++it2)
            {
                if (Ac.Colliding(*it2->second))
                {
                    break;
                }
            }

            boost::ptr_list<Explosion>::iterator it3 = Explosions.begin();
            for (; it3 != Explosions.end(); ++it3)
            {
                if (Ac.Colliding(*it3))
                {
                    break;
                }
            }

            bool ScoreUpdate = false;

            if (it2 != Aircrafts.end())
            {
                sf::Vector2f Pos1 = Ac.GetPos(), Pos2 = it2->second->GetPos();
                SpawnExplosion(Pos1);
                SpawnExplosion(Pos2);
                Score -= Ac.GetTemplate().Value + it2->second->GetTemplate().Value;
                ScoreUpdate = true;

                if (Pathing == it->second || Pathing == it2->second) // take care if were drawing a path
                {
                    Pathing = NULL;
                }

                if (Net.IsServer())
                {
                    Net.SendTcp(sf::Packet() << 0 << PacketTypes::AircraftDestroy << it->first);
                    Net.SendTcp(sf::Packet() << 0 << PacketTypes::AircraftDestroy << it2->first);
                }

                Aircrafts.erase(it2);
                it = Aircrafts.erase(it);
            }
            else if (it3 != Explosions.end() && it3->Deadly())
            {
                sf::Vector2f Pos1 = Ac.GetPos()/*, Pos2 = it3->GetPos()*/;
                SpawnExplosion(Pos1);
                Score -= Ac.GetTemplate().Value;
                ScoreUpdate = true;

                if (Pathing == it->second)
                {
                    Pathing = NULL;
                }

                if (Net.IsServer())
                {
                    Net.SendTcp(sf::Packet() << 0 << PacketTypes::AircraftDestroy << it->first);
                }

                it = Aircrafts.erase(it);
            }
            else if (Ac.Step(FT, Wind))
            {
                Score += 2*Ac.GetTemplate().Value / sqrt(Ac.GetTime());
                ScoreUpdate = true;

                if (Pathing == it->second) // take care if were drawing a path
                {
                    Pathing = NULL;
                }

                if (Net.IsServer())
                {
                    Net.SendTcp(sf::Packet() << 0 << PacketTypes::AircraftDestroy << it->first);
                }

                it = Aircrafts.erase(it);
            }
            else
            {
                ++it;
            }

            if (ScoreUpdate && (Score > HighScore))
                HighScore = Score;

            if (Net.IsServer() && ScoreUpdate)
            {
                Net.SendTcp(sf::Packet() << 0 << PacketTypes::ScoreUpdate << Score << HighScore);
            }
        }
        else
        {
            Ac.Step(FT, Wind);
            ++it;
        }
    }

    for (boost::ptr_list<Explosion>::iterator it = Explosions.begin(); it != Explosions.end();)
    {
        if (it->Step(FT))
        {
            it = Explosions.erase(it);
        }
        else
        {
            ++it;
        }
    }

    for (boost::ptr_list<Scenery>::iterator it = Sceneries.begin(); it != Sceneries.end(); ++it)
    {
        it->Step(FT);
    }

    ScoreText.setString(boost::lexical_cast<string>(Score));
    HighScoreText.setString(boost::lexical_cast<string>(HighScore));
}

void AirTrafficScreen::Draw()
{
    // surface
    Background->Draw(App);
    App.draw(AirportArea);

    // runways
    for (boost::ptr_list<Runway>::iterator it = Runways.begin(); it != Runways.end(); ++it)
    {
        it->Draw(App);
    }

    // scenery shadows
    for (boost::ptr_list<Scenery>::iterator it = Sceneries.begin(); it != Sceneries.end(); ++it)
    {
        it->DrawShadow(App);
    }

    // sceneries
    for (boost::ptr_list<Scenery>::iterator it = Sceneries.begin(); it != Sceneries.end(); ++it)
    {
        it->Draw(App);
    }

    // aircraft shadows
    for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
    {
        it->second->DrawShadow(App);
    }

    // landing areas
    for (boost::ptr_list<Runway>::iterator it = Runways.begin(); it != Runways.end(); ++it)
    {
        if (Pathing != NULL)
        {
            if (Pathing->GetDirection() == Aircraft::In)
            {
                vector<string> Landable = Pathing->GetTemplate().Runways;
                if (find(Landable.begin(), Landable.end(), it->GetTemplate().Name) != Landable.end())
                {
                    App.draw(Circle(it->GetPos(),
                                    it->GetTemplate().Radius - 3.f,
                                    sf::Color(0, 255, 255, 96),
                                    3.f,
                                    sf::Color(0, 255, 255)));
                }
            }
            else
            {
                switch (Pathing->GetOutDirection())
                {
                    case Aircraft::OutUp:
                        App.draw(Rectangle(0.f, 0.f, 800.f, 50.f, sf::Color(0, 255, 255, 96), 3.f, sf::Color(0, 255, 255)));
                        break;
                    case Aircraft::OutDown:
                        App.draw(Rectangle(0.f, 550.f, 800.f, 50.f, sf::Color(0, 255, 255, 96), 3.f, sf::Color(0, 255, 255)));
                        break;
                    case Aircraft::OutLeft:
                        App.draw(Rectangle(0.f, 0.f, 50.f, 600.f, sf::Color(0, 255, 255, 96), 3.f, sf::Color(0, 255, 255)));
                        break;
                    case Aircraft::OutRight:
                        App.draw(Rectangle(750.f, 0.f, 50.f, 600.f, sf::Color(0, 255, 255, 96), 3.f, sf::Color(0, 255, 255)));
                        break;
                }
            }
        }
    }

    // aircraft paths
    for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
    {
        it->second->GetPath().Draw(App);
    }
    // aircrafts
    for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
    {
        it->second->Draw(App);
    }
    // aircraft collision warnings
    bool AlarmOn = false;
    for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
    {
        const Aircraft &Ac = *it->second;

        // with explosion
        for (boost::ptr_list<Explosion>::iterator it2 = Explosions.begin(); it2 != Explosions.end(); ++it2)
        {
            sf::Vector2f Pos1 = Ac.GetPos(), Pos2 = it2->GetPos();
            float R1 = Ac.GetRadius(), R2 = it2->GetRadius();
            float MaxDist = (R1 + R2) / 1.5f;
            float Dist = Distance(Pos1, Pos2);

            if (it2->Deadly() && Dist < MaxDist)
            {
                AlarmOn = true;
                float MinDist = (R1 + R2) / 2.5f;
                App.draw(Circle(Pos1,
                                R1 - 3.f,
                                sf::Color(255, 255, 0, Map<float>(Dist, MinDist, MaxDist, 128, 32)),
                                3.f,
                                sf::Color(255, 255, 0, Map<float>(Dist, MinDist, MaxDist, 255, 64))));
            }
        }
        // with other aircraft
        boost::ptr_map<sf::Uint32, Aircraft>::iterator it2 = it;
        for (++it2; it2 != Aircrafts.end(); ++it2)
        {
            const Aircraft &Ac2 = *it2->second;

            sf::Vector2f Pos1 = Ac.GetPos(), Pos2 = Ac2.GetPos();
            float R1 = Ac.GetRadius(), R2 = Ac2.GetRadius();
            float MaxDist = 1.75 * (R1 + R2);
            float Dist = Distance(Pos1, Pos2);

            if (Ac.OnRunway() == Ac2.OnRunway() && Dist < MaxDist)
            {
                AlarmOn = true;
                float MinDist = (R1 + R2) / 1.3f;
                App.draw(Circle(Pos1,
                                R1 - 3.f,
                                sf::Color(255, 0, 0, Map<float>(Dist, MinDist, MaxDist, 128, 32)),
                                3.f,
                                sf::Color(255, 0, 0, Map<float>(Dist, MinDist, MaxDist, 255, 64))));
                App.draw(Circle(Pos2,
                                R2 - 3.f,
                                sf::Color(255, 0, 0, Map<float>(Dist, MinDist, MaxDist, 128, 32)),
                                3.f,
                                sf::Color(255, 0, 0, Map<float>(Dist, MinDist, MaxDist, 255, 64))));
            }
        }
    }
    if (AlarmSound.getStatus() == sf::Sound::Stopped && AlarmOn)
    {
        AlarmSound.play();
    }
    else if (AlarmSound.getStatus() == sf::Sound::Playing && !AlarmOn)
    {
        AlarmSound.stop();
    }

    // explosions
    for (boost::ptr_list<Explosion>::iterator it = Explosions.begin(); it != Explosions.end(); ++it)
    {
        it->Draw(App);
    }

    // hud
    if (HighScore == 0)
        ScoreText.setColor(Score < 0 ? sf::Color(255, 128, 128) : sf::Color::White);
    else
    {
        float RecordP = max(float(Score) / HighScore, 0.f);
        if (RecordP >= 0.5f)
        {
            int val = Map(RecordP, 0.5f, 1.f, 255, 128);
            ScoreText.setColor(sf::Color(val, 255, val));
        }
        else
        {
            int val = Map(RecordP, 0.f, 0.5f, 128, 255);
            ScoreText.setColor(sf::Color(255, val, val));
        }
    }
    App.draw(ScoreText);
    App.draw(HighScoreText);

    // chat
    ChatText.setStyle(sf::Text::Regular);
    for (unsigned int i = 0; i < ChatLines.size(); i++)
    {
        ChatText.setPosition(20, 480 + 17 * i);
        ChatText.setColor(sf::Color(255, 255, 255, Map(static_cast<int>(i), 0, static_cast<int>(ChatLines.size() - 1), 128, 255)));
        ChatText.setString(ChatLines[i]);
        App.draw(ChatText);
    }

    if (Net.IsActive())
    {
        ChatText.setPosition(20, 480 + 17 * ChatLines.size());
        ChatText.setColor(sf::Color::White);
        ChatText.setStyle(sf::Text::Bold);
        ChatText.setString(UserChatLine + "_");
        App.draw(ChatText);
    }

    // wind
    App.draw(Line(sf::Vector2f(750.f, 50.f), sf::Vector2f(750.f, 50.f) + Wind * 4.f, 3.f, sf::Color::White));
    App.draw(Circle(sf::Vector2f(750.f, 50.f), 5.f, sf::Color::Red));

    // pointers
    for (map<sf::Uint32, sf::Vector2i>::iterator it = Pointers.begin(); it != Pointers.end(); ++it)
    {
        sf::Sprite Pointer(Textures["Pointer.png"]);
        Pointer.setPosition(sf::Vector2f(it->second.x, it->second.y));
        App.draw(Pointer);
    }
}

void AirTrafficScreen::Pause(bool Status)
{
    for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
    {
        it->second->Pause(Status);
    }

    for (boost::ptr_list<Explosion>::iterator it = Explosions.begin(); it != Explosions.end(); ++it)
    {
        it->Pause(Status);
    }

    if (Status)
    {
        AlarmSound.pause();
    }
    else
    {
        if (AlarmSound.getStatus() == sf::Sound::Paused)
        {
            AlarmSound.play();
        }
    }
}

void AirTrafficScreen::PathingFinish(Aircraft *Ac)
{
	if (!Ac->Pathable())
		return;

    Path &P = Ac->GetPath();

    Ac->GetPath().Highlight = false;
    if (Ac->GetDirection() == Aircraft::In)
    {
        Runway *Land = 0;
        for (boost::ptr_list<Runway>::iterator it = Runways.begin(); it != Runways.end(); ++it)
        {
            vector<string> Landable = Ac->GetTemplate().Runways;
            if (find(Landable.begin(), Landable.end(), it->GetTemplate().Name) != Landable.end() &&
                it->OnMe(P[P.NumPoints() - 1]) &&
                abs(AngleDiff(P.NumPoints() < 2 ? Ac->GetAngle() : P.EndAngle(), it->GetAngle())) <= it->GetTemplate().LandAngle)
            {
                Land = &*it;
                Ac->GetPath().Highlight = true;
                break;
            }
        }
        Ac->SetRunway(Land);
    }
    else
    {
        sf::Vector2f Point = P[P.NumPoints() - 1];
        Aircraft::OutDirections OutDirection = Ac->GetOutDirection();

        if ((OutDirection == Aircraft::OutUp && Point.y < 50) ||
            (OutDirection == Aircraft::OutDown && Point.y > 550) ||
            (OutDirection == Aircraft::OutLeft && Point.x < 50) ||
            (OutDirection == Aircraft::OutRight && Point.x > 750))
        {
            Ac->GetPath().Highlight = true;
        }
    }
}

void AirTrafficScreen::AddChatLine(const sf::String &Line)
{
    rotate(ChatLines.begin(), ChatLines.begin() + 1, ChatLines.end());

    ChatLines.back() = Line;
}

void AirTrafficScreen::LoadTexture(const string &FileName)
{
    sf::Texture Texture;
    Texture.loadFromFile("res/" + FileName);
    Textures.insert(make_pair(FileName, Texture));
}

void AirTrafficScreen::LoadSound(const string &FileName)
{
    if (!FileName.empty())
    {
        sf::SoundBuffer Sound;
        Sound.loadFromFile("res/" + FileName);
        Sounds.insert(make_pair(FileName, Sound));
    }
}

void AirTrafficScreen::SpawnRunway()
{
    map<string, RunwayTemplate>::iterator it;
    bool CanSpawn = false;
    do
    {
        it = RunwayTemplates.begin();
        advance(it, rand() % RunwayTemplates.size());

        if (find(it->second.Surfaces.begin(), it->second.Surfaces.end(), Background->GetTemplate().Name) != it->second.Surfaces.end())
        {
            CanSpawn = true;
            break;
        }
    }
    while (!CanSpawn);

    const RunwayTemplate &Temp = it->second;

    sf::Vector2f Pos;
    float Angle;

    Runway *New;
    bool Ready;
    do
    {
        Ready = true;

        Pos.x = Random(200.f, 600.f);
        Pos.y = Random(200.f, 400.f);

        Angle = Random(0.f, 360.f);

        New = new Runway(Temp, Textures, Pos, Angle);
        for (boost::ptr_list<Runway>::iterator it = Runways.begin(); it != Runways.end(); ++it)
        {
            if (New->Colliding(*it))
            {
                Ready = false;
                break;
            }
        }
        if (!Ready)
        {
            delete New;
        }
    }
    while (!Ready);

    Runways.push_back(New);
}

void AirTrafficScreen::SpawnAircraft()
{
    map<string, AircraftTemplate>::iterator it;
    bool CanLand = false;
    do
    {
        it = AircraftTemplates.begin();
        advance(it, rand() % AircraftTemplates.size());

        for (boost::ptr_list<Runway>::iterator it2 = Runways.begin(); it2 != Runways.end(); ++it2)
        {
            vector<string> &Landable = it->second.Runways;
            if (find(Landable.begin(), Landable.end(), it2->GetTemplate().Name) != Landable.end())
            {
                CanLand = true;
                break;
            }
        }
    }
    while (!CanLand);

    const AircraftTemplate &Temp = it->second;

    sf::Vector2f Pos;
    float Angle;

    bool Ready = false;
    Aircraft* New;

    if (Chance(0.4f))
    {
        //takeoff
        vector<Runway*> Rws;

        for (boost::ptr_list<Runway>::iterator it2 = Runways.begin(); it2 != Runways.end(); ++it2)
        {
            const vector<string> &Landable = Temp.Runways;
            if (find(Landable.begin(), Landable.end(), it2->GetTemplate().Name) != Landable.end())
            {
                Rws.push_back(&*it2);
            }
        }

        random_shuffle(Rws.begin(), Rws.end(), Shuffler);

        for (vector<Runway*>::iterator it2 = Rws.begin(); it2 != Rws.end(); ++it2)
        {
            Ready = true;
            Angle = (*it2)->GetAngle();
            Pos = (*it2)->GetPos();
            Aircraft::OutDirections OutDirection = static_cast<Aircraft::OutDirections>(Random(Aircraft::OutUp, Aircraft::OutRight)); //right order, shitty random
            New = new Aircraft(Temp, Textures, Sounds, Pos, Angle, *it2, OutDirection);

            for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it3 = Aircrafts.begin(); it3 != Aircrafts.end(); ++it3)
            {
                const Aircraft &Ac = *it3->second;

                if (Ac.Colliding(*New) || Ac.GetLand() == *it2)
                {
                    Ready = false;
                    break;
                }
            }

            if (Ready)
            {
                break;
            }
            else
            {
                delete New;
            }
        }
    }
    else
    {
        do
        {
            Ready = true;

            Pos.x = Random(-25.f, 825.f);
            Pos.y = Random(-25.f, 625.f);
            Angle = 0.f;

            if (Chance(0.5f))
            {
                if (Chance(0.5f))
                {
                    Pos.y = -25.f;
                }
                else
                {
                    Pos.y = 625.f;
                }
            }
            else
            {
                if (Chance(0.5f))
                {
                    Pos.x = -25.f;
                }
                else
                {
                    Pos.x = 825.f;
                }
            }

            New = new Aircraft(Temp, Textures, Sounds, Pos, Angle);
            for (boost::ptr_map<sf::Uint32, Aircraft>::iterator it = Aircrafts.begin(); it != Aircrafts.end(); ++it)
            {
                if (it->second->Colliding(*New))
                {
                    Ready = false;
                    break;
                }
            }

            if (!Ready)
            {
                delete New;
            }
        }
        while (!Ready);
    }

    if (Ready)
    {
        sf::Uint32 Aid = AidNext++;
        Aircrafts.insert(Aid, New);

        if (Net.IsServer())
        {
            sf::Int32 Rw = -1;
            if (New->GetLand() != 0)
            {
                Rw = 0;
                for (boost::ptr_list<Runway>::iterator it = Runways.begin(); it != Runways.end(); ++it, Rw++)
                {
                    if (&*it == New->GetLand())
                        break;
                }
            }

            sf::Packet Packet;
            Packet << 0 << PacketTypes::AircraftSpawn << Aid << Temp.Name << New->GetPos().x << New->GetPos().y << New->GetAngle() << Rw;
            if (New->GetDirection() == Aircraft::Out)
            {
                Packet << static_cast<sf::Uint16>(New->GetOutDirection());
            }
            Net.SendTcp(Packet);
        }
    }
}

void AirTrafficScreen::SpawnExplosion(sf::Vector2f Pos)
{
    map<string, ExplosionTemplate>::iterator it = ExplosionTemplates.begin();
    advance(it, rand() % ExplosionTemplates.size());
    const ExplosionTemplate &Temp = it->second;
    float Angle = Random(0.f, 360.f);
    Explosions.push_back(new Explosion(Temp, Textures, Sounds, Pos, Angle));

    if (Net.IsServer())
    {
        Net.SendTcp(sf::Packet() << 0 << PacketTypes::ExplosionUpdate << Temp.Name << Pos.x << Pos.y << Angle << 0.f);
    }
}

void AirTrafficScreen::SpawnScenery()
{
    map<string, SceneryTemplate>::iterator it;
    bool CanSpawn = false;
    do
    {
        it = SceneryTemplates.begin();
        advance(it, rand() % SceneryTemplates.size());

        if (find(it->second.Surfaces.begin(), it->second.Surfaces.end(), Background->GetTemplate().Name) != it->second.Surfaces.end())
        {
            CanSpawn = true;
            break;
        }
    }
    while (!CanSpawn);

    const SceneryTemplate &Temp = it->second;

    sf::Vector2f Pos;
    float Angle;

    Scenery *New;
    bool Ready;
    do
    {
        Ready = true;

        Pos.x = Random(100.f, 700.f);
        Pos.y = Random(100.f, 500.f);

        Angle = Random(0.f, 360.f);

        New = new Scenery(Temp, Textures, Pos, Angle);
        for (boost::ptr_list<Scenery>::iterator it = Sceneries.begin(); it != Sceneries.end(); ++it)
        {
            if (New->Colliding(*it))
            {
                Ready = false;
                break;
            }
        }

        for (boost::ptr_list<Runway>::iterator it = Runways.begin(); it != Runways.end(); ++it)
        {
            if (New->Colliding(*it))
            {
                Ready = false;
                break;
            }
        }

        if (CollidingShapes(AirportArea, New->GetShape()) != Temp.Airport)
            Ready = false;

        if (!Ready)
        {
            delete New;
        }
    }
    while (!Ready);

    Sceneries.push_back(New);
}

void AirTrafficScreen::PickSurface()
{
    if (Background)
    {
        delete Background;
    }

    map<string, SurfaceTemplate>::iterator it = SurfaceTemplates.begin();
    advance(it, rand() % SurfaceTemplates.size());
    const SurfaceTemplate &Temp = it->second;

    Background = new Surface(Temp, Textures);
}

void AirTrafficScreen::CalculateHull()
{
    vector<sf::Vector2f> Points;
    for (boost::ptr_list<Runway>::iterator it = Runways.begin(); it != Runways.end(); ++it)
    {
        sf::Sprite Shape = it->GetShape();
        for (int i = 0; i < 4; i++)
        {
            sf::Vector2f Point;
            switch (i)
            {
                case 0:
                    Point = sf::Vector2f(0.f, 0.f);
                    break;
                case 1:
                    Point = sf::Vector2f(Shape.getLocalBounds().width, 0.f);
                    break;
                case 2:
                    Point = sf::Vector2f(0.f, Shape.getLocalBounds().height);
                    break;
                case 3:
                    Point = sf::Vector2f(Shape.getLocalBounds().width, Shape.getLocalBounds().height);
                    break;
            }
            Points.push_back(Shape.getTransform().transformPoint(Point));
        }
    }

    AirportArea = ConvexHull(Points);
    AirportArea.setTexture(&Textures[Background->GetTemplate().AreaRes]);

    sf::FloatRect Bounds = AirportArea.getGlobalBounds();
    sf::Vector2f Origin(Bounds.left + Bounds.width / 2.f, Bounds.top + Bounds.height / 2.f);
    for (unsigned int i = 0; i < AirportArea.getPointCount(); i++)
        AirportArea.setPoint(i, AirportArea.getPoint(i) - Origin);
    AirportArea.scale(1.03f, 1.03f);
    AirportArea.setPosition(Origin);
}

bool AirTrafficScreen::IsNetActive()
{
    return Net.IsActive();
}


text:
#include "application_impl.h"

#include <server_lib/asserts.h>
#include <server_lib/emergency_helper.h>
#include <server_lib/platform_config.h>
#include <server_lib/thread_sync_helpers.h>

#include <boost/filesystem.hpp>

#include <cstring>

#include <server_clib/app.h>
#include <ssl_helpers/utils.h>
#include <ssl_helpers/encoding.h>

#if defined(SERVER_LIB_PLATFORM_LINUX)
#include <pthread.h>
#include <sys/syscall.h>
#include <iostream>

#include <unistd.h>
#include <sys/resource.h>

#endif

#include <signal.h>

#include "logger_set_internal_group.h"

#ifndef NDEBUG
#define TRACE_SIG_NUMBER(signo)                      \
    int32_t s__                                      \
        = 0x3030 | (signo / 10) | (signo % 10) << 8; \
    SRV_TRACE_SIGNAL_((char*)&s__);                  \
    SRV_TRACE_SIGNAL_("\n")
#else
#define TRACE_SIG_NUMBER(signo)
#endif

#define EXIT_FAILURE_SIG_BASE_ 128

namespace server_lib {
namespace {

    struct __signal_data
    {
        using control_signal = application::control_signal;

        union {
            int signo = 0;
            control_signal signal;
        } data;

        enum class data_type
        {
            null = 0,
            fail,
            exit,
            usersignal
        };
        data_type type = data_type::null;

        char path_data[PATH_MAX] = { 0 };

        static __signal_data make_empty_data();
        static __signal_data make_fail_data(const int signo, const char* = nullptr);
        static __signal_data make_exit_data(const int signo);
        static __signal_data make_control_data(const control_signal);
    };

    __signal_data __signal_data::make_empty_data()
    {
        return {};
    }

    __signal_data __signal_data::make_fail_data(const int signo, const char* stdump_file_path)
    {
        __signal_data r;
        r.type = data_type::fail;
        r.data.signo = signo;
        memset(r.path_data, 0, PATH_MAX);
        if (stdump_file_path)
        {
            std::strncpy(r.path_data, stdump_file_path, PATH_MAX - 1);
        }
        return r;
    }

    __signal_data __signal_data::make_exit_data(const int signo)
    {
        __signal_data r;
        r.type = data_type::exit;
        r.data.signo = signo;
        return r;
    }

    __signal_data __signal_data::make_control_data(const control_signal sig)
    {
        __signal_data r;
        r.type = data_type::usersignal;
        r.data.signal = sig;
        return r;
    }

    static __signal_data last_signal_data = __signal_data::make_empty_data();
    static char stdump_file_path[PATH_MAX] = { 0 };

    static std::function<void(int)> sig_callback = nullptr;
    static std::function<void(void)> atexit_callback = nullptr;
    static std::function<void(void)> fail_callback = nullptr;

    void __sig_callback_wrapper(int signo)
    {
        if (sig_callback)
            sig_callback(signo);
    }

    void __atexit_callback_wrapper()
    {
        if (atexit_callback)
            atexit_callback();
    }

    void __fail_callback_wrapper()
    {
        if (fail_callback)
            fail_callback();
    }

    void __set_signal_data(__signal_data&& __signal_data)
    {
        last_signal_data = std::move(__signal_data);
    }

    __signal_data __get_signal_data()
    {
        return last_signal_data;
    }

    void __init_stdump_generation(const std::string& path_to_dump_file)
    {
        std::strncpy(stdump_file_path, path_to_dump_file.c_str(), PATH_MAX - 1);
    }

    const char* __get_stdump_file_name()
    {
        if (stdump_file_path[0])
            return stdump_file_path;
        else
            return nullptr;
    }

    bool __enable_corefile(bool disable_excl_policy)
    {
        // Core dump creation is OS responsibility and OS specific
#if defined(SERVER_LIB_PLATFORM_LINUX)
        try
        {
            struct rlimit old_r, new_r;

            new_r.rlim_cur = RLIM_INFINITY;
            new_r.rlim_max = RLIM_INFINITY;

            SRV_ASSERT(prlimit(getpid(), RLIMIT_CORE, &new_r, &old_r) != -1);

            if (disable_excl_policy)
            {
                namespace fs = boost::filesystem;

                auto prev_core = fs::current_path() / "core";
                if (fs::exists(prev_core))
                {
                    std::ifstream core(prev_core.generic_string(), std::ifstream::binary);

                    char elf_header[4];
                    SRV_ASSERT(core && core.read(elf_header, sizeof(elf_header)), "File is not readable");

                    core.close();

                    auto elf_hex = ssl_helpers::to_hex(std::string(elf_header, sizeof(elf_header)));
                    SRV_ASSERT(elf_hex == "7f454c46", "Wrong file type");

                    auto tm = ssl_helpers::to_iso_string(fs::last_write_time(prev_core));
                    auto renamed = prev_core;
                    renamed = renamed.parent_path() / (std::string("core.") + tm);
                    fs::rename(prev_core, renamed);
                }
            }
        }
        catch (const std::exception& e)
        {
            std::cerr << e.what() << std::endl;
            return false;
        }
#endif
        return true;
    }

} // namespace

application_impl::application_impl()
{
    SRV_ASSERT(event_loop::is_main_thread(), "Only for main thread allowed");
    _exit_code = 0;
    cleanup();
}

void application_impl::set_config(const application_config& config)
{
    _config = config;
}

void application_impl::init()
{
#if defined(SERVER_LIB_PLATFORM_LINUX)
    auto this_pid = getpid();
    namespace fs = boost::filesystem;
    bool forbidden_threads = false;
    for (fs::directory_entry& th : fs::directory_iterator("/proc/self/task"))
    {
        auto p = th.path();
        if (this_pid != atol(p.filename().generic_string().c_str()))
        {
            forbidden_threads = true;
            std::cerr << "Not permitted because detected forbidden thread: " << p.filename().generic_string() << std::endl;
            break;
        }
    }
    SRV_ASSERT(!forbidden_threads,
               "Application must be initiated in main thread "
               "before creation of any other thread "
               "to make signal handling safe in multithreaded environment");
#endif

    SRV_ASSERT(event_loop::is_main_thread(), "Only for main thread allowed");

    std::string executable_name = emergency::get_executable_name();
    if (executable_name.empty())
        executable_name = "MAIN";

    _main_el.change_thread_name(executable_name);

    if (config().is_enable_corefile())
    {
        SRV_ASSERT(__enable_corefile(config().is_corefile_disable_excl_policy()), "Incompatible with current corefile creation system");
    }

    if (!config().path_to_stdump_file().empty())
        __init_stdump_generation(config().path_to_stdump_file());

    sig_callback = [this](int signo) { application_impl::process_signal(signo); };
    atexit_callback = [this]() { application_impl::process_atexit(); };
    fail_callback = [this]() { application_impl::process_fail(); };

    srv_c_app_init_default_signals_should_register();

    if (config().is_daemon())
    {
        srv_c_app_mt_init_daemon(__atexit_callback_wrapper,
                                 __sig_callback_wrapper,
                                 __fail_callback_wrapper,
                                 config().is_corefile_fail_thread_only() ? TRUE : FALSE);
    }
    else
    {
        srv_c_app_mt_init(__atexit_callback_wrapper,
                          __sig_callback_wrapper,
                          __fail_callback_wrapper,
                          config().is_corefile_fail_thread_only() ? TRUE : FALSE,
                          config().is_lock_io() ? TRUE : FALSE);
    }
}

int application_impl::run()
{
    if (_main_el.is_running())
        return exit_code_error;

    run_impl();
    return exit_code_ok;
}

void application_impl::on_start(start_callback_type&& callback)
{
    SRV_ASSERT(!_main_el.is_running(), "Call before running");

    _start_observer.subscribe(std::forward<start_callback_type>(callback));
}

void application_impl::on_exit(exit_callback_type&& callback)
{
    SRV_ASSERT(!_main_el.is_running(), "Call before running");

    _exit_observer.subscribe(std::forward<exit_callback_type>(callback));
}

void application_impl::on_fail(fail_callback_type&& callback)
{
    SRV_ASSERT(!_main_el.is_running(), "Call before running");

    _fail_observer.subscribe(std::forward<fail_callback_type>(callback));
}

void application_impl::on_control(control_callback_type&& callback)
{
    SRV_ASSERT(!_main_el.is_running(), "Call before running");

    _control_observer.subscribe(std::forward<control_callback_type>(callback));
}

void application_impl::wait()
{
    if (!_main_el.is_run())
    {
        std::unique_lock<std::mutex> lck(_wait_started_condition_lock);
        _wait_started_condition.wait(lck, [& el = _main_el] { return el.is_run(); });
    }
}

void application_impl::application_abort()
{
    SRV_TRACE_SIGNAL(__FUNCTION__);

#if defined(SERVER_LIB_PLATFORM_LINUX)
    ::srv_c_app_abort(0);
#else
    abort();
#endif
}

void application_impl::process_signal(int signal)
{
    if (srv_c_is_fail_signal(signal))
    {
        SRV_TRACE_SIGNAL_HEADER_("Got fail signal ");
        TRACE_SIG_NUMBER(signal);

        bool raw_dump_saved = false;
        if (__get_stdump_file_name())
        {
            raw_dump_saved = emergency::save_raw_stdump_s(__get_stdump_file_name()) > 0;
        }

        if (raw_dump_saved && __get_stdump_file_name())
        {
            __set_signal_data(__signal_data::make_fail_data(signal, __get_stdump_file_name()));
        }
        else
        {
            __set_signal_data(__signal_data::make_fail_data(signal));
        }

#if !defined(SERVER_LIB_PLATFORM_LINUX)
        application_impl::application_abort();
#endif
    }
    else
    {
#if defined(SERVER_LIB_PLATFORM_LINUX)
        using control_signal = application::control_signal;

        switch (signal)
        {
        case SIGUSR1:

            SRV_TRACE_SIGNAL("Got USR1 control signal");

            __set_signal_data(__signal_data::make_control_data(control_signal::USR1));
            break;
        case SIGUSR2:

            SRV_TRACE_SIGNAL("Got USR2 control signal");

            __set_signal_data(__signal_data::make_control_data(control_signal::USR2));
            break;
        default:
#else
        {
#endif
            SRV_TRACE_SIGNAL("Got exit signal");

            __set_signal_data(__signal_data::make_exit_data(signal));
        }
    }
}

void application_impl::process_atexit()
{
    SRV_TRACE_SIGNAL(__FUNCTION__);

    if (logger::check_instance())
        logger::instance().lock(); //boost logger implementation failes after 'exit' instructions

    auto& app = instance();
    if (app.is_running())
    {
        SRV_ERROR("std::exit is not allowed to stop application. Use application.stop instead");
    }
}

void application_impl::process_fail()
{
    SRV_TRACE_SIGNAL(__FUNCTION__);

    auto& app = instance();
    if (!app.config().is_corefile_fail_thread_only())
    {
        if (logger::check_instance())
            logger::instance().lock(); //some logger configurations can make application suspended in forked thread
    }

    using Callbackdata_type = __signal_data::data_type;

    auto last_signal_data = __get_signal_data();
    if (last_signal_data.type == Callbackdata_type::fail)
    {
        app.process_fail(last_signal_data.data.signo, last_signal_data.path_data);
    }
}

void application_impl::run_impl()
{
#if defined(SERVER_LIB_PLATFORM_LINUX)
    pthread_t signal_thread_id = 0;
#endif

    std::thread signal_thread([&]() {
        SRV_LOGC_TRACE("Signal thread has just started");

#if defined(SERVER_LIB_PLATFORM_LINUX)
        signal_thread_id = pthread_self();
#endif

        using Callbackdata_type = __signal_data::data_type;

        auto process_handler = [this]() -> bool {
            auto last_signal_data = __get_signal_data();
            switch (last_signal_data.type)
            {
            case Callbackdata_type::exit:
                this->_signal_thread_terminating = true;
                this->process_exit(last_signal_data.data.signo);
                break;
            case Callbackdata_type::usersignal:
                this->process_control(last_signal_data.data.signal);
                break;
            default:;
            }
            return this->_signal_thread_terminating.load();
        };
        bool terminated = false;

#if defined(SERVER_LIB_PLATFORM_LINUX)
        pthread_setname_np(pthread_self(), "signal");
#endif

        if (!terminated)
        {
            bool expected = false;
            while (!this->_signal_thread_initiated.compare_exchange_weak(expected, true))
            {
                if (!expected)
                    return;

                expected = false;
                spin_loop_pause();
            }

            SRV_LOGC_TRACE("Signal thread has completely initiated");
        }

        while (!terminated)
        {
            // Only here async signals can arise
            srv_c_app_mt_wait_sig_callback(__sig_callback_wrapper);

            SRV_LOGC_INFO("Got signal in signal thread");

            terminated = process_handler();
        }

        SRV_LOGC_TRACE("Signal thread has stopped");
    });
    _signal_thread.swap(signal_thread);

    bool expected = true;
    while (!this->_signal_thread_initiated.compare_exchange_weak(expected, true))
    {
        if (expected)
            return;

        expected = true;
        spin_loop_pause();
    }
    // Here signal thread has started, signal_thread_id has intialized

    // Stuck into main loop
    _main_el.on_start([this]() {
                SRV_LOGC_TRACE("Application has started");
                _start_observer.notify();
                _wait_started_condition.notify_all();
            })
        .start();

    // Here smth. has stopped main loop
    SRV_LOGC_TRACE("Application is stopping");

    if (_signal_thread_terminating.load())
        _signal_thread.join();
    else
    {
        // Main loop has stopped not by signal thread and signal thread
        // must be informed
        SRV_LOGC_TRACE("Throw notification to signal thread");

#if defined(SERVER_LIB_PLATFORM_LINUX)
        // It should send signal to exact thread because the signals was blocked
        // for others
        SRV_ASSERT(0 == pthread_kill(signal_thread_id, SIGTERM));
#else
        SRV_ASSERT(0 == raise(SIGTERM));
#endif
        _signal_thread.join();

        // It was not called from signal thread because main loop has not run
        _exit_observer.notify(0);
    }

    SRV_LOGC_INFO("Application has stopped");
    cleanup();
    auto exit_code = _exit_code.load();
    if (exit_code != 0)
    {
        if (exit_code < 0)
            exit_code = exit_code_error;

        _exit(exit_code);
    }
}

void application_impl::process_fail(const int signo, const char* dump_file_path)
{
    if (_fail_observer.is_any_subscriber())
    {
        SRV_LOGC_TRACE(__FUNCTION__ << "(" << dump_file_path << ")");

        _fail_observer.notify(signo, dump_file_path);
    }
}

void application_impl::process_exit(const int signo)
{
    if (_main_el.is_running())
    {
        SRV_LOGC_TRACE(__FUNCTION__);

        if (_exit_observer.is_any_subscriber())
        {
            _main_el.wait([this, signo]() {
                _exit_observer.notify(signo);
            });
        }

        stop(EXIT_FAILURE_SIG_BASE_ + signo);
    }
}

void application_impl::process_control(control_signal signal)
{
    if (_control_observer.is_any_subscriber())
    {
        SRV_LOGC_TRACE(__FUNCTION__);

        _main_el.post([this, signal]() {
            _control_observer.notify(signal);
        });
    }
    else
    {
        SRV_LOGC_TRACE(__FUNCTION__ << " - Ignored");
    }
}

} // namespace server_lib

ext: cpp

max_stars_repo_name: Andrei-Masilevich/barbacoa-server-lib

max_issues_repo_name: Andrei-Masilevich/barbacoa-server-lib

max_forks_repo_name: Andrei-Masilevich/barbacoa-server-lib


text:
/*****************************************************************************/
/*  Copyright (c) 2015, Karl Pauwels                                         */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include <chrono>
#include <iostream>
#include <sstream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <stdexcept>
#include <boost/filesystem.hpp>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <simtrack_nodes/multi_rigid_node.h>
#include <windowless_gl_context.h>
#undef Success
#include <Eigen/Geometry>
#include <translation_rotation_3d.h>
#include <hdf5_file.h>
#include <utilities.h>

using namespace util;

namespace simtrack {

void MultiRigidNode::detectorThreadFunction(cv::Mat camera_matrix, size_t width,
                                            size_t height) {

  // initialize CUDA in detector thread
  util::initializeCUDARuntime(device_id_detector_);

  int detector_object_index = 0;

  multi_rigid_detector_ =
      interface::MultiRigidDetector::Ptr(new interface::MultiRigidDetector(
          width, height, camera_matrix, obj_filenames_, device_id_detector_,
          parameters_detector_));

  while (!shutdown_detector_.load()) {

    if (detector_enabled_.load()) {

      // update selected objects if new objects selected
      if (switched_detector_objects_.load()) {
        {
          std::lock_guard<std::mutex> lock(obj_filenames_mutex_);
          multi_rigid_detector_->setObjects(obj_filenames_);
        }
        detector_object_index = 0;
        switched_detector_objects_.store(false);
      }

      // estimate pose if objects loaded in detector
      if (multi_rigid_detector_->getNumberOfObjects() > 0) {

        // process frame
        cv::Mat img_gray;
        {
          std::lock_guard<std::mutex> lock(img_gray_detector_mutex_);
          img_gray = img_gray_detector_.clone();
        }
        pose::TranslationRotation3D detector_pose;
        multi_rigid_detector_->estimatePose(img_gray, detector_object_index,
                                            detector_pose);

        // transmit pose to tracker
        {
          std::lock_guard<std::mutex> lock(most_recent_detector_pose_mutex_);
          most_recent_detector_object_index_ = detector_object_index;
          most_recent_detector_pose_ = detector_pose;
        }

        // select next object to detect
        detector_object_index = (detector_object_index + 1) %
                                multi_rigid_detector_->getNumberOfObjects();

      } else {
        // reduce load on this thread when no objects selected
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
      }
    }
  }
}

MultiRigidNode::MultiRigidNode(ros::NodeHandle nh)
    : nh_(nh), device_id_detector_(0), most_recent_detector_object_index_(0),
      detector_enabled_(true), shutdown_detector_(false), ready_(false),
      output_image_(
          interface::MultiRigidTracker::OutputImageType::model_appearance),
      recording_(false), root_recording_path_("/dev/shm/"), frame_count_(0),
      recording_start_time_(ros::Time::now()), auto_disable_detector_(false),
      color_only_mode_(false), switched_tracker_objects_(false) {
  // get model names from parameter server
  if (!ros::param::get("/simtrack/model_path", model_path_))
    throw std::runtime_error(
        std::string("MultiRigidNode::MultiRigidNode: could not "
                    "find /simtrack/model_path on parameter server\n"));

  std::vector<std::string> model_names;
  if (!ros::param::get("/simtrack/model_names", model_names))
    throw std::runtime_error(
        std::string("MultiRigidNode::MultiRigidNode: could not "
                    "find /simtrack/model_names on parameter server\n"));

  for (auto &it : model_names) {
    objects_.push_back(composeObjectInfo(it));
    obj_filenames_.push_back(composeObjectFilename(it));
    pose_publishers_[it] =
        nh.advertise<geometry_msgs::PoseStamped>("/simtrack/" + it, 1);
  }

  // get optical flow parameters
  ros::param::get("simtrack/optical_flow/n_scales", parameters_flow_.n_scales_);
  ros::param::get("simtrack/optical_flow/median_filter",
                  parameters_flow_.median_filter_);
  ros::param::get("simtrack/optical_flow/consistent",
                  parameters_flow_.consistent_);
  ros::param::get("simtrack/optical_flow/cons_thres",
                  parameters_flow_.cons_thres_);
  ros::param::get("simtrack/optical_flow/four_orientations",
                  parameters_flow_.four_orientations_);

  // get pose tracker parameters
  ros::param::get("simtrack/tracker/color_only_mode", color_only_mode_);
  ros::param::get("simtrack/tracker/n_icp_outer_it",
                  parameters_pose_.n_icp_outer_it_);
  ros::param::get("simtrack/tracker/n_icp_inner_it",
                  parameters_pose_.n_icp_inner_it_);
  ros::param::get("simtrack/tracker/w_flow", parameters_pose_.w_flow_);
  ros::param::get("simtrack/tracker/w_ar_flow", parameters_pose_.w_ar_flow_);
  ros::param::get("simtrack/tracker/w_disp", parameters_pose_.w_disp_);
  ros::param::get("simtrack/tracker/max_samples",
                  parameters_pose_.max_samples_);
  int key_bits = parameters_pose_.getKeyBits();
  ros::param::get("simtrack/tracker/key_bits", key_bits);
  parameters_pose_.setKeyBits(key_bits);
  ros::param::get("simtrack/tracker/near_plane", parameters_pose_.near_plane_);
  ros::param::get("simtrack/tracker/far_plane", parameters_pose_.far_plane_);
  ros::param::get("simtrack/tracker/reliability_threshold",
                  parameters_pose_.reliability_threshold_);
  ros::param::get("simtrack/tracker/max_proportion_projected_bounding_box",
                  parameters_pose_.max_proportion_projected_bounding_box_);
  ros::param::get("simtrack/tracker/sparse_intro_reliability_threshold",
                  parameters_pose_.sparse_intro_reliability_threshold_);
  ros::param::get("simtrack/tracker/sparse_intro_allowed_reliability_decrease",
                  parameters_pose_.sparse_intro_allowed_reliability_decrease_);
  ros::param::get("simtrack/tracker/max_t_update_norm_squared",
                  parameters_pose_.max_t_update_norm_squared_);

  // get detector parameters
  ros::param::get("simtrack/detector/device_id", device_id_detector_);
  ros::param::get("simtrack/detector/vec_size", parameters_detector_.vec_size_);
  ros::param::get("simtrack/detector/num_iter_ransac",
                  parameters_detector_.num_iter_ransac_);

  /*****************************/
  /* Setup CUDA for GL interop */
  /*****************************/

  int device_id_tracker = 0;
  ros::param::get("simtrack/tracker/device_id", device_id_tracker);

  // Create dummy GL context before cudaGL init
  render::WindowLessGLContext dummy(10, 10);

  // CUDA Init
  util::initializeCUDARuntime(device_id_tracker);

  // auto-disable detector in case of single gpu
  auto_disable_detector_ = (device_id_tracker == device_id_detector_);

  ready_ = true;
}

MultiRigidNode::~MultiRigidNode() {
  // cleanly shutdown detector thread (if running)
  if (detector_thread_ != nullptr) {
    shutdown_detector_.store(true);
    detector_thread_->join();
  }
}

bool MultiRigidNode::start() {
  if (!ready_) {
    return false;
  }

  switch_objects_srv_ = nh_.advertiseService(
      "/simtrack/switch_objects", &MultiRigidNode::switchObjects, this);

  bool compressed_streams = false;
  ros::param::get("simtrack/use_compressed_streams", compressed_streams);

  image_transport::TransportHints rgb_hint, depth_hint;
  if (compressed_streams) {
    rgb_hint = image_transport::TransportHints("compressed");
    depth_hint = image_transport::TransportHints("compressedDepth");
  } else {
    rgb_hint = image_transport::TransportHints("raw");
    depth_hint = image_transport::TransportHints("raw");
  }

  rgb_it_.reset(new image_transport::ImageTransport(nh_));
  sub_rgb_.subscribe(*rgb_it_, "rgb", 2, rgb_hint);
  sub_rgb_info_.subscribe(nh_, "rgb_info", 2);

  if (color_only_mode_) {
    sync_rgb_.reset(
        new SynchronizerRGB(SyncPolicyRGB(5), sub_rgb_, sub_rgb_info_));
    sync_rgb_->registerCallback(
        boost::bind(&MultiRigidNode::colorOnlyCb, this, _1, _2));
  } else {
    depth_it_.reset(new image_transport::ImageTransport(nh_));
    sub_depth_.subscribe(*depth_it_, "depth", 2, depth_hint);
    sync_rgbd_.reset(new SynchronizerRGBD(SyncPolicyRGBD(5), sub_depth_,
                                          sub_rgb_, sub_rgb_info_));
    sync_rgbd_->registerCallback(
        boost::bind(&MultiRigidNode::depthAndColorCb, this, _1, _2, _3));
  }

  debug_img_it_.reset(new image_transport::ImageTransport(nh_));
  debug_img_pub_ = debug_img_it_->advertise("/simtrack/image", 1);

  dynamic_reconfigure::Server<simtrack_nodes::VisualizationConfig>::CallbackType
  f;
  f = boost::bind(&MultiRigidNode::reconfigureCb, this, _1, _2);
  dynamic_reconfigure_server_.setCallback(f);

  return true;
}

bool MultiRigidNode::switchObjects(simtrack_nodes::SwitchObjectsRequest &req,
                                   simtrack_nodes::SwitchObjectsResponse &res) {
  std::stringstream ss;
  ss << "simtrack switching to models: ";
  for (auto &it : req.model_names)
    ss << it << " ";
  ROS_INFO("%s", ss.str().c_str());

  // switch tracker
  objects_.clear();
  for (auto &it : req.model_names)
    objects_.push_back(composeObjectInfo(it));
  switched_tracker_objects_ = true;

  // switch detector
  // the detector may start issuing poses out of bounds to the tracker
  {
    std::lock_guard<std::mutex> lock(obj_filenames_mutex_);
    obj_filenames_.clear();
    for (auto &it : req.model_names)
      obj_filenames_.push_back(composeObjectFilename(it));
  }
  switched_detector_objects_.store(true);

  return true;
}

void MultiRigidNode::depthAndColorCb(
    const sensor_msgs::ImageConstPtr &depth_msg,
    const sensor_msgs::ImageConstPtr &rgb_msg,
    const sensor_msgs::CameraInfoConstPtr &rgb_info_msg) {
  // we'll assume registration is correct so that rgb and depth camera matrices
  // are equal
  camera_matrix_rgb_ = composeCameraMatrix(rgb_info_msg);

  cv_bridge::CvImageConstPtr cv_rgb_ptr, cv_depth_ptr;
  try {
    if (rgb_msg->encoding == "8UC1") // fix for kinect2 mono output
      cv_rgb_ptr = cv_bridge::toCvShare(rgb_msg);
    else
      cv_rgb_ptr = cv_bridge::toCvShare(rgb_msg, "mono8");
    cv_depth_ptr = cv_bridge::toCvShare(depth_msg);
  }
  catch (cv_bridge::Exception &e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  updatePose(cv_rgb_ptr, cv_depth_ptr, rgb_msg->header.frame_id);
}

void MultiRigidNode::colorOnlyCb(
    const sensor_msgs::ImageConstPtr &rgb_msg,
    const sensor_msgs::CameraInfoConstPtr &rgb_info_msg) {
  // we'll assume registration is correct so that rgb and depth camera matrices
  // are equal
  camera_matrix_rgb_ = composeCameraMatrix(rgb_info_msg);

  cv_bridge::CvImageConstPtr cv_rgb_ptr, cv_depth_ptr;
  try {
    if (rgb_msg->encoding == "8UC1") // fix for kinect2 mono output
      cv_rgb_ptr = cv_bridge::toCvShare(rgb_msg);
    else
      cv_rgb_ptr = cv_bridge::toCvShare(rgb_msg, "mono8");
  }
  catch (cv_bridge::Exception &e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  updatePose(cv_rgb_ptr, cv_depth_ptr, rgb_msg->header.frame_id);
}

void MultiRigidNode::updatePose(const cv_bridge::CvImageConstPtr &cv_rgb_ptr,
                                const cv_bridge::CvImageConstPtr &cv_depth_ptr,
                                const std::string &frame_id) {
  if ((!color_only_mode_) && (cv_depth_ptr == nullptr))
    throw std::runtime_error("MultiRigidNode::updatePose: received "
                             "nullptr depth while not in color_only_mode_\n");

  if (cv_rgb_ptr->image.type() != CV_8UC1)
    throw std::runtime_error("MultiRigidNode::updatePose: image type "
                             "should be CV_8UC1\n");

  // initialize detector thread if not yet active
  // the engine is created here since we need camera info
  if (detector_thread_ == nullptr) {
    detector_thread_ = std::unique_ptr<std::thread>(new std::thread(
        &MultiRigidNode::detectorThreadFunction, this, camera_matrix_rgb_,
        cv_rgb_ptr->image.cols, cv_rgb_ptr->image.rows));
  }

  // copy the image for the detector (if running)
  if (detector_enabled_.load()) {
    std::lock_guard<std::mutex> lock(img_gray_detector_mutex_);
    img_gray_detector_ = cv_rgb_ptr->image.clone();
  }

  // rescale tracker image and adjust camera matrix if size differs from depth
  // image, in this case the depth image size will dominate the conversion, this
  // allows for using a high-res image for detection while still maintaining
  // full frame rate for tracking
  cv::Mat img_gray_tracker;
  cv::Mat camera_matrix_rgb_tracker = camera_matrix_rgb_.clone();
  if ((!color_only_mode_) &&
      (cv_rgb_ptr->image.size() != cv_depth_ptr->image.size())) {
    cv::resize(cv_rgb_ptr->image, img_gray_tracker, cv_depth_ptr->image.size());
    // adjust camera matrix
    double scale_x =
        (double)cv_depth_ptr->image.cols / (double)cv_rgb_ptr->image.cols;
    double scale_y =
        (double)cv_depth_ptr->image.rows / (double)cv_rgb_ptr->image.rows;
    camera_matrix_rgb_tracker.at<double>(0, 0) *= scale_x;
    camera_matrix_rgb_tracker.at<double>(0, 2) *= scale_x;
    camera_matrix_rgb_tracker.at<double>(1, 1) *= scale_y;
    camera_matrix_rgb_tracker.at<double>(1, 2) *= scale_y;
  } else {
    img_gray_tracker = cv_rgb_ptr->image;
  }

  // initialize tracker engine if not yet active
  // the engine is created here since we need camera info
  if (multi_rigid_tracker_ == nullptr) {
    multi_rigid_tracker_ =
        interface::MultiRigidTracker::Ptr(new interface::MultiRigidTracker(
            img_gray_tracker.cols, img_gray_tracker.rows,
            camera_matrix_rgb_tracker, objects_, parameters_flow_,
            parameters_pose_));
  }

  // update selected objects if new objects selected
  if (switched_tracker_objects_) {
    multi_rigid_tracker_->setObjects(objects_);
    switched_tracker_objects_ = false;
  }

  // process frame if objects loaded in tracker
  // ------------------------------------------
  if (multi_rigid_tracker_->getNumberOfObjects() > 0) {

    // update detector pose in tracker
    {
      std::lock_guard<std::mutex> lock(most_recent_detector_pose_mutex_);
      if (most_recent_detector_object_index_ <
          multi_rigid_tracker_->getNumberOfObjects()) {
        multi_rigid_tracker_->setRigidDetectorPose(
            most_recent_detector_pose_, most_recent_detector_object_index_);
      }
    }

    // update tracker pose
    if (color_only_mode_)
      multi_rigid_tracker_->updatePoses(img_gray_tracker);
    else
      multi_rigid_tracker_->updatePoses(img_gray_tracker, cv_depth_ptr->image);

    // publish reliable poses
    std::vector<geometry_msgs::Pose> poses =
        multi_rigid_tracker_->getPoseMessages();

    for (int object_index = 0; object_index < poses.size(); object_index++) {
      if (multi_rigid_tracker_->isPoseReliable(object_index)) {
        geometry_msgs::Pose curr_pose = poses.at(object_index);
        tf::StampedTransform object_transform;
        object_transform.setOrigin(tf::Vector3(
            curr_pose.position.x, curr_pose.position.y, curr_pose.position.z));
        object_transform.setRotation(
            tf::Quaternion(curr_pose.orientation.x, curr_pose.orientation.y,
                           curr_pose.orientation.z, curr_pose.orientation.w));
        //      object_transform.stamp_ = ros::Time::now();
        object_transform.stamp_ = cv_rgb_ptr->header.stamp;
        object_transform.frame_id_ = frame_id;
        object_transform.child_frame_id_ = objects_.at(object_index).label_;
        tfb_.sendTransform(object_transform);

        geometry_msgs::PoseStamped curr_pose_stamped;
        curr_pose_stamped.pose = curr_pose;
        curr_pose_stamped.header.frame_id = frame_id;
        curr_pose_stamped.header.stamp = cv_rgb_ptr->header.stamp;
        pose_publishers_[objects_.at(object_index).label_]
            .publish(curr_pose_stamped);
      }
    }

    // disable detector if all objects tracked and auto-disable enabled
    // (single-gpu case)
    if (auto_disable_detector_)
      detector_enabled_.store(!multi_rigid_tracker_->areAllPosesReliable());

    // generate output image
    cv::Mat texture = multi_rigid_tracker_->generateOutputImage(output_image_);

    bool show_bounding_boxes = false;
    if (show_bounding_boxes) {
      auto bounding_boxes =
          multi_rigid_tracker_->getBoundingBoxesInCameraImage();
      for (int object_index = 0; object_index < poses.size(); object_index++) {
        if (multi_rigid_tracker_->isPoseReliable(object_index)) {
          for (int r = 0; r < 8; r++) {
            // draw in image
            auto p = cv::Point(bounding_boxes.at(object_index).at(r * 2),
                               bounding_boxes.at(object_index).at(r * 2 + 1));
            cv::circle(texture, p, 3, CV_RGB(255, 0, 0), -1, 8);
          }
        }
      }
    }

    debug_img_pub_.publish(
        cv_bridge::CvImage(std_msgs::Header(), "rgba8", texture).toImageMsg());

    // record data to new file if requested
    if (recording_) {
      // create file
      std::stringstream file_name;
      file_name << "frame_" << std::setw(6) << std::setfill('0') << frame_count_
                << ".h5";
      boost::filesystem::path file_path = recording_path_ / file_name.str();
      util::HDF5File file(file_path.string());

      // store frame time
      std::vector<int> time_size{ 1 };
      std::vector<double> time_data{(ros::Time::now() - recording_start_time_)
                                        .toSec() };
      file.writeArray("time", time_data, time_size);

      // store poses
      if (recording_flags_.poses_)
        multi_rigid_tracker_->savePoses(file);

      // store rgb
      if (recording_flags_.image_) {
        cv::Mat rgb;
        cv::cvtColor(cv_rgb_ptr->image, rgb, CV_BGR2RGB);
        std::vector<int> rgb_size = { rgb.rows, rgb.cols, rgb.channels() };
        int rgb_n = accumulate(rgb_size.begin(), rgb_size.end(), 1,
                               std::multiplies<int>());
        std::vector<uint8_t> rgb_data((uint8_t *)rgb.data,
                                      (uint8_t *)rgb.data + rgb_n);
        file.writeArray("image", rgb_data, rgb_size);
      }

      // store depth
      if (recording_flags_.depth_ && (!color_only_mode_)) {
        cv::Mat depth = cv_depth_ptr->image;
        std::vector<int> depth_size = { depth.rows, depth.cols };
        int depth_n = depth.rows * depth.cols;
        if (depth.type() == CV_32FC1) {
          std::vector<float> depth_data((float *)depth.data,
                                        (float *)depth.data + depth_n);
          file.writeArray("depth", depth_data, depth_size);
        } else if (depth.type() == CV_16UC1) {
          std::vector<uint16_t> depth_data((uint16_t *)depth.data,
                                           (uint16_t *)depth.data + depth_n);
          file.writeArray("depth", depth_data, depth_size);
        }
      }

      // store optical flow
      if (recording_flags_.optical_flow_)
        multi_rigid_tracker_->saveOpticalFlow(file);
    }
  }
  frame_count_++;
}

void MultiRigidNode::reconfigureCb(simtrack_nodes::VisualizationConfig &config,
                                   uint32_t level) {
  switch (config.visualization) {
  case 0:
    output_image_ =
        interface::MultiRigidTracker::OutputImageType::model_appearance;
    break;
  case 1:
    output_image_ =
        interface::MultiRigidTracker::OutputImageType::model_appearance_blended;
    break;
  case 2:
    output_image_ =
        interface::MultiRigidTracker::OutputImageType::optical_flow_x;
    break;
  case 3:
    output_image_ =
        interface::MultiRigidTracker::OutputImageType::optical_flow_y;
    break;
  }

  // update recording flags
  recording_flags_.poses_ = config.save_object_poses;
  recording_flags_.image_ = config.save_image;
  recording_flags_.depth_ = config.save_depth;
  recording_flags_.optical_flow_ = config.save_optical_flow;

  // generate a new folder whenever recording is activated
  if ((!recording_) && (config.start_stop_recording)) {
    // create new recording folder
    // count up from simtrack_000 until one doesn't exist
    int folder_count = 0;
    bool path_exists = true;
    while (path_exists) {
      std::stringstream relative_path;
      relative_path << "simtrack_recording_" << std::setw(3)
                    << std::setfill('0') << folder_count;
      recording_path_ = root_recording_path_ / relative_path.str();
      path_exists = boost::filesystem::exists(recording_path_);
      folder_count++;
    }
    boost::filesystem::create_directory(recording_path_);

    // save configuration file (camera_matrix, object info)
    util::HDF5File file((recording_path_ / "scene_info.h5").string());
    std::vector<int> size{ camera_matrix_rgb_.rows, camera_matrix_rgb_.cols };
    int n = accumulate(size.begin(), size.end(), 1, std::multiplies<int>());
    std::vector<double> data((double *)camera_matrix_rgb_.data,
                             (double *)camera_matrix_rgb_.data + n);
    file.writeArray("camera_matrix_rgb", data, size);
    std::vector<std::string> object_labels, object_filenames;
    for (auto &it : objects_) {
      object_labels.push_back(it.label_);
      object_filenames.push_back(it.filename_);
    }
    size = {(int)objects_.size() };
    file.writeArray("object_labels", object_labels, size);
    file.writeArray("object_filenames", object_filenames, size);
    frame_count_ = 0;
    recording_start_time_ = ros::Time::now();
  }

  recording_ = config.start_stop_recording;
}

interface::MultiRigidTracker::ObjectInfo
MultiRigidNode::composeObjectInfo(std::string model_name) {
  std::string obj_file_name =
      model_path_ + "/" + model_name + "/" + model_name + ".obj";
  return (interface::MultiRigidTracker::ObjectInfo(model_name, obj_file_name));
}

std::string MultiRigidNode::composeObjectFilename(std::string model_name) {
  return (model_path_ + "/" + model_name + "/" + model_name + "_SIFT.h5");
}

cv::Mat MultiRigidNode::composeCameraMatrix(
    const sensor_msgs::CameraInfoConstPtr &info_msg) {
  cv::Mat camera_matrix =
      cv::Mat(3, 4, CV_64F, (void *)info_msg->P.data()).clone();
  camera_matrix.at<double>(0, 2) -= info_msg->roi.x_offset;
  camera_matrix.at<double>(1, 2) -= info_msg->roi.y_offset;
  return (camera_matrix);
}

} // end namespace simtrack

max_stars_repo_name: carlo-/simtrack

max_issues_repo_name: carlo-/simtrack

max_forks_repo_name: carlo-/simtrack