#include "Map.h"
#include "algorithm"

Map::Map(){
    
}

void Map::Setup(){
    scale = 10000;
    isDebug = true;

    json_embeddings = ofLoadJson(jsonPath);
    if(!json_embeddings.is_array()){
        ofLogError() << "JSON is not an array";
        return;
    } else {
        std::cout << "JSON LOADED" << std::endl;
        SetupTSNE();
    }

    mapFbo.allocate(ofGetWindowWidth() / 2, ofGetWindowHeight(), GL_RGB);

    fboImage.allocate(ofGetWindowWidth() / 2, ofGetWindowHeight(), OF_IMAGE_COLOR);

    Setup3D();

    SetupNodes();
}

void Map::Update(std::string& vp_img, bool& is_active){
    time = ofGetElapsedTimef();
    for(auto& n : nodes){
        glm::vec2 offset = n.amplitude * sin(n.speed * time);
        n.offset = glm::vec3(offset.x, offset.y, 0);
    }
    std::cout << cam.getPosition() << std::endl;
    if(is_active){
        has_reached = false;
    }

    if(!is_active)
        MoveCamera(vp_img);
}

void Map::Draw(){
    mapFbo.begin();

    cam.begin();

    ofClear(0, 0, 0, 1);

    ofMatrix4x4 projectionMatrix = cam.getProjectionMatrix();
    ofMatrix4x4 viewMatrix = cam.getModelViewMatrix();
    ofMatrix4x4 mvpMatrix = projectionMatrix * viewMatrix;

    for (auto& n :sortedNodes){
        glm::vec3 node_position = n->position + n->offset;


        if(isFrustum(node_position, 50)){
            n->texture.getTexture().bind();
            ofPushMatrix();
                ofFill();
                n->geom.setPosition(node_position);
                n->geom.draw();
            ofPopMatrix();
            n->texture.getTexture().unbind();
        }
        
    }

    cam.end();

    mapFbo.end();

    mapFbo.readToPixels(fboPixels);

    fboImage.setFromPixels(fboPixels);
}

/*
    Setup texture for each node
*/
void Map::SetupNodes(){
    std::cout << "Setting up nodes.." << std::endl;
    for (auto& node : nodes){
        ofImage img;
        ofPlanePrimitive plane;

        img.load("data/" + node.image_path);
        int imageW = img.getWidth() * 0.1;
        int imageH = img.getHeight() * 0.1;
        int maxWidth = 1;
        int maxHeight = 1;
        float aspectRatio = (float)imageW / (float)imageH;

        // Determine plane dimensions based on aspect ratio and constraints
        float planeW, planeH;
        if (aspectRatio > (float)maxWidth / maxHeight) {
            // Image is wider relative to the max constraints
            planeW = maxWidth;
            planeH = maxWidth / aspectRatio;
        } else {
            // Image is taller relative to the max constraints
            planeH = maxHeight;
            planeW = maxHeight * aspectRatio;
        }

        // Ensure that dimensions do not exceed constraints
        if (planeH > maxHeight) {
            planeH = maxHeight;
            planeW = maxHeight * aspectRatio;
        }
        if (planeW > maxWidth) {
            planeW = maxWidth;
            planeH = maxWidth / aspectRatio;
        }

        plane.set(imageW, imageH);
        plane.setPosition(node.position);
        plane.setResolution(10, 10);
        plane.mapTexCoordsFromTexture(img.getTexture());
        img.getTexture().setTextureWrap(GL_REPEAT, GL_REPEAT);
        plane.setScale(1);

        node.geom = plane;
        node.texture = img;
        node.speed = glm::vec2(ofRandom(0.1, 0.5), ofRandom(0.1, 0.5));
        node.amplitude = glm::vec2(ofRandom(1, 5), ofRandom(1, 5));
        node.phase = glm::vec2(ofRandom(0, TWO_PI), ofRandom(0, TWO_PI));
    }
    std::cout << "Finished setting up nodes!" << std::endl;
}

