I HATE EVERYTHING

#include <stdio.h>
#include <iostream>
#include <map>
#include <malloc.h>
#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"
#include "lwip/sockets.h"
#include "lwip/ip_addr.h"
#include "dhcp.h"

bool network_init(const char *ap_ssid, const char *ap_password, const ip_addr_t &ip, const ip_addr_t &netmask, const ip_addr_t &gateway) {
    if (cyw43_arch_init()) {
        std::cout << "Failed to initialize cyw43" << std::endl;
        return false;
    }

    cyw43_arch_enable_ap_mode(ap_ssid, ap_password, CYW43_AUTH_WPA2_AES_PSK);

    netif_set_ipaddr(cyw43_state.netif, &ip);
    netif_set_netmask(cyw43_state.netif, &netmask);
    netif_set_gw(cyw43_state.netif, &gateway);

    cyw43_state.netif->flags |= NETIF_FLAG_UP;

    if (!(cyw43_state.netif->flags & NETIF_FLAG_UP)) {
        std::cout << "Failed to bring network interface up!" << std::endl;
        return false;
    } else std::cout << "Network interface is up!" << std::endl;
    return true;
}

void network_deinit() {
    cyw43_arch_disable_ap_mode();
    cyw43_arch_deinit();
}

class UDPServer {
public:
    UDPServer(ip_addr_t ip_address) : _ap_addr(ip_address) {
        IP_ADDR4(&_bc_addr, 0xFF, 0xFF, 0xFF, 0xFF);
    }

    bool start() {
        _ap_pcb = udp_new();
        if (_ap_pcb == NULL) {
            std::cout << "Failed to create UDP PCB" << std::endl;
            return false;
        }

        err_t err = udp_bind(_ap_pcb, &_ap_addr, 67);
        if (err != ERR_OK) {
            std::cout << "Failed to bind address" << std::endl;
            return false;
        }

        udp_recv(_ap_pcb, udp_recv_callback, this);
        return true;
    }

private:
    static void udp_recv_callback(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) {
        UDPServer* server = (UDPServer*)arg;
        uint8_t *payload = (uint8_t*)p->payload;
        DHCPPacket *recv_pack = reinterpret_cast<DHCPPacket*>(payload);
        DHCPPacket ret_pack;

        if (!server->handle_dhcp_packet(*recv_pack, ret_pack)) {
            pbuf_free(p);
            return;
        }

        std::vector<uint8_t> ret_pack_vec = ret_pack.convert();
        const uint8_t *ret_data = ret_pack_vec.data();
        size_t ret_data_size = ret_pack_vec.size();

        pbuf *p_ret = pbuf_alloc(PBUF_TRANSPORT, ret_data_size, PBUF_RAM);
        if (p_ret == NULL) {
            std::cout << "Failed to create pbuf" << std::endl;
            pbuf_free(p);
            return;
        }
        std::cout << "pbuf created" << std::endl;
        std::memcpy(p_ret->payload, ret_data, ret_data_size);

        if (cyw43_state.netif == nullptr || !(cyw43_state.netif->flags & NETIF_FLAG_UP)) {
            std::cout << "Network interface is down or not properly initialized." << std::endl;
            pbuf_free(p_ret);
            pbuf_free(p);
            return;
        }

        std::cout << "Size of p_ret: " << ret_data_size << std::endl;
        std::cout << "IP address: " << ip4addr_ntoa(&cyw43_state.netif->ip_addr) << std::endl;
        std::cout << "Network interface status: " << (cyw43_state.netif != nullptr ? "Up" : "Down") << std::endl;
        std::cout << "Network interface flags before sending: " << cyw43_state.netif->flags << std::endl;
        err_t err = udp_sendto_if(server->_ap_pcb, p_ret, &server->_bc_addr, 68, cyw43_state.netif);
        std::cout << "Sending done" << std::endl;
        if (err != ERR_OK) {
            std::cout << "Failed to send, Error code: " << (int)err << std::endl;
            if (cyw43_state.netif == nullptr) std::cout << "Network interface is down." << std::endl;
            else std::cout << "Network interface status: " << (cyw43_state.netif->flags) << std::endl;
        } else std::cout << "Success" << std::endl;

        std::cout << "Success" << std::endl;
        pbuf_free(p_ret);
        pbuf_free(p);
    }

    bool handle_dhcp_packet(const DHCPPacket &recv_pack, DHCPPacket &ret_pack) {
        auto opts = recv_pack.get_opts();

        if (opts.find(53) == opts.end()) {
            std::cout << "Missing option 53" << std::endl;
            return false;
        }
        if (recv_pack.is_discover() && opts[53][0] == 0x01) return handle_dhcp_discover(recv_pack, ret_pack, opts);

        std::cout << "Unknown Package: " << (int)opts[53][0] << std::endl;
        return false;
    }

    bool handle_dhcp_discover(const DHCPPacket &recv_pack, DHCPPacket &ret_pack, const std::map<uint8_t, std::vector<uint8_t>> &opts) {
        std::cout << "DHCP DISCOVER" << std::endl;

        std::map<uint8_t, std::vector<uint8_t>> ret_opts;
        ret_pack.op = BOOTREPLY;
        ret_pack.htype = ETHERNET;
        ret_pack.xid = recv_pack.xid;
        ret_pack.hlen = recv_pack.hlen;
        std::memcpy(ret_pack.chaddr, recv_pack.chaddr, sizeof(recv_pack.chaddr));
        std::memcpy(ret_pack.magic_cookie, recv_pack.magic_cookie, sizeof(recv_pack.magic_cookie));
        ret_pack.yiaddr = 0xC0A80220;

        ret_opts[1] = { 255, 255, 255, 0 }; // SUBNET MASK
        ret_opts[3] = { 192, 168, 2, 1 }; // GATEWAY
        ret_opts[51] = { 0x00, 0x00, 0x01, 0x51 }; // LEASE TIME
        ret_opts[53] = { 0x02 }; // DHCP OFFER
        ret_opts[54] = { 192, 168, 2, 1 }; // SERVER IP

        ret_pack.set_options(ret_opts);
        return true;
    }

    ip_addr_t _ap_addr;
    ip_addr_t _bc_addr;
    udp_pcb *_ap_pcb;
};

int main() {
    stdio_init_all();
    sleep_ms(1000);
    std::cout << "Alive" << std::endl;

    ip_addr_t ap_addr;
    ip_addr_t nm_addr;
    ip_addr_t gw_addr;
    IP4_ADDR(&ap_addr, 192, 168, 2, 1);
    IP4_ADDR(&nm_addr, 255, 255, 255, 0);
    IP4_ADDR(&gw_addr, 192, 168, 2, 1);

    if (!network_init("Fly NET", "12345678", ap_addr, nm_addr, gw_addr)) return -1;
    std::cout << "AP alive" << std::endl;

    UDPServer server(ap_addr);
    server.start();

    while (1) {
        sleep_ms(1);
    }

    network_deinit();
    sleep_ms(1000);
}