#include "Bullet.h"

void Bullet::setup(vector<Node>& _nodes){
    /* setup camera & world */
    camera_start_pos = ofVec3f(0, -3.f, -40.f);
    camera_end_position = glm::vec3(ofRandom(-100, 100), ofRandom(-100, 100), ofRandom(0, 0));
    camera_move_duration = 10.f;
    camera_move_start_time = ofGetElapsedTimef();
    is_camera_moving = false;

    camera.setPosition(camera_start_pos);
	camera.lookAt(ofVec3f(0, 0, 0), ofVec3f(0, -1, 0));
    camera.enableOrtho();
    camera.setScale(0.01);
    camera.setNearClip(-10000);
    camera.setFarClip(10000);   

	world.setup();
	world.setCamera(&camera);
	world.setGravity( ofVec3f(0, 0, 0) );

    for (int i = 0; i < numThreads; ++i) {
        workerThreads.emplace_back(&Bullet::workerThreadFunction, this, i);
    }

    shader.load("shaders/vertex_snap");

    std::cout << workerThreads.size() << std::endl;
}

void Bullet::update(){
    updateRepulsionNode();
    cameraBounds = getCameraBounds(camera);
    world.update();
    if(is_camera_moving){
        float t = (ofGetElapsedTimef() - camera_move_start_time) / camera_move_duration;
        if (t >= 1.0f){
            t = 1.0f;
            is_camera_moving = false;
        }

        float easedT = easeInOutCubic(t);

        glm::vec3 new_position = glm::mix(camera_start_pos, camera_end_position, easedT);
        camera.setPosition(new_position);

        float baseZoom = 0.01f;
        float peakZoom = 0.04f;
        float zoomT = 1.0f - abs(2.0f * easedT - 1.0f);  // Triangle wave using easedT
        float easedZoomT = easeInOutCubic(zoomT);  // Apply easing to the zoom
        float currentZoom = glm::mix(baseZoom, peakZoom, easedZoomT);
        camera.setScale(currentZoom);
    } else {
        //setNewCameraEndpoint();
    }
}

void Bullet::draw(){
    std::lock_guard<std::mutex> lock(shapeMutex);
    float shapes_drawn = 0;
    
    gridSize = calculateGridSize(camera.getScale().x);
    
    camera.begin();
    glEnable( GL_DEPTH_TEST );
    
    ofSetLineWidth(10.f);
    ofDrawGrid(20, 100, true, false, false, true);

    ofNoFill();
    ofSetColor(ofColor::yellow);

    ofNoFill();
    
    ofSetColor(ofColor::orange);
    for(const auto& n : nodes){
        if(checkNodeVisibility(n)){
            ofPushMatrix();
            n.collider->transformGL();
            ofScale(n.scale * 1.4);
            n.col_mesh.draw();
            n.collider->restoreTransformGL();
            ofPopMatrix();
        }
        
    }
    

    ofSetColor(ofColor::white);
    ofFill();
    
    for(const auto& n : nodes){
        
        if(checkNodeVisibility(n)){
            ofPushMatrix();
            shader.begin();
            shader.setUniform1f("gridSize", gridSize);
            shader.setUniform2f("resolution", glm::vec2(1280.0, 960.0));
            shader.setUniformTexture("tex0", n.tex, 0);
            n.collider->transformGL();        
            ofScale(n.scale);
            n.mesh.draw();
            n.collider->restoreTransformGL();
            shader.end();
            ofPopMatrix();
            shapes_drawn++;
        }
    }
    
	camera.end();
	glDisable(GL_DEPTH_TEST);

    

    int totalShapes = nodes.size();
	ofVec3f gravity = world.getGravity();
	stringstream ss;
	ss << "Total Shapes: " << totalShapes << endl;
    ss << "FPS: " << ofGetFrameRate() << endl;
    ss << "Shapes Drawn: " << shapes_drawn << endl;
    ss << "Camera Position: " << camera.getPosition() << std::endl;
    ss << "Camera Scale: " << camera.getScale() << std::endl;
    ss << "Camera Bounds: " << cameraBounds.z << std::endl;
	ofSetColor(255, 255, 255);
	ofDrawBitmapString(ss.str().c_str(), 20, 20);
    now = false;
}

void Bullet::addMesh(ofMesh _mesh, ofMesh _simple_mesh, Node& _node){
    std::lock_guard<std::mutex> lock(shapeMutex);
    // n.tex = _tex;
    float rand = ofRandom(0.01, 0.02);
    glm::vec3 random_szie(rand, rand, rand);
    _node.scale = random_szie;
    ofQuaternion startRot = ofQuaternion(0., 0., 0., PI);
    // ofVec3f target_location = ofVec3f( ofRandom(0, 0), ofRandom(0, 0), -5 );
    ofVec3f start_location = ofVec3f( ofRandom(-3000, 3000), ofRandom(-3000, 3000) -5 );

    ofxBulletCustomShape* s = new ofxBulletCustomShape();
    s->addMesh(_simple_mesh, _node.scale * 1.4, true);
    s->create( world.world, start_location, startRot, 3.);
    s->add();
    s->getRigidBody()->setAngularFactor(btVector3(0, 0, 1));
    s->getRigidBody()->setDamping(btScalar(0.001), btScalar(0.05));
    s->getRigidBody()->setLinearFactor(btVector3(1, 1, 0));
    s->getRigidBody()->setActivationState(DISABLE_DEACTIVATION);
    s->getRigidBody()->setRestitution(btScalar(0.5));
    s->getRigidBody()->setFriction(btScalar(1.0));

    //positions.push_back(target_location);
    
    // Set how the col mesh is drawn!
    _simple_mesh.setMode(OF_PRIMITIVE_LINES);

    _node.collider = s;
    _node.mesh = _mesh;
    _node.col_mesh = _simple_mesh;
    // nodes.push_back(n);
}

