#include "SGeoFactory.h"

#include "spdlog/spdlog.h"
#include "utils/MIODecoder.h"
#include "GeoCommands.h"
#include "Companion.h"
#include <iostream>

namespace fs = std::filesystem;

enum GeoCommand {
    BRANCH_AND_LINK,
    END,
    BRANCH,
    RETURN,
    OPEN_NODE,
    CLOSE_NODE,
    ASSIGN_AS_VIEW,
    UPDATE_NODE_FLAGS,
    NODE_ROOT,
    NODE_ORTHO_PROJECTION,
    NODE_PERSPECTIVE,
    NODE_START,
    NODE_MASTER_LIST,
    NODE_LEVEL_OF_DETAIL,
    NODE_SWITCH_CASE,
    NODE_CAMERA,
    NODE_TRANSLATION_ROTATION,
    NODE_TRANSLATION,
    NODE_ROTATION,
    NODE_ANIMATED_PART,
    NODE_BILLBOARD,
    NODE_DISPLAY_LIST,
    NODE_SHADOW,
    NODE_OBJECT_PARENT,
    NODE_ASM,
    NODE_BACKGROUND,
    NOP,
    COPY_VIEW,
    NODE_HELD_OBJ,
    NODE_SCALE,
    NOP2,
    NOP3,
    NODE_CULLING_RADIUS,
    // Custom
    BRANCH_AND_LINK_OTR,
    BRANCH_OTR,
    NODE_SWITCH_CASE_OTR,
    NODE_CAMERA_OTR,
    NODE_TRANSLATION_ROTATION_OTR,
    NODE_TRANSLATION_OTR,
    NODE_ROTATION_OTR,
    NODE_ANIMATED_PART_OTR,
    NODE_BILLBOARD_OTR,
    NODE_DISPLAY_LIST_OTR,
    NODE_ASM_OTR,
    NODE_BACKGROUND_OTR,
    NODE_HELD_OBJ_OTR,
    NODE_SCALE_OTR
};

Vec3s gVec3sZero = { 0, 0, 0 };

int16_t *read_vec3s_to_vec3f(Vec3f dst, int16_t *src) {
    dst[0] = next_s16_in_geo_script(&src);
    dst[1] = next_s16_in_geo_script(&src);
    dst[2] = next_s16_in_geo_script(&src);
    return src;
}

int16_t *read_vec3s(Vec3s dst, int16_t *src) {
    dst[0] = next_s16_in_geo_script(&src);
    dst[1] = next_s16_in_geo_script(&src);
    dst[2] = next_s16_in_geo_script(&src);
    return src;
}

int16_t *read_vec3s_angle(Vec3s dst, int16_t *src) {
    dst[0] = next_s16_in_geo_script(&src);
    dst[1] = next_s16_in_geo_script(&src);
    dst[2] = next_s16_in_geo_script(&src);
    return src;
}

void vec3s_copy(Vec3s dest, Vec3s src) {
    dest[0] = src[0];
    dest[1] = src[1];
    dest[2] = src[2];
}

void vec3s_set(Vec3s dest, int16_t x, int16_t y, int16_t z) {
    dest[0] = x;
    dest[1] = y;
    dest[2] = z;
}

int current_tab_size = 0;

static inline void print_cmd(GeoCommand cmd, std::string c){
    if(cmd == GeoCommand::CLOSE_NODE){
        current_tab_size--;
    }

    for(int i = 0; i < current_tab_size; i++){
        std::cout << "\t";
    }

    if(cmd == GeoCommand::OPEN_NODE){
        current_tab_size++;
    }

    std::cout << "GEO_" << c << "()" << std::endl;
}

typedef uint32_t GeoLayout;

