mcl

#include <algorithm>
#include <cmath>
#include <vector>
#include <bitset>
#include <random>
//#include "../lemlib/util.hpp"

//things to try on this project in the future
    //gradient decent for particle location

namespace mcl {
    struct sensor {
        double x, y, theta;
        double distance;

        sensor(double x, double y, double theta) : x(x), y(y), theta(theta) {}
    };

    class Robot {
    public:
        std::vector<sensor*> sensors;
        std::vector<std::tuple<double>> particles;
        double x, y, theta;

        Robot(std::vector<sensor*>) : sensors(std::vector<sensor*>()) {}
    };

    class Particle {
    public:
        Particle(double x, double y, double theta, Robot* robot) : x(x), y(y), theta(theta), robot(robot) {}

        double x, y, theta, fitness;
        double last_x, last_y = 0.0;
        double last_theta = 0.0;
        double delta_distance () {return std::hypot(x, y) - std::hypot(last_x, last_y);}
        double delta_angle () {return theta - last_theta;}
        Robot* robot;

        void update (double x_in, double y_in, double theta_in) {
            last_x = x;
            last_y = y;
            last_theta = theta;

            x = x_in;
            y = y_in;
            theta = theta_in;
        }
    };

    class ParticleFilter {
        ParticleFilter(Robot* robot, unsigned int points) : robot(robot), points(points) {
            Particle default_particle(7200, 7200, 0.0f, robot);
            std::vector<Particle> ps(points, default_particle);

            for (int i = 0; i < ps.size(); ++i) {
                mutate_particle(ps[i], 144.0f, 144.0f);
            }
        }

        Robot* robot;

        void predict(double velocity, double angular_velocity, double delta_t) {
        for (auto& particle : particles) {
            // Update theta
            particle.theta += angular_velocity * delta_t;
            normalize_angle(particle.theta);

            // Update position
            particle.x += velocity * std::cos(particle.theta) * delta_t;
            particle.y += velocity * std::sin(particle.theta) * delta_t;
        }
    }

        void update () {
            for (int j = 0; j < ps.size(); ++j) {
                std::vector<double> sensor_predictions = calculated_intercept(ps[j]);

                double loss = 0.0f;
                for (int i = 0; i < sensor_predictions.size(); ++i) {
                    loss += calculate_loss(ps[j].robot->sensors[i]->distance, sensor_predictions[i]);
                }

                ps[j].fitness = loss;
            }

            std::vector<Particle> mc_selection = monte_carlo(ps, points / 5);

            for (int k = 0; k < mc_selection.size(); ++k) {
                for (int m = 0; m < 5; ++m) {
                    ps[k * (points / 5) + m] = mc_selection[k];
                }
            }

            for (int n = 0; n < mc_selection.size(); ++n) {
                mutate_particle(ps[n], 144.0f, 144.0f);
            }
        }

    private:
        unsigned int points;
        std::vector<Particle> ps;

        double compute_distance(double sensor_x, double sensor_y, double sin_theta, double cos_theta) {
        std::vector<double> distances;

        // Check intersection with left wall (x = field_min)
        if (cos_theta != 0) {
            double distance_to_left = (field_min - sensor_x) / cos_theta;
            double y_intersection = sensor_y + distance_to_left * sin_theta;
            if (y_intersection >= field_min && y_intersection <= field_max && distance_to_left > 0) {
                distances.push_back(std::abs(distance_to_left));
            }
        }

        // Check intersection with right wall (x = field_max)
        if (cos_theta != 0) {
            double distance_to_right = (field_max - sensor_x) / cos_theta;
            double y_intersection = sensor_y + distance_to_right * sin_theta;
            if (y_intersection >= field_min && y_intersection <= field_max && distance_to_right > 0) {
                distances.push_back(std::abs(distance_to_right));
            }
        }

        // Check intersection with bottom wall (y = field_min)
        if (sin_theta != 0) {
            double distance_to_bottom = (field_min - sensor_y) / sin_theta;
            double x_intersection = sensor_x + distance_to_bottom * cos_theta;
            if (x_intersection >= field_min && x_intersection <= field_max && distance_to_bottom > 0) {
                distances.push_back(std::abs(distance_to_bottom));
            }
        }

        // Check intersection with top wall (y = field_max)
        if (sin_theta != 0) {
            double distance_to_top = (field_max - sensor_y) / sin_theta;
            double x_intersection = sensor_x + distance_to_top * cos_theta;
            if (x_intersection >= field_min && x_intersection <= field_max && distance_to_top > 0) {
                distances.push_back(std::abs(distance_to_top));
            }
        }

        if (!distances.empty()) {
            return *std::min_element(distances.begin(), distances.end());
        } else {
            return std::numeric_limits<double>::infinity();
        }
    }

        double calculate_loss (double e_distance, double o_distance) {
            return pow(fabs(o_distance - e_distance), 2);
        }

        void resample() {
        // Resample particles based on weights
        std::vector<Particle> new_particles;
        std::vector<double> weights;
        for (const auto& particle : particles) {
            weights.push_back(particle.weight);
        }

        std::discrete_distribution<size_t> distribution(weights.begin(), weights.end());
        std::mt19937 gen(std::random_device{}());

        for (size_t i = 0; i < num_particles; ++i) {
            new_particles.push_back(particles[distribution(gen)]);
        }

        particles = std::move(new_particles);
    }

        void update_weights(double sensor_x, double sensor_y, double sensor_theta) {
        // Precompute sin and cos for the sensor's angle
        double sin_theta = std::sin(sensor_theta);
        double cos_theta = std::cos(sensor_theta);

        for (auto& particle : particles) {
            double distance = compute_distance(sensor_x, sensor_y, sin_theta, cos_theta);
            particle.weight = calculate_loss();
        }
    }

        void my_clean_rads (double angle) {
           while (angle < 0) angle += 2 * M_PI;
            while (angle >= 2 * M_PI) angle -= 2 * M_PI;
        }
    };
    // need completely new filter class
};