void Bullet::workerThreadFunction(int threadId) {
    while (!shouldTerminate) {
        size_t batchSize = shapes.size() / numThreads;
        size_t start = threadId * batchSize;
        size_t end = (threadId == numThreads - 1) ? nodes.size() : start + batchSize;

        updateShapeBatch(start, end);

        // Add a small sleep to prevent busy-waiting
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

void Bullet::updateShapeBatch(size_t start, size_t end) {
    // for (size_t i = start; i < end; ++i) {
    //     std::lock_guard<std::mutex> lock(shapeMutex);
    //     glm::vec3 pos = nodes[i].collider->getPosition();
    //     glm::vec3 direction = glm::vec3(0, 0, -5) - pos;
    //     float dist_sq = glm::length(direction);
    //     glm::vec3 norm_dir = glm::normalize(direction);
    //     nodes[i].collider->applyCentralForce(1.0 * norm_dir);

        // if(now){
        //     std::lock_guard<std::mutex> lock(shapeMutex);
        //     nodes[i].collider->applyCentralForce(glm::vec3(ofRandom(-1, 1), ofRandom(-1, 1), ofRandom(-1, 1)));
        // }

        // if (dist_sq > FORCE_RADIUS_SQ){
        //     glm::vec3 norm_dir = glm::normalize(direction);
        //     std::lock_guard<std::mutex> lock(shapeMutex);
        //     nodes[i].collider->applyCentralForce(0.001 * direction);
        // }


        // btVector3 vel = nodes[i].collider->getRigidBody()->getLinearVelocity();
        // float vel_mag = vel.length();

        // if (vel_mag > 1.0f) {  // Cap at magnitude 1
        //     vel.normalize(); // Normalize the velocity
        //     vel *= 1.0f;     // Scale to cap
        //     nodes[i].collider->getRigidBody()->setLinearVelocity(vel); // Set the capped velocity
        // }

    //}
    for (size_t i = start; i < end; ++i) {
            std::lock_guard<std::mutex> lock(shapeMutex);
            
            glm::vec3 pos = nodes[i].collider->getPosition();
            glm::vec3 target_pos(nodes[i].tsne_position.x, nodes[i].tsne_position.y, -5);
            glm::vec3 direction = target_pos - pos;
            float dist_sq = glm::length(direction);
            glm::vec3 norm_dir = glm::normalize(direction);
            nodes[i].collider->applyCentralForce(1.0f * norm_dir);

            // Apply repulsion force if needed
            // if (shouldApplyRepulsion && i != repulsionNodeIndex) {
            //     const Node& repulsionNode = nodes[repulsionNodeIndex];
            //     glm::vec3 repulsionDir = pos - repulsionNode.collider->getPosition();
            //     float repulsionDist = glm::length(repulsionDir);

            //     if (repulsionDist < repulsionRadius && repulsionDist > 0) {
            //         repulsionDir = glm::normalize(repulsionDir);
            //         float forceMagnitude = repulsionStrength * (1.0f - repulsionDist / repulsionRadius);
            //         glm::vec3 repulsionForce = repulsionDir * forceMagnitude;
            //         nodes[i].collider->applyCentralForce(repulsionForce * 100.0f);
            //     }
            // }
        }
}

float Bullet::easeInOutCubic(float t) {
    return t < 0.5 ? 4 * t * t * t : 1 - pow(-2 * t + 2, 3) / 2;
}

float Bullet::calculateGridSize(float zoom){
    const float ref_zoom = 0.01;
    const float ref_scale = 20;
    float new_grid_size = (ref_zoom * ref_scale) / zoom;
    if(new_grid_size < 5){
        new_grid_size = 5;
    }
    return new_grid_size;
}

void Bullet::setNewCameraEndpoint(){
    camera_move_start_time = ofGetElapsedTimef();
    is_camera_moving = true;
    camera_start_pos = camera.getPosition();
    camera_end_position = glm::vec3(ofRandom(-75, 75), ofRandom(-75, 75), 0.0);
}

glm::vec4 Bullet::getCameraBounds(const ofCamera& cam){
    
    glm::vec3 cam_pos = cam.getPosition();
    float cam_scale = cam.getScale().x;
    float bounds = ofMap(cam_scale, 0.01, 0.2, 10, 240) + 20;
    return glm::vec4(cam_pos.x + bounds, cam_pos.x - bounds, cam_pos.y + bounds, cam_pos.y - bounds);
    
}

bool Bullet::checkNodeVisibility(const Node& n){
    glm::vec3 n_pos = n.collider->getPosition();
    if(n_pos.x < cameraBounds.x && n_pos.x > cameraBounds.y && n_pos.y < cameraBounds.z && n_pos.y > cameraBounds.w ){
        return true;
    } else {
        return false;
    }
}

glm::vec3 Bullet::getCameraPosition(){
    return camera.getPosition();
}

void Bullet::setNodes(vector<Node>& _nodes){
    nodes = _nodes;
}