bool SGeoFactory::process(LUS::BinaryWriter* writer, YAML::Node& data, std::vector<uint8_t>& buffer) {

    WriteHeader(writer, LUS::ResourceType::Blob, 0);

    auto offset = data["offset"].as<size_t>();
    auto cmd = (uint8_t*) buffer.data() + offset;
    auto processing = true;

    std::vector<GeoLayout> layout;
    auto geo_writer = new LUS::BinaryWriter();

#define GeoWrite(x) layout.insert(layout.end(), {x})

    while(processing){
        auto opcode = cmd[0x00];
        switch (opcode) {
            case CMD_INFO(BRANCH_AND_LINK) {
                geo_writer->Write((uint8_t) BRANCH_AND_LINK_OTR);
                uint32_t ptr = SEGMENT_OFFSET(BSWAP32(cur_geo_cmd_u32(0x04)));
                auto decl = Companion::Instance->GetNodeByAddr(ptr);

                if(decl.has_value()){
                    uint64_t hash = CRC64(std::get<0>(decl.value()).c_str());
                    SPDLOG_INFO("Found geo: 0x{:X} Hash: 0x{:X} Path: {}", ptr, hash, std::get<0>(decl.value()));
                    geo_writer->Write(hash);
                } else {
                    SPDLOG_INFO("Warning: Could not find geo at 0x{:X}", ptr);
                    geo_writer->Write((uint64_t) 0);
                }

                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(BRANCH) {
                geo_writer->Write((uint8_t) BRANCH_OTR);
                geo_writer->Write(cur_geo_cmd_u8(0x01));

                int32_t ptr = SEGMENT_OFFSET(BSWAP32(cur_geo_cmd_u32(0x04)));
                auto decl = Companion::Instance->GetNodeByAddr(ptr);

                if(decl.has_value()){
                    uint64_t hash = CRC64(std::get<0>(decl.value()).c_str());
                    SPDLOG_INFO("Found geo: 0x{:X} Hash: 0x{:X} Path: {}", ptr, hash, std::get<0>(decl.value()));
                    geo_writer->Write(hash);
                } else {
                    SPDLOG_INFO("Warning: Could not find geo at 0x{:X}", ptr);
                    geo_writer->Write((uint64_t) 0);
                }

                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(END)
                geo_writer->Write((uint8_t) END);
                processing = false;
                break;
            case CMD_INFO(RETURN)
                geo_writer->Write((uint8_t) RETURN);
                processing = false;
                break;
            case CMD_NOP(OPEN_NODE)
            case CMD_NOP(CLOSE_NODE)
            case CMD_INFO(ASSIGN_AS_VIEW) {
                geo_writer->Write(ASSIGN_AS_VIEW);
                geo_writer->Write(cur_geo_cmd_s16(0x02));
                break;
            }
            case CMD_INFO(UPDATE_NODE_FLAGS) {
                geo_writer->Write((uint8_t) UPDATE_NODE_FLAGS);
                geo_writer->Write(cur_geo_cmd_u8(0x01));
                geo_writer->Write(cur_geo_cmd_s16(0x02));
                break;
            }
            case CMD_INFO(NODE_ROOT) {
                auto a1 = cur_geo_cmd_s16(0x02);
                auto a2 = cur_geo_cmd_s16(0x04);
                auto a3 = cur_geo_cmd_s16(0x06);
                auto a4 = cur_geo_cmd_s16(0x08);
                auto a5 = cur_geo_cmd_s16(0x0A);
                GeoWrite(GEO_NODE_SCREEN_AREA(a1, a2, a3, a4, a5));
                cmd += 0x0C << CMD_SIZE_SHIFT;
                processing = false;
            }
            case CMD_INFO(NODE_ORTHO_PROJECTION) {
                geo_writer->Write((uint8_t) NODE_ORTHO_PROJECTION);
                geo_writer->Write(cur_geo_cmd_s16(0x02));
                cmd += 0x04 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_PERSPECTIVE) {
                geo_writer->Write((uint8_t) NODE_PERSPECTIVE);
                auto param = cur_geo_cmd_u8(0x01);
                auto fov = cur_geo_cmd_s16(0x02);
                auto near = cur_geo_cmd_s16(0x04);
                auto far = cur_geo_cmd_s16(0x06);

                geo_writer->Write(param);
                geo_writer->Write(fov);
                geo_writer->Write(near);
                geo_writer->Write(far);

                if (param != 0) {
                    auto func = cur_geo_cmd_u32(0x08);
                    geo_writer->Write(func);
                    cmd += 4 << CMD_SIZE_SHIFT;
                }

                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_NOP(NODE_START)
            case CMD_INFO(NODE_MASTER_LIST) {
                geo_writer->Write((uint8_t) NODE_MASTER_LIST);
                geo_writer->Write(cur_geo_cmd_u8(0x01));
                cmd += 0x04 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_LEVEL_OF_DETAIL) {
                geo_writer->Write((uint8_t) NODE_LEVEL_OF_DETAIL);
                int16_t minDistance = cur_geo_cmd_s16(0x04);
                int16_t maxDistance = cur_geo_cmd_s16(0x06);

                geo_writer->Write(minDistance);
                geo_writer->Write(maxDistance);

                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_SWITCH_CASE) {
                geo_writer->Write((uint8_t) NODE_SWITCH_CASE_OTR);
                auto cs = cur_geo_cmd_s16(0x02);
                auto ptr = cur_geo_cmd_u32(0x04);

                geo_writer->Write(cs);
                geo_writer->Write(ptr);

                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_CAMERA) {
                geo_writer->Write((uint8_t) NODE_CAMERA_OTR);
                auto cmd_pos = (int16_t *) &cmd[4];
                auto mode = cur_geo_cmd_s16(0x02);

                geo_writer->Write(mode);

                Vec3f pos, focus;

                cmd_pos = read_vec3s_to_vec3f(pos, cmd_pos);
                read_vec3s_to_vec3f(focus, cmd_pos);

                geo_writer->Write(pos[0]);
                geo_writer->Write(pos[1]);
                geo_writer->Write(pos[2]);
                geo_writer->Write(focus[0]);
                geo_writer->Write(focus[1]);
                geo_writer->Write(focus[2]);

                auto ptr = SEGMENT_OFFSET(BSWAP32(cur_geo_cmd_u32(0x10)));
                geo_writer->Write(ptr);

                cmd += 0x14 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_TRANSLATION_ROTATION) {
                geo_writer->Write(NODE_TRANSLATION_ROTATION_OTR);
                auto params = cur_geo_cmd_u8(0x01);
                auto cmd_pos = (int16_t*) cmd;

                Vec3s translation, rotation;

                geo_writer->Write(params);

                switch ((params & 0x70) >> 4) {
                    case 0:
                        cmd_pos = read_vec3s(translation, &cmd_pos[2]);
                        geo_writer->Write(translation[0]);
                        geo_writer->Write(translation[1]);
                        geo_writer->Write(translation[2]);
                        cmd_pos = read_vec3s_angle(rotation, cmd_pos);
                        geo_writer->Write(rotation[0]);
                        geo_writer->Write(rotation[1]);
                        geo_writer->Write(rotation[2]);
                        break;
                    case 1:
                        cmd_pos = read_vec3s(translation, &cmd_pos[1]);
                        geo_writer->Write(translation[0]);
                        geo_writer->Write(translation[1]);
                        geo_writer->Write(translation[2]);
                        break;
                    case 2:
                        cmd_pos = read_vec3s_angle(rotation, &cmd_pos[1]);
                        geo_writer->Write(rotation[0]);
                        geo_writer->Write(rotation[1]);
                        geo_writer->Write(rotation[2]);
                        break;
                    case 3:
                        cmd_pos += 2 << CMD_SIZE_SHIFT;
                        break;
                }

                if (params & 0x80) {
                    auto dlist = SEGMENT_OFFSET(BSWAP32(*(uint32_t*) &cmd_pos[0]));
                    auto decl = Companion::Instance->GetNodeByAddr(dlist);
                    if(decl.has_value()){
                        uint64_t hash = CRC64(std::get<0>(decl.value()).c_str());
                        SPDLOG_INFO("Found display list: 0x{:X} Hash: 0x{:X} Path: {}", dlist, hash, std::get<0>(decl.value()));
                        geo_writer->Write(hash);
                    } else {
                        SPDLOG_INFO("Warning: Could not find display list at 0x{:X}", dlist);
                        geo_writer->Write((uint64_t) 0);
                    }
                    cmd_pos += 2 << CMD_SIZE_SHIFT;
                }

                cmd = (uint8_t*) cmd_pos;
                break;
            }
            case CMD_INFO(NODE_TRANSLATION) {
                geo_writer->Write(NODE_TRANSLATION_OTR);
                auto params = cur_geo_cmd_u8(0x01);
                auto cmd_pos = (int16_t*) cmd;

                Vec3s translation;
                cmd_pos = read_vec3s(translation, &cmd_pos[1]);

                geo_writer->Write(translation[0]);
                geo_writer->Write(translation[1]);
                geo_writer->Write(translation[2]);

                if (params & 0x80) {
                    auto dlist = SEGMENT_OFFSET(BSWAP32(*(uint32_t*) &cmd_pos[0]));
                    auto decl = Companion::Instance->GetNodeByAddr(dlist);
                    if(decl.has_value()){
                        uint64_t hash = CRC64(std::get<0>(decl.value()).c_str());
                        SPDLOG_INFO("Found display list: 0x{:X} Hash: 0x{:X} Path: {}", dlist, hash, std::get<0>(decl.value()));
                        geo_writer->Write(hash);
                    } else {
                        SPDLOG_INFO("Warning: Could not find display list at 0x{:X}", dlist);
                        geo_writer->Write((uint64_t) 0);
                    }
                    cmd_pos += 2 << CMD_SIZE_SHIFT;
                }

                cmd = (uint8_t*) cmd_pos;
                break;
            }
            case CMD_INFO(NODE_ROTATION) {
                geo_writer->Write(NODE_ROTATION_OTR);
                auto params = cur_geo_cmd_u8(0x01);
                auto cmd_pos = (int16_t*) cmd;

                Vec3s sp2c;
                cmd_pos = read_vec3s(sp2c, &cmd_pos[1]);

                geo_writer->Write(sp2c[0]);
                geo_writer->Write(sp2c[1]);
                geo_writer->Write(sp2c[2]);

                if (params & 0x80) {
                    auto dlist = SEGMENT_OFFSET(BSWAP32(*(uint32_t*) &cmd_pos[0]));
                    auto decl = Companion::Instance->GetNodeByAddr(dlist);
                    if(decl.has_value()){
                        uint64_t hash = CRC64(std::get<0>(decl.value()).c_str());
                        SPDLOG_INFO("Found display list: 0x{:X} Hash: 0x{:X} Path: {}", dlist, hash, std::get<0>(decl.value()));
                        geo_writer->Write(hash);
                    } else {
                        SPDLOG_INFO("Warning: Could not find display list at 0x{:X}", dlist);
                        geo_writer->Write((uint64_t) 0);
                    }
                    cmd_pos += 2 << CMD_SIZE_SHIFT;
                }

                cmd = (uint8_t*) cmd_pos;
                break;
            }
            case CMD_INFO(NODE_ANIMATED_PART) {
                geo_writer->Write(NODE_ANIMATED_PART_OTR);

                auto layer = cur_geo_cmd_u8(0x01);
                auto ptr = cur_geo_cmd_u32(0x08);

                geo_writer->Write(layer);
                geo_writer->Write(ptr);

                auto dlist = SEGMENT_OFFSET(BSWAP32(cur_geo_cmd_u32(0x08)));
                auto decl = Companion::Instance->GetNodeByAddr(dlist);
                if(decl.has_value()){
                    uint64_t hash = CRC64(std::get<0>(decl.value()).c_str());
                    SPDLOG_INFO("Found display list: 0x{:X} Hash: 0x{:X} Path: {}", dlist, hash, std::get<0>(decl.value()));
                    geo_writer->Write(hash);
                } else {
                    SPDLOG_INFO("Warning: Could not find display list at 0x{:X}", dlist);
                    geo_writer->Write((uint64_t) 0);
                }

                cmd += 0x0C << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_BILLBOARD) {
                geo_writer->Write(NODE_BILLBOARD_OTR);

                Vec3s translation;
                auto params = cur_geo_cmd_u8(0x01);
                auto cmd_pos = (int16_t*) cmd;

                cmd_pos = read_vec3s(translation, &cmd_pos[1]);
                geo_writer->Write(params);
                if (params & 0x80) {
                    auto dlist = SEGMENT_OFFSET(BSWAP32(*(uint32_t*) &cmd_pos[0]));
                    auto decl = Companion::Instance->GetNodeByAddr(dlist);
                    if (decl.has_value()) {
                        uint64_t hash = CRC64(std::get<0>(decl.value()).c_str());
                        SPDLOG_INFO("Found display list: 0x{:X} Hash: 0x{:X} Path: {}", dlist, hash,
                                    std::get<0>(decl.value()));
                        geo_writer->Write(hash);
                    } else {
                        SPDLOG_INFO("Warning: Could not find display list at 0x{:X}", dlist);
                        geo_writer->Write((uint64_t) 0);
                    }

                    cmd_pos += 2 << CMD_SIZE_SHIFT;
                }

                cmd = (uint8_t*) cmd_pos;
                break;
            }
            case CMD_INFO(NODE_DISPLAY_LIST) {
                geo_writer->Write((uint8_t) NODE_DISPLAY_LIST_OTR);

                int32_t layer = cur_geo_cmd_u8(0x01);
                auto ptr = SEGMENT_OFFSET(BSWAP32(cur_geo_cmd_u32(0x04)));

                geo_writer->Write(layer);

                auto decl = Companion::Instance->GetNodeByAddr(ptr);

                if(decl.has_value()){
                    uint64_t hash = CRC64(std::get<0>(decl.value()).c_str());
                    SPDLOG_INFO("Found display list: 0x{:X} Hash: 0x{:X} Path: {}", ptr, hash, std::get<0>(decl.value()));
                    geo_writer->Write(hash);
                } else {
                    SPDLOG_INFO("Warning: Could not find display list at 0x{:X}", ptr);
                    geo_writer->Write((uint64_t) 0);
                }

                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_SHADOW) {
                geo_writer->Write((uint8_t) NODE_SHADOW);
                auto type = cur_geo_cmd_s16(0x02);
                auto solidity = cur_geo_cmd_s16(0x04);
                auto scale = cur_geo_cmd_s16(0x06);

                geo_writer->Write(type);
                geo_writer->Write(solidity);
                geo_writer->Write(scale);

                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_NOP(NODE_OBJECT_PARENT)
            case CMD_INFO(NODE_ASM) {
                geo_writer->Write(NODE_ASM_OTR);
                auto param = cur_geo_cmd_s16(0x02);
                auto ptr = cur_geo_cmd_u32(0x04);

                geo_writer->Write(param);
                geo_writer->Write(SEGMENT_OFFSET(BSWAP32(ptr)));
                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_BACKGROUND) {
                geo_writer->Write((uint8_t) NODE_BACKGROUND_OTR);
                auto bg = cur_geo_cmd_s16(0x02);
                geo_writer->Write(bg);

                auto ptr = SEGMENT_OFFSET(BSWAP32(cur_geo_cmd_u32(0x04)));
                geo_writer->Write(ptr);

                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NOP) {
                geo_writer->Write((uint8_t) NOP);
                geo_writer->Write((uint32_t) 0);
                geo_writer->Write((uint32_t) 0);
                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(COPY_VIEW) {
                geo_writer->Write((uint32_t) COPY_VIEW);
                auto idx = cur_geo_cmd_s16(0x02);
                geo_writer->Write(idx);
                break;
            }
            case CMD_INFO(NODE_HELD_OBJ) {
                geo_writer->Write((uint8_t) NODE_HELD_OBJ_OTR);

                auto idx = cur_geo_cmd_u8(0x01);
                geo_writer->Write(idx);

                Vec3s c_offset;
                read_vec3s(c_offset, (int16_t*) &cmd[0x02]);

                geo_writer->Write(c_offset[0]);
                geo_writer->Write(c_offset[1]);
                geo_writer->Write(c_offset[2]);

                auto ptr = SEGMENT_OFFSET(BSWAP32(cur_geo_cmd_u32(0x08)));
                geo_writer->Write(ptr);

                cmd += 0x0C << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_SCALE) {
                geo_writer->Write(NODE_SCALE_OTR);
                auto params = cur_geo_cmd_u8(0x01);
                auto scale = cur_geo_cmd_u32(0x04);

                geo_writer->Write(params);
                geo_writer->Write(scale);

                if (params & 0x80) {
                    auto dlist = SEGMENT_OFFSET(BSWAP32(cur_geo_cmd_u32(0x08)));
                    auto decl = Companion::Instance->GetNodeByAddr(dlist);
                    if(decl.has_value()){
                        uint64_t hash = CRC64(std::get<0>(decl.value()).c_str());
                        SPDLOG_INFO("Found display list: 0x{:X} Hash: 0x{:X} Path: {}", dlist, hash, std::get<0>(decl.value()));
                        geo_writer->Write(hash);
                    } else {
                        SPDLOG_INFO("Warning: Could not find display list at 0x{:X}", dlist);
                        geo_writer->Write((uint64_t) 0);
                    }
                    cmd += 4 << CMD_SIZE_SHIFT;
                }
                break;
            }
            case CMD_INFO(NOP2) {
                geo_writer->Write((uint8_t) NOP2);
                geo_writer->Write((uint32_t) 0);
                geo_writer->Write((uint32_t) 0);
                cmd += 0x08 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NOP3) {
                geo_writer->Write((uint8_t) NOP3);
                geo_writer->Write((uint32_t) 0);
                geo_writer->Write((uint32_t) 0);
                geo_writer->Write((uint32_t) 0);
                cmd += 0x10 << CMD_SIZE_SHIFT;
                break;
            }
            case CMD_INFO(NODE_CULLING_RADIUS) {
                geo_writer->Write((uint8_t) NODE_CULLING_RADIUS);
                auto radius = cur_geo_cmd_s16(0x02);
                geo_writer->Write(radius);
                break;
            }
            default:
                std::cout << "Unknown command: " << std::hex << (int) cmd[0x00] << std::endl;
                break;
        }
    }

    auto size = geo_writer->GetLength();
    auto data_ptr = geo_writer->ToVector();
//    auto s = size + layout.size() * sizeof(GeoLayout);
//    writer->Write(s);
//    writer->Write(data_ptr.data(), size);

    writer->Write((uint32_t) layout.size());
    writer->Write((char*) layout.data(), layout.size() * sizeof(GeoLayout));
    geo_writer->Close();

    return true;
}