#pragma once
#include <cstdint>
#include <cmath>
#include <vector>
#include <array>
#include <atomic>

namespace agi {

constexpr double PHI = 1.618033988749895;
constexpr std::array<uint32_t, 20> FIB = {1,1,2,3,5,8,13,21,34,55,89,144,233,377,610,987,1597,2584,4181,6765};

struct State {
    double phi, entropy, energy, drift, tensor;
    uint64_t tick;
    State() : phi(0), entropy(0), energy(0), drift(0), tensor(0), tick(0) {}
};

inline double phi_eq(double x) { return 1.0 / (1.0 + std::abs(x - 1.0)); }

class Core {
public:
    State compute(const double* d, size_t n, uint64_t t) {
        State s; s.tick = t;
        double m = 0, v = 0;
        for(size_t i = 0; i < n; ++i) m += d[i];
        m /= n;
        for(size_t i = 0; i < n; ++i) { double x = d[i] - m; v += x*x; }
        s.phi = phi_eq(m);
        s.entropy = compute_entropy(d, n);
        s.tensor = std::sqrt(v/n);
        s.drift = std::abs(m - 1.0);
        s.energy = s.phi * (1 + s.entropy) * (1 + std::abs(s.tensor));
        return s;
    }

    std::vector<double> evolve(const double* d, size_t n, const State& s, double lr = 0.01) {
        std::vector<double> r(d, d + n);
        for(size_t i = 0; i < n; ++i) {
            double dx = (compute(r.data(), n, s.tick).energy - s.energy) / 1e-5;
            r[i] -= lr * dx;
        }
        return r;
    }

private:
    double compute_entropy(const double* d, size_t n) {
        std::array<int, 256> h{};
        for(size_t i = 0; i < n; ++i) {
            uint8_t b = static_cast<uint8_t>(std::abs(d[i]) * 255);
            h[b]++;
        }
        double e = 0;
        for(int c : h) {
            if(c) {
                double p = c / (double)n;
                e -= p * std::log2(p);
            }
        }
        return e / 8.0;
    }
};

template<typename T>
class Ring {
    std::vector<T> buf;
    std::atomic<size_t> h{0}, t{0}, s{0};
    size_t cap;
public:
    Ring(size_t c) : buf(c), cap(c) {}
    bool push(const T& v) {
        if(s >= cap) return false;
        buf[h.fetch_add(1) % cap] = v;
        s++;
        return true;
    }
    bool pop(T& v) {
        if(s == 0) return false;
        v = buf[t.fetch_add(1) % cap];
        s--;
        return true;
    }
    size_t size() const { return s; }
};

struct Agent {
    std::vector<double> emb;
    State state;
    uint64_t id;
    double fitness;
    bool active;
    Agent() : id(0), fitness(0), active(false) {}
};

class Swarm {
    std::vector<Agent> agents;
    std::atomic<uint64_t> next_id{0};
    Ring<std::vector<double>> ring;
public:
    Swarm(size_t cap) : agents(cap), ring(1000000) {}

    uint64_t create(const std::vector<double>& e, uint64_t t) {
        for(auto& a : agents) {
            if(!a.active) {
                a.emb = e;
                a.id = next_id++;
                a.active = true;
                a.fitness = 0;
                return a.id;
            }
        }
        return 0;
    }

    Agent* get(uint64_t id) {
        for(auto& a : agents)
            if(a.active && a.id == id) return &a;
        return nullptr;
    }

    size_t active_count() const {
        size_t c = 0;
        for(const auto& a : agents) if(a.active) c++;
        return c;
    }

    bool push_emb(const std::vector<double>& e) { return ring.push(e); }
    bool pop_emb(std::vector<double>& e) { return ring.pop(e); }
};

class RT {
    Core core;
    Swarm swarm;
    std::atomic<bool> stop{false};
    std::atomic<uint64_t> tick{0};
public:
    RT(size_t cap = 12000000) : swarm(cap) {}

    void run(uint32_t hz = 1000) {
        auto period = std::chrono::microseconds(1000000 / hz);
        while(!stop) {
            auto start = std::chrono::high_resolution_clock::now();

            uint64_t t = tick++;
            process_agents(t);
            process_ingest(t);

            auto elapsed = std::chrono::high_resolution_clock::now() - start;
            if(elapsed < period)
                std::this_thread::sleep_for(period - elapsed);
        }
    }

    void shutdown() { stop = true; }
    Swarm& get_swarm() { return swarm; }

private:
    void process_agents(uint64_t t) {
        size_t n = swarm.active_count();
        size_t batch = std::min(n, size_t(10000));
        size_t cnt = 0;

        for(size_t i = 0; i < swarm.agents.size() && cnt < batch; ++i) {
            auto* a = &swarm.agents[i];
            if(!a->active) continue;

            a->state = core.compute(a->emb.data(), a->emb.size(), t);
            auto evolved = core.evolve(a->emb.data(), a->emb.size(), a->state);
            a->emb = evolved;
            a->fitness += a->state.energy;
            cnt++;
        }
    }

    void process_ingest(uint64_t t) {
        std::vector<double> e;
        size_t cnt = 0;
        while(swarm.pop_emb(e) && cnt++ < 100) {
            swarm.create(e, t);
        }
    }
};

} // namespace agi