/*
    Setup TSNE - reads JSON, converts to points, creates nodes & hashmap
*/
void Map::SetupTSNE(){
    for (const auto& entry: json_embeddings) {
        if (entry.contains("vector") && entry["vector"].is_array()) {
            Node n;
            n.foldername = entry["folder"];
            n.image_path =  entry["image"];
            n.isLost = entry["lost"].get<int>();

            std::vector<float> emb;

            for (const auto& value: entry["vector"]){
                if(value.is_number()){
                    emb.push_back(value.get<float>());
                } else {
                    ofLogError() << "Vector value is not a number";
                }
            }

            n.emotion_vector = emb;

            nodes.push_back(n);
            embeddings.push_back(emb);
        }
    }

    std::cout << nodes.size() << std::endl;

    points = tsne.run(embeddings, dims, perplexity, theta, normalise, runManually);

    for (size_t i = 0; i < points.size(); i++){
        const auto& vec = points[i];
        auto& n = nodes[i];
        n.position = (glm::vec3(vec[0] * scale, vec[1] * scale, vec[2] * scale));
        node_hash_map[n.image_path] = &n;
    }

    // Instead of sorting nodes, just create a sorted reference list
    for (auto& node : nodes) {
        sortedNodes.push_back(&node);  // Pointers to the original nodes
    }
    
    SortNodes();  // Sort the references for rendering
}

/*
    Setup 3D environment
*/
void Map::Setup3D(){
    cam.setFov(20);
    cam.removeAllInteractions();
}

/*
    Query hashmap
*/
bool Map::SearchMap(std::string& search_string){
    std::string t = search_string;
    if(node_hash_map.find(t) != node_hash_map.end()){
        Node* n = node_hash_map[t];
        std::cout << n->foldername << std::endl;
        return true;
    } else {
        return false;
    }
}

/* search if image is in the hashmap -> if true, move to its position */
void Map::MoveCamera(std::string& vp_img){
    if(SearchMap(vp_img)){
        Node* n = node_hash_map[vp_img];
        glm::vec3 cur_pos(cam.getPosition().x, cam.getPosition().y, cam.getPosition().z);
        glm::vec3 target_pos((n->position.x), (n->position.y), (n->position.z) + 100);
        glm::vec3 dir = target_pos - cur_pos;
        float dist = glm::length(dir);
        dir = glm::normalize(dir);
        accel = dir * 0.02f;
        vel += accel;

        // Define minimum and maximum velocities based on distance
        const float minVelocity = 0.01f; // Minimum velocity when close to the target
        const float maxVelocityDistance = 1000.0f; // Distance at which max velocity is applied
        const float maxVelocity = 50.0f; // Maximum velocity

        // Scale max velocity based on distance
        float dynamicMaxVelocity = glm::clamp(dist / maxVelocityDistance * maxVelocity, minVelocity, maxVelocity);

        // Clamp the velocity to the range [-dynamicMaxVelocity, dynamicMaxVelocity]
        float velocityLength = glm::length(vel);
        if (velocityLength > dynamicMaxVelocity) {
            vel = glm::normalize(vel) * dynamicMaxVelocity;
        }

        // Update position
        glm::vec3 newPosition = cur_pos + vel;
        

        if(cam.getPosition() == target_pos){
            has_reached = true;
        }

        if(!has_reached){
            cam.setPosition(target_pos);
        }

        std::cout << "velocity " << vel << std::endl;
        std::cout << "target " << target_pos << std::endl;
        std::cout << "new position " << cam.getPosition() << std::endl;
    }

}

void Map::buttonPressed(int key){
    if(key==OF_KEY_LEFT){
        z_adj -= 1;
    } else {
        z_adj += 1;
    }
}

/*
    Sort nodes by z position, for draw calls (lowest -> highest)
*/
void Map::SortNodes(){
    std::sort(sortedNodes.begin(), sortedNodes.end(), [](const Node* a, const Node* b){
        return a->position.z < b->position.z;  // Sorting by z-position
    });
}

bool Map::isFrustum(const glm::vec3& position, float radius){
    float box_w = 2000;
    float box_h = 2000;
    glm::vec3 cam_position = cam.getPosition();

    float left = cam_position.x - box_w / 2.0f;
    float right = cam_position.x + box_w / 2.0f;
    float top = cam_position.y + box_h / 2.0f;
    float bottom = cam_position.y - box_h / 2.0f;

    // Check if the object (with radius) is within the bounding box
    if (position.x + radius < left || position.x - radius > right) return false;
    if (position.y + radius < bottom || position.y - radius > top) return false;

    // Z-depth doesn't matter in this approach, so we're ignoring it
    return true;
}