#include "Servo.h"
#include <sstream>

#pragma comment(lib, "Psapi.lib")
#include <Psapi.h>

#include <algorithm> 
#include <functional> 
#include <cctype>
#include <locale>

// trim from start (in place)
static inline void ltrim(std::string &s) {
    s.erase(s.begin(), std::find_if(s.begin(), s.end(),
        std::not1(std::ptr_fun<int, int>(std::isspace))));
}

// trim from end (in place)
static inline void rtrim(std::string &s) {
    s.erase(std::find_if(s.rbegin(), s.rend(),
        std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
}

// trim from both ends (in place)
static inline void trim(std::string &s) {
    ltrim(s);
    rtrim(s);
}

// trim from start (copying)
static inline std::string ltrimmed(std::string s) {
    ltrim(s);
    return s;
}

// trim from end (copying)
static inline std::string rtrimmed(std::string s) {
    rtrim(s);
    return s;
}

// trim from both ends (copying)
static inline std::string trimmed(std::string s) {
    trim(s);
    return s;
}

std::string Servo::StringSub(std::string instring, std::string subin, std::string subout)
{
    if (instring.find(subin) != std::string::npos)
        return(instring.replace(instring.find(subin), subin.length(), subout));
    else
        return instring;
}

void Servo::formatCommand(std::string* command)
{
    std::stringstream id;
    id << m_AshitaCore->GetDataManager()->GetTarget()->GetTargetServerId();

    auto p0 = m_AshitaCore->GetDataManager()->GetParty()->GetMemberName(0);
    auto p1 = m_AshitaCore->GetDataManager()->GetParty()->GetMemberName(1);
    auto p2 = m_AshitaCore->GetDataManager()->GetParty()->GetMemberName(2);
    auto p3 = m_AshitaCore->GetDataManager()->GetParty()->GetMemberName(3);
    auto p4 = m_AshitaCore->GetDataManager()->GetParty()->GetMemberName(4);
    auto p5 = m_AshitaCore->GetDataManager()->GetParty()->GetMemberName(5);

    *command = StringSub(*command, "[me]", p0 != nullptr ? p0 : "");
    *command = StringSub(*command, "[p0]", p0 != nullptr ? p0 : "");
    *command = StringSub(*command, "[p1]", p1 != nullptr ? p1 : "");
    *command = StringSub(*command, "[p2]", p2 != nullptr ? p2 : "");
    *command = StringSub(*command, "[p3]", p3 != nullptr ? p3 : "");
    *command = StringSub(*command, "[p4]", p4 != nullptr ? p4 : "");
    *command = StringSub(*command, "[p5]", p5 != nullptr ? p5 : "");
    *command = StringSub(*command, "[t]", id.str());
}

void Servo::SendCommand(std::string command)
{
    while (!lock.try_lock())
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    buffer.push_back(command);
    lock.unlock();
}

void Servo::WalkTo(float x, float z)
{
    if (zoning)
        return;

    auto myX = m_AshitaCore->GetDataManager()->GetEntity()->GetLocalX(m_AshitaCore->GetDataManager()->GetParty()->GetMemberTargetIndex(0));
    auto myZ = m_AshitaCore->GetDataManager()->GetEntity()->GetLocalZ(m_AshitaCore->GetDataManager()->GetParty()->GetMemberTargetIndex(0));
    auto myStatus = m_AshitaCore->GetDataManager()->GetEntity()->GetStatus(m_AshitaCore->GetDataManager()->GetParty()->GetMemberTargetIndex(0));
    float dist = sqrt(pow((myX - x), 2.0f) + pow((myZ - z), 2.0f));
    if (dist > 1.5f && (myStatus == 0 || myStatus == 5))
    {
        if (StructPointer->AutoRun != 1)
            StructPointer->AutoRun = 1;
        StructPointer->DirectionX = x - myX;
        StructPointer->DirectionZ = z - myZ;
        StructPointer->DirectionY = 0;
    }
    else
        StructPointer->AutoRun = 0;
}

void Servo::RecvCommand(std::string* command)
{
    command->erase();
    if (lock.try_lock())
    {
        if (!buffer.empty())
        {
            command->swap(buffer.front());
            buffer.pop_front();
        }
        lock.unlock();
    }
}

void Servo::SetState(ServoMode::Mode mode)
{
    ServoMode::Mode prevState = state;
    state = ServoMode::Mode::Standby;
    follow = false;
    zoning = false;
    connected = false;
    if (prevState != ServoMode::Mode::Standby)
    {
        conn.join();
        pos.join();
    }
    while (!buffer.empty())
        buffer.pop_front();
    state = mode;
}

void Servo::Sync(std::string host)
{
    me = m_AshitaCore->GetDataManager()->GetParty()->GetMemberName(0);
    log << Chat::Format::RoyalBlue << "[Servo] Servo started in client mode, connected to: ";
    while (!lservAddr.try_lock())
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if (host != "")
    {
        servAddr = host;
        log << servAddr;
    }
    else
    {
        servAddr = "";
        log << "localhost";
    }
    lservAddr.unlock();

    SetState(ServoMode::Mode::Client);
    conn.swap(std::thread(&Servo::Client, this));
    pos.swap(std::thread(&Servo::Follow, this));
    log << Chat::Control::flush;
}

bool Servo::Initialize(IAshitaCore* ashitaCore, ILogManager* logManager, uint32_t dwPluginId)
{
    this->m_AshitaCore = ashitaCore;
    this->m_PluginId = dwPluginId;

    // Obtain the base module information..
    MODULEINFO mod = { 0 };
    if (!::GetModuleInformation(::GetCurrentProcess(), ::GetModuleHandle("FFXiMain.dll"), &mod, sizeof(MODULEINFO)))
        return false;

    auto Pointer = (unsigned char*)Ashita::Memory::FindPattern((uintptr_t)mod.lpBaseOfDll, mod.SizeOfImage, "8BCFE8????????8B0D????????E8????????8BE885??750CB9", 0, 0);
    StructPointer = nullptr;
    if (Pointer != nullptr)
    {

        Pointer += 25;
        StructPointer = (auto_follow*)(*((DWORD*)Pointer));
    }
    else
        ashitaCore->GetChatManager()->Write("[Servo] Failed to locate required signature for autofollow!");

    if (StructPointer)
    {
        StructPointer->DirectionX;
        StructPointer->DirectionZ;
        StructPointer->AutoRun;
    }

    cmdParse = new CommandParser;
    state = ServoMode::Mode::Standby;
    lservAddr.lock();
    servAddr = "";
    lservAddr.unlock();
    me = "";
    follow = false;
    zoning = false;

    zContext = zmq::context_t(1);
    conn = std::thread();
    pos = std::thread();

    log.SetCore(m_AshitaCore);
    log << Chat::Mode::Echo;

    return 1;
}

void Servo::Serv()
{
    zmq::socket_t publisher(zContext, ZMQ_PUB);
    std::string hostmask = "tcp://";
    while (!lservAddr.try_lock())
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if (!servAddr.empty())
        hostmask.append(servAddr);
    else
        hostmask.append("127.0.0.1");
    lservAddr.unlock();
    hostmask.append(":56556");
    try {
        publisher.bind(hostmask.c_str());
    }
    catch (...) {
        this->m_AshitaCore->GetChatManager()->Write("[Servo] Failed to bind to socket.");
        publisher.close();
        return;
    }

    connected = true;

    while (connected) {
        while (!lock.try_lock())
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        if (!buffer.empty())
        {
            auto it = buffer.begin();
            zmq::message_t message(it->length() + 1);
            strcpy_s((char*)message.data(), (size_t)(it->length() + 1), it->c_str());
            buffer.pop_front();
            lock.unlock();
            publisher.send(message, ZMQ_DONTWAIT);
        }
        else
            lock.unlock();
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
    publisher.close();
}

void Servo::ServPos()
{
    zmq::socket_t publisher(zContext, ZMQ_PUB);
    std::string hostmask = "tcp://";
    while (!lservAddr.try_lock())
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if (!servAddr.empty())
        hostmask.append(servAddr);
    else
        hostmask.append("127.0.0.1");
    lservAddr.unlock();
    hostmask.append(":56557");
    try {
        publisher.bind(hostmask.c_str());
    }
    catch (...) {
        this->m_AshitaCore->GetChatManager()->Write("[Servo] Failed to bind to socket.");
        publisher.close();
        return;
    }


    if (!connected)
        std::this_thread::sleep_for(std::chrono::milliseconds(100));

    while (connected) {
        if (!zoning && m_AshitaCore->GetDataManager()->GetParty()->GetMemberTargetIndex(0))
        {
            auto myX = m_AshitaCore->GetDataManager()->GetEntity()->GetLocalX(m_AshitaCore->GetDataManager()->GetParty()->GetMemberTargetIndex(0));
            auto myZ = m_AshitaCore->GetDataManager()->GetEntity()->GetLocalZ(m_AshitaCore->GetDataManager()->GetParty()->GetMemberTargetIndex(0));

            std::stringstream ss;
            ss << m_AshitaCore->GetDataManager()->GetParty()->GetMemberZone(0) << " " << myX << " " << myZ;
            zmq::message_t message(strlen(ss.str().c_str()) + 1);
            strcpy_s((char*)message.data(), strlen(ss.str().c_str()) + 1, ss.str().c_str());
            publisher.send(message, ZMQ_DONTWAIT);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }
    publisher.close();
}

void Servo::Client()
{
    std::string host = "tcp://";
    while (!lservAddr.try_lock())
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if (!servAddr.empty())
        host.append(servAddr);
    else
        host.append("localhost");
    lservAddr.unlock();
    host.append(":56556");
    zmq::socket_t subscriber(zContext, ZMQ_SUB);
    subscriber.connect(host.c_str());
    subscriber.setsockopt(ZMQ_SUBSCRIBE, me.c_str(), me.size());
    for (auto it = groups.begin(); it != groups.end(); ++it)
        subscriber.setsockopt(ZMQ_SUBSCRIBE, it->c_str(), it->size());
    subscriber.setsockopt(ZMQ_SUBSCRIBE, "::", 2);
    subscriber.setsockopt(ZMQ_SUBSCRIBE, ":SERVOFUNC:", 11);
    connected = true;

    while (connected)
    {
        zmq::message_t update;

        while (subscriber.recv(&update, ZMQ_DONTWAIT) == false)
        {
            if (connected)
                std::this_thread::sleep_for(std::chrono::milliseconds(20));
            else
                break;
        }

        std::string s(static_cast<char*>(update.data()));
        if (!s.empty())
        {
            while (!lock.try_lock())
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
            buffer.push_back(s);
            lock.unlock();
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
    subscriber.close();
}

void Servo::Follow()
{
    std::string host = "tcp://";
    while (!lservAddr.try_lock())
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    if (!servAddr.empty())
        host.append(servAddr);
    else
        host.append("localhost");
    lservAddr.unlock();
    host.append(":56557");
    zmq::socket_t subscriber(zContext, ZMQ_SUB);
    subscriber.connect(host.c_str());
    subscriber.setsockopt(ZMQ_SUBSCRIBE, "", 0);

    if (!connected)
        std::this_thread::sleep_for(std::chrono::milliseconds(100));

    while (connected)
    {
        zmq::message_t update;
        while (follow)
        {
            update = zmq::message_t();
            while (subscriber.recv(&update, ZMQ_DONTWAIT) == false)
            {
                if (connected)
                    std::this_thread::sleep_for(std::chrono::milliseconds(50));
                else
                    break;
            }
            std::stringstream ss(static_cast<char*>(update.data()));
            if (!ss.str().empty())
            {
                float WARPX, WARPY;
                short zoneid;
                ss >> zoneid >> WARPX >> WARPY;
                if (zoneid == m_AshitaCore->GetDataManager()->GetParty()->GetMemberZone(0))
                    WalkTo(WARPX, WARPY);
                else
                    StructPointer->AutoRun = 0; // If no longer sharing the same zone, stop running (will trigger after the server char has arrived in the new area, so there is some delay)
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

        while (subscriber.recv(&update, ZMQ_DONTWAIT) == false)
        {
            if (connected & !follow)
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
            else
                break;
        }

        update.data(); // Discard data when not following.
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
    subscriber.close();
}

void Servo::Release()
{
    SetState(ServoMode::Mode::Standby);
    zContext.close();
    delete cmdParse;
}

plugininfo_t Servo::GetPluginInfo(void)
{
    return (*g_PluginInfo);
}

bool  Servo::HandleCommand(const char* szCommand, int iType)
{
    if (iType == 0)
    {
        return false;
    }

    char szClean[1024];
    if (iType != -2)
        m_AshitaCore->GetChatManager()->ParseAutoTranslate(szCommand, szClean, 1024, false);
    else
        strncpy_s(szClean, szCommand, 1024);

    std::string arg;

    cmdParse->InputCommand(szClean);
    cmdParse->GetFirstCommand(&arg);

    if (arg == "/servo")
    {
        cmdParse->GetNextCommand(&arg);
        if (arg == "on")
        {
            log << Chat::Format::RoyalBlue << "[Servo] Servo started in server mode, with a hostmask of: ";
            while (!lservAddr.try_lock())
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
            if (cmdParse->GetNextCommand(&arg))
            {
                servAddr = arg;
                log << servAddr;
            }
            else
            {
                servAddr = "";
                log << "localhost";
            }
            lservAddr.unlock();

            SetState(ServoMode::Mode::Server);
            conn.swap(std::thread(&Servo::Serv, this));
            pos.swap(std::thread(&Servo::ServPos, this));
            log << Chat::Control::flush;
        }
        else if (arg == "sync")
        {
            if (!cmdParse->GetNextCommand(&arg))
                arg = "";
            Sync(arg);
        }
        else if (arg == "off")
        {
            SetState(ServoMode::Mode::Standby);
            log << Chat::Format::RoyalBlue << "[Servo] Servo set to standy!" << Chat::Control::flush;
        }
        else if (arg == "command" || arg == "send")
        {
            if (cmdParse->GetRemainingCommands(&arg))
            {
                arg = ":: " + arg;
                formatCommand(&arg);
                SendCommand(arg);
            }

        }
        else if (arg == "sendto")
        {
            if (cmdParse->GetRemainingCommands(&arg))
            {
                formatCommand(&arg);
                SendCommand(arg);
            }

        }
        else if (arg == "followme")
        {
            if (state.load() == ServoMode::Mode::Server)
            {
                if (!cmdParse->GetNextCommand(&arg))
                {
                    if (arg == "on")
                        SendCommand(":SERVOFUNC: FOLLOWON");
                    else if (arg == "off")
                        SendCommand(":SERVOFUNC: FOLLOWOFF");
                }
                else
                    SendCommand(":SERVOFUNC: FOLLOW");
            }
        }
        else if (arg == "follow")
        {
            if (state.load() == ServoMode::Mode::Client)
            {
                if (!cmdParse->GetNextCommand(&arg))
                {
                    if (arg == "on")
                        follow = true;
                    else if (arg == "off")
                        follow = false;
                }
                else
                    follow = follow ? false : true;
            }
        }
        else if (arg == "addgroup")
        {
            std::string group;
            cmdParse->GetNextCommand(&group);
            groups.push_back(group);
            if (state == ServoMode::Mode::Client)
                Sync(servAddr);
        }
        else if (arg == "delgroup")
        {
            bool changed = false;
            std::string group;
            cmdParse->GetNextCommand(&group);
            for (auto it = groups.begin(); it != groups.end();)
            {
                if (*it == group)
                {
                    changed = true;
                    it = groups.erase(it);
                }
                else
                    ++it;
            }
            if (state == ServoMode::Mode::Client && changed)
                Sync(servAddr);
        }
        return true;
    }
    return false;
}

bool Servo::Direct3DInitialize(IDirect3DDevice8* mDevice)
{
    return true;
}

void Servo::Direct3DRender()
{
    if (state.load() == ServoMode::Mode::Client)
    {
        std::string commandtype;
        std::string incommand;
        RecvCommand(&incommand);
        if (!incommand.empty())
        {
            commandtype = incommand.substr(0, incommand.find_first_of(" "));
            incommand = incommand.substr(incommand.find_first_of(" ") + 1);
            if (commandtype != ":SERVOFUNC:")
            {
                auto cmd = incommand;
                trim(cmd);
                m_AshitaCore->GetChatManager()->QueueCommand(cmd.c_str(), (int32_t)Ashita::CommandInputType::Menu);
            }
            else
            {
                if (incommand == "FOLLOWON")
                    follow = true;
                else if (incommand == "FOLLOWOFF")
                    follow = false;
                else if (incommand == "FOLLOW")
                    follow = follow ? false : true;
            }
        }
    }
}

bool Servo::HandleOutgoingPacket(uint16_t id, uint32_t size, void* data, void* modified, bool blocked)
{
    if (id == 0x5E)
        zoning = true;
    return false;
}

bool Servo::HandleIncomingPacket(uint16_t id, uint32_t size, void* data, void* modified, bool blocked)
{
    if (id == 0x0A)
        zoning = false;
    return false;
}

__declspec(dllexport) double __stdcall GetInterfaceVersion(void)
{
    return ASHITA_INTERFACE_VERSION;
}

__declspec(dllexport) void __stdcall CreatePluginInfo(plugininfo_t* lpBuffer)
{
    g_PluginInfo = lpBuffer;

    strcpy_s(g_PluginInfo->Name, sizeof(g_PluginInfo->Name), "Servo");
    strcpy_s(g_PluginInfo->Author, sizeof(g_PluginInfo->Author), "bluekirby0 & atom0s");

    g_PluginInfo->InterfaceVersion = ASHITA_INTERFACE_VERSION;
    g_PluginInfo->PluginVersion = 3.02f;
    g_PluginInfo->Priority = 0;
}

__declspec(dllexport) IPlugin* __stdcall CreatePlugin(void)
{
    return (IPlugin*)new Servo();
}