OrdonnancementCorrectif/src/OrdoCorr/Solver/TrackPlansILP/SimulatedAnnealing.cpp

#include "SimulatedAnnealing.hpp"
#include "../PseCarlierRivreau/omp.h"
#include "DynamicProgramming.hpp"
#include "Penalty.hpp"
#include "Solution.hpp"
#include "TrackPlan.hpp"
#include <algorithm>
#include <array>
#include <cmath>
#include <ctime>
#include <iterator>
#include <optional>
#include <random>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include "Random.hpp"
#include "sourceSolTrPlan.hpp"


namespace solverlib {
    using namespace random;

    bool StatSimulatedAnnealing::activate = true;
    bool SimulatedAnnealing::withDynProg = false;
    bool SimulatedAnnealing::authorizeInfeasible = true;

    constexpr double decreaseFunction(double p){return std::exp(-0.75*(1.0-p));};

    std::unordered_map<EMovingOperators, std::string> StatSimulatedAnnealing::names = {
        {EMovingOperators::CHANGE_MODE, "CHANGE_MODE"},
        {EMovingOperators::INSERT, "INSERT"},
        {EMovingOperators::REMOVE, "REMOVE"},
        {EMovingOperators::SWAP, "SWAP"},
        {EMovingOperators::SWAP_WITHIN_INTERVAL, "SWAP_WITHIN_SEQUENCE"},
        {EMovingOperators::DYN_PROG, "DYN_PROG"},
        {EMovingOperators::MOVE, "MOVE"}
    };


    SimulatedAnnealing::SimulatedAnnealing(std::unordered_map<unsigned short, Decision>& decs, std::shared_ptr<modellib::STFMockInstance> mock, ESourceTrackPlan source)
    {
        randomEngine = solverlib::random::makeEngine();
        maxCostOfJob = (*std::max_element(STFMockInstance::jobs.begin(), STFMockInstance::jobs.end(), [&](auto op1, auto op2){return op1->getPoidsRetard() < op2->getPoidsRetard();}))->getPoidsRetard();
        setPenaltyWeights();
        addSolutionToPool(decs, mock, source);
        for (unsigned int i = 0; i< STFMockInstance::machines.size(); ++i)
            solutions[0].penaltyPerMachine[i] = Penalty{};
    }

    void SimulatedAnnealing::addSolutionToPool(std::unordered_map<unsigned short, Decision>& decs, std::shared_ptr<modellib::STFMockInstance> mock, ESourceTrackPlan source, bool isPutFirst)
    {
        auto costs = evaluate(decs);
        if(!isPutFirst)
            solutions.push_back({source, mock, decs, costs.first, costs.second});
        else
        {
            solutions.insert(solutions.begin(), {source, mock, decs, costs.first, costs.second});
        }
    }


    std::pair<unsigned int, unsigned int> SimulatedAnnealing::evaluate(const std::unordered_map<unsigned short, Decision>& decs)
    {
        unsigned int cost = 0;
        unsigned int diagCost = 0;
        for(auto& dec : decs)
        {
            if(dec.second.excluded)
            {
                cost += MAXIMUM_TIME_OFFSET * modellib::STFMockInstance::jobs[dec.first]->getPoidsRetard();
            }
            else
                cost += modellib::STFMockInstance::jobs[dec.first]->getPoidsRetard() * dec.second.lastCreneau.first;

            if(dec.second.rejected)
            {
                diagCost += modellib::STFMockInstance::jobs[dec.first]->getPoidsRejet();
            }
        }
        return {cost, diagCost};
    }

    EMovingOperators SimulatedAnnealing::pick_operator() {
        double p = progress();

        float w_remove = static_cast<float>(2*std::max(0.0,1.0 - decreaseFunction(p)));
        float w_insert = static_cast<float>(2*std::min(1.0, decreaseFunction(p)));
        std::vector<float> base_weights = {
            1,   
            w_insert,   
            w_remove,
            1,   
            1,   
            1    
            // DYN_PROG ajouté si besoin
        };
        if (withDynProg) base_weights.push_back(1.0f); // DYN_PROG

        return static_cast<EMovingOperators>(
            std::discrete_distribution<int>(base_weights.begin(), base_weights.end())(randomEngine)
        );
    }

    // Applique un opérateur et retourne un voisin (nullopt si infaisable)
    std::optional<SASolution> SimulatedAnnealing::apply_operator(const SASolution& current, EMovingOperators op)
    {
        switch (op)
        {
            case EMovingOperators::SWAP:                    return move_swap_WC(current);
            case EMovingOperators::INSERT:                  return move_insert_WC(current);
            case EMovingOperators::REMOVE:                  return move_remove_WC(current);
            case EMovingOperators::CHANGE_MODE:             return move_change_mode_WC(current);
            case EMovingOperators::SWAP_WITHIN_INTERVAL:                    return move_swap_within_interval(current);
            case EMovingOperators::MOVE:                                    return move_move_WC(current);
            case EMovingOperators::DYN_PROG:                                return move_dynprog(current);                       
              break;
            }
        return std::nullopt;
    }

    // Boucle principale
    SASolution SimulatedAnnealing::solve(double T_max, double T_min, double cooling_rate, int iterations_per_temp)
    {
        SASolution current  = solutions[0];
        SASolution best     = solutions[0];
        current.fictiveCost = current.cost;
        best.fictiveCost = current.cost;

        Tmax = T_max;
        double   cost_cur = current.cost;
        double   cost_best= cost_cur;
        T = Tmax;
        Tmin = T_min;
        rate = cooling_rate;
        std::uniform_real_distribution<double> uniform(0.0, 1.0);

        while (T > Tmin + 10e-6)
        {
            for (int i = 0; i < iterations_per_temp; ++i)
            {
                EMovingOperators op = pick_operator();
                stats.addUsed(op);
                auto neighbor = apply_operator(current, op);

                if (!neighbor.has_value()) continue;

                auto costs_neighbor = std::make_pair(neighbor->cost, neighbor->diagCost);

                double delta = effectiveCost(*neighbor) - effectiveCost(current);
                double diff = (double)neighbor->cost - (double)current.cost;

                if(neighbor->penalty.isFeasible())
                    stats.addFeas(op);

                if(delta <= 0)
                    stats.addImproved(op);

                if(diff <= 0 && neighbor->penalty.isFeasible())
                    stats.addImprovedReal(op);

                if(!neighbor->penalty.isFeasible())
                    stats.addInfeasible(op);
            
                if(delta > 0)
                {
                    stats.addFailInfo(op, getP(delta, op), diff, delta, T, neighbor->penalty.isFeasible());   
                }
                //TODO AJOUTER AUTORISER AVEC PENALITE LES INFEASABLES => NECESSITE DE METTRE À JOUR LA GENERATION TRACK PLAN POUR LE ILP ET IGNORER LES TRACKPLANS INF

                if (delta < 0 || uniform(randomEngine) < getP(delta, op))
                {
                    current  = std::move(*neighbor);
                    cost_cur = costs_neighbor.first;

                    if (current.penalty.isFeasible() && cost_cur < cost_best) {
                        best      = current;
                        cost_best = cost_cur;
                    }

                    solutions.push_back(current);
                }
            }

            T *= cooling_rate;
            //loggerlib::Logger::systemNotify(loggerlib::LOGGER_PROGRESS, "Temperature : " + std::to_string(T));
            //loggerlib::Logger::systemNotify(loggerlib::LOGGER_PROGRESS, "Solution pool : " + std::to_string(solutions.size()));
        }
        //loggerlib::Logger::systemNotify(loggerlib::LOGGER_PROGRESS, "Best solution : " + std::to_string(best.cost));

        return best;
    }

    double SimulatedAnnealing::effectiveCost(const SASolution& s) const {
        return s.fictiveCost + s.penalty.weighted(effectiveLambdas());
    }

    std::unordered_map<EPenaltyType, double> SimulatedAnnealing::effectiveLambdas() const {
        double p = progress();
        return {
            {EPenaltyType::TIME_WINDOW_OVERRUN,  std::exp(8*(p-0.2))-0.8},
        };
    }

    double SimulatedAnnealing::progress() const {
        //return 1.0 - (T - Tmin) / (Tmax - Tmin); // 0 au début, 1 à la fin
        return 1.0 - (std::log(T) - std::log(Tmin)) / (std::log(Tmax) - std::log(Tmin));
    }

    double SimulatedAnnealing::fictiveCostExcluded(unsigned short op_id) const {
        return decreaseFunction(progress()) * MAXIMUM_TIME_OFFSET
            * STFMockInstance::jobs[op_id]->getPoidsRetard();
    }


    double SimulatedAnnealing::getP(double delta, EMovingOperators op)
    {
        switch (op) {
            case EMovingOperators::SWAP:
            case EMovingOperators::INSERT: return std::exp(-delta/(T*50));
            case EMovingOperators::REMOVE: return std::exp(-delta/(T*50));
            case EMovingOperators::CHANGE_MODE:
            case EMovingOperators::SWAP_WITHIN_INTERVAL:
            case EMovingOperators::MOVE: return std::exp(-delta/(T*20));
            case EMovingOperators::DYN_PROG:
          break;
        }
        return std::exp(-delta/T);
    }

    //OPERATEURS

    std::optional<SASolution> SimulatedAnnealing::move_dynprog(const SASolution& sol)
    {
        DynamicProgramming prog(sol);
        prog.mode = true;
        prog.saveSols = false;
        auto result = prog.solve();
        return std::optional<SASolution>(result);
    }

    std::optional<SASolution> SimulatedAnnealing::move_swap_within_interval(const SASolution& sol)
    {
        auto mock = sol.mock;
        if (!mock) return std::nullopt;

        std::vector<unsigned short> active_ops;
        for (auto& [op_id, dec] : sol.decisions)
            if (!dec.excluded) active_ops.push_back(op_id);
        if (active_ops.size() < 2) return std::nullopt;

        std::uniform_int_distribution<int> dist(0, (int)active_ops.size() - 1);
        unsigned short op_a = active_ops[dist(randomEngine)];
        const Decision& dec_a = sol.decisions.at(op_a);

        std::vector<unsigned short> seq_swap(1, op_a);

        for (auto& op_id : active_ops) {
            if (op_id == op_a) continue;
            const Decision& dec_b = sol.decisions.at(op_id);
            if (dec_b.empV != dec_a.empV) continue;

            seq_swap.push_back({
                op_id
            });
        }
        if (seq_swap.size() == 1) return std::nullopt;

        std::sort(seq_swap.begin(), seq_swap.end(), [&](auto job1, auto job2){
            const Decision& dec_a = sol.decisions.at(job1);
            const Decision& dec_b = sol.decisions.at(job2);

            return dec_a.lastCreneau.first < dec_b.lastCreneau.first;
        });

        auto posA = std::find(seq_swap.begin(), seq_swap.end(), op_a);
        if(posA == seq_swap.end()) return std::nullopt;
        
        auto IposA = std::distance(seq_swap.begin(), posA);

        // Tirer un index différent de IposA pour le swap
        std::uniform_int_distribution<int> posR(0, (int)seq_swap.size() - 1);
        int posB = posR(randomEngine);
        while (posB == IposA)
            posB = posR(randomEngine);

        auto seqCop = seq_swap;
        std::swap(seqCop[IposA], seqCop[posB]);

        // Reconstruire le vecteur de décisions dans le bon ordre
        std::vector<std::pair<unsigned short, Decision>> jobsSeq;
        jobsSeq.reserve(seqCop.size());
        for (auto& job_id : seqCop)
            jobsSeq.push_back({job_id, sol.decisions.at(job_id)});    

        Penalty oldPen = sol.penaltyPerMachine.count(dec_a.empV)
            ? sol.penaltyPerMachine.at(dec_a.empV)
            : Penalty{};

        unsigned int oldCost = 0;
        for (auto& job_id : seq_swap)
            oldCost += STFMockInstance::jobs[job_id]->getPoidsRetard()
                    * sol.decisions.at(job_id).lastCreneau.first;

        Penalty newPen; bool feasible = true;
        auto res = checkSequence(jobsSeq, dec_a.empV, newPen, feasible);
        if (!res) return std::nullopt;

        // Construire le voisin
        SASolution neighbor = sol;
        unsigned int newCost = 0;
        applySequenceResult(neighbor, jobsSeq, *res, newCost, newPen, feasible);

        neighbor.penalty          = sol.penalty - oldPen + newPen;
        neighbor.penaltyPerMachine[dec_a.empV] = newPen;
        neighbor.isFeasible       = neighbor.penalty.isFeasible();
        neighbor.fictiveCost      = (neighbor.fictiveCost - oldCost) + newCost;
        neighbor.cost             = (neighbor.cost        - oldCost) + newCost;
        neighbor.source           = ESourceTrackPlan::SimAn;
        return neighbor;
    }

    std::optional<SASolution> SimulatedAnnealing::move_swap_WC(const SASolution& sol)
    {
        auto mock = sol.mock;
        if (!mock) return std::nullopt;

        std::vector<unsigned short> active_ops;
        for (auto& [op_id, dec] : sol.decisions)
            if (!dec.excluded) active_ops.push_back(op_id);
        if (active_ops.size() < 2) return std::nullopt;

        std::uniform_int_distribution<int> dist(0, (int)active_ops.size() - 1);
        unsigned short op_a = active_ops[dist(randomEngine)];
        const Decision& dec_a = sol.decisions.at(op_a);

        struct SwapCandidate {
            unsigned short op_a;
            unsigned short op_b;
            unsigned int   empR_a_new; // empR que prendra op_a après le swap
            unsigned int   empR_b_new;
        };
        std::vector<SwapCandidate> swap_candidates;

        // Chercher une op dans une EmpV différent compatible
        //pair < op, empR post swap>
        for (auto& op_id : active_ops) {
            if (op_id == op_a) continue;
            const Decision& dec_b = sol.decisions.at(op_id);
            if (dec_b.empV == dec_a.empV) continue;

            // Vérifier compatibilité infrastructure
            if (!STFMockInstance::infraComp[op_a][dec_b.voie]) continue;
            if (!STFMockInstance::infraComp[op_id][dec_a.voie]) continue;
            unsigned short empRA = 0;
            unsigned short empRB = 0;
            auto findDispOpA = std::find_if(mock->jobDispoVoiesRames[op_a].begin(), mock->jobDispoVoiesRames[op_a].end(), [&](auto& el){
                return mock->dispoVoiesRames[el].dispoVoie == dec_b.empV;
            });
            auto findDispOpB = std::find_if(mock->jobDispoVoiesRames[op_id].begin(), mock->jobDispoVoiesRames[op_id].end(), [&](auto& el){
                return mock->dispoVoiesRames[el].dispoVoie == dec_a.empV;
            });
            if(findDispOpA != mock->jobDispoVoiesRames[op_a].end() && findDispOpB != mock->jobDispoVoiesRames[op_id].end())
            {
                empRA = mock->dispoVoiesRames[*findDispOpA].dispoRame;
                empRB = mock->dispoVoiesRames[*findDispOpB].dispoRame;
            }
            else
                continue;

            swap_candidates.push_back({
                op_a, op_id, empRA,empRB
            });
        }
        if (swap_candidates.empty()) return std::nullopt;

        // Tirer deuxième op parmi les candidats
        std::uniform_int_distribution<int> cand_dist(0, (int)swap_candidates.size() - 1);
        auto swap = swap_candidates[cand_dist(randomEngine)];
        const Decision& dec_b = sol.decisions.at(swap.op_b);

        // échanger tracks et slots
        SASolution neighbor = sol;
        Decision& new_dec_a = neighbor.decisions[op_a];
        Decision& new_dec_b = neighbor.decisions[swap.op_b];

        new_dec_a.voie        = dec_b.voie;
        new_dec_a.site        = dec_b.site;
        new_dec_a.empV        = dec_b.empV;
        new_dec_a.empR        = swap.empR_a_new;
        new_dec_a.timeslotGraphSplited = mock->trajectoryStops[swap.empR_a_new].getDispoStop();
        new_dec_a.lastCreneau = {0,0};

        new_dec_b.voie        = dec_a.voie;
        new_dec_b.site        = dec_a.site;
        new_dec_b.empV        = dec_a.empV;
        new_dec_b.empR        = swap.empR_b_new;
        new_dec_b.timeslotGraphSplited = mock->trajectoryStops[swap.empR_b_new].getDispoStop();
        new_dec_b.lastCreneau = {0,0};

        auto oldCrenA = dec_a.lastCreneau;
        auto oldCrenB = dec_b.lastCreneau;
        
        // Pénalités anciennes O(1)
        Penalty oldPenA = sol.penaltyPerMachine.count(dec_a.empV)
            ? sol.penaltyPerMachine.at(dec_a.empV) : Penalty{};
        Penalty oldPenB = sol.penaltyPerMachine.count(dec_b.empV)
            ? sol.penaltyPerMachine.at(dec_b.empV) : Penalty{};

        std::vector<std::pair<unsigned short, decision>> jobsEmpVA;
        std::vector<std::pair<unsigned short, decision>> jobsEmpVB;
        unsigned int oldCost = dec_a.lastCreneau.first * STFMockInstance::jobs[op_a]->getPoidsRetard() + dec_b.lastCreneau.first * STFMockInstance::jobs[swap.op_b]->getPoidsRetard();
        for(auto& dec : neighbor.decisions)
        {
            if(!dec.second.excluded)
            {
                if(dec.second.empV == new_dec_a.empV)
                {
                    jobsEmpVA.push_back({dec.first, dec.second});
                    oldCost += dec.second.lastCreneau.first * STFMockInstance::jobs[dec.first]->getPoidsRetard();
                }
                if(dec.second.empV == new_dec_b.empV)
                {
                    jobsEmpVB.push_back({dec.first, dec.second});
                    oldCost += dec.second.lastCreneau.first * STFMockInstance::jobs[dec.first]->getPoidsRetard();
                }
            }
        }
        std::sort(jobsEmpVA.begin(), jobsEmpVA.end(), [&](auto& el1, auto& el2){
            auto cren1 = el1.second.lastCreneau;
            auto cren2 = el2.second.lastCreneau;
            if(cren1.first == 0 && cren1.second == 0)
            {
                cren1 = oldCrenB;
            }
            if(cren2.first == 0 && cren2.second == 0)
            {
                cren2 = oldCrenB;
            }
            return cren1.first < cren2.first;
        });

        std::sort(jobsEmpVB.begin(), jobsEmpVB.end(), [&](auto& el1, auto& el2){
            auto cren1 = el1.second.lastCreneau;
            auto cren2 = el2.second.lastCreneau;
            if(cren1.first == 0 && cren1.second == 0)
            {
                cren1 = oldCrenA;
            }
            if(cren2.first == 0 && cren2.second == 0)
            {
                cren2 = oldCrenA;
            }
            return cren1.first < cren2.first;
        });

        Penalty penA; Penalty penB;
        bool feasA = true, feasB = true;
        auto resA = checkSequence(jobsEmpVA, new_dec_a.empV, penA, feasA);
        auto resB = checkSequence(jobsEmpVB, new_dec_b.empV, penB, feasB);
        unsigned int newCost = 0;
        if(!resA || !resB) return std::nullopt;

        applySequenceResult(neighbor, jobsEmpVA, *resA, newCost, penA, feasA);
        applySequenceResult(neighbor, jobsEmpVB, *resB, newCost, penB, feasB);

        neighbor.penalty                    = sol.penalty - oldPenA - oldPenB + penA + penB;
        neighbor.penaltyPerMachine[new_dec_a.empV] = penA;
        neighbor.penaltyPerMachine[new_dec_b.empV] = penB;
        neighbor.isFeasible                 = neighbor.penalty.isFeasible();
        neighbor.fictiveCost                = (neighbor.fictiveCost - oldCost) + newCost;
        neighbor.cost                       = (neighbor.cost        - oldCost) + newCost;
        neighbor.source                     = ESourceTrackPlan::SimAn;
        return neighbor;
    }

    std::optional<SASolution> SimulatedAnnealing::move_insert_WC(const SASolution& sol)
    {
        auto mock = sol.mock;
        if (!mock) return std::nullopt;

        std::vector<unsigned short> inactive_ops;
        for (auto& [op_id, dec] : sol.decisions)
            if (dec.excluded) inactive_ops.push_back(op_id);
        if (inactive_ops.empty()) return std::nullopt;

        std::uniform_int_distribution<int> dist(0, (int)inactive_ops.size() - 1);
        unsigned short op_a = inactive_ops[dist(randomEngine)];

        std::vector<unsigned int> dispCandidate;
        for (auto& disp : mock->jobDispoVoiesRames[op_a]) {

            auto dur = mock->dispoVoiesRames[disp].match.second - mock->dispoVoiesRames[disp].match.first;
            if(dur >= STFMockInstance::jobs[op_a]->getDuree())
            {
                dispCandidate.push_back(disp);
            }
            else if(dur >= STFMockInstance::jobs[op_a]->getDureeDiag() && sol.diagCost + STFMockInstance::jobs[op_a]->getPoidsRejet() <= configlib::Configuration::Global.EPSILON)
            {
                dispCandidate.push_back(disp);
            }
        }
        if(dispCandidate.empty()) return std::nullopt;

        std::uniform_int_distribution<int> cand_dist(0, (int)dispCandidate.size() - 1);
        auto disp = dispCandidate[cand_dist(randomEngine)];

        //Construire le voisin - try insert
        SASolution neighbor = sol;
        Decision& new_dec_a = neighbor.decisions[op_a];
        auto dur = mock->dispoVoiesRames[disp].match.second - mock->dispoVoiesRames[disp].match.first;

        new_dec_a.rejected = dur >= STFMockInstance::jobs[op_a]->getDuree() ? false : true,
        new_dec_a.excluded = false;
        new_dec_a.voie        = mock->dispoVoiesRames[disp].voie;
        new_dec_a.site        = mock->dispoVoiesRames[disp].site;
        new_dec_a.empV        = mock->dispoVoiesRames[disp].dispoVoie;
        new_dec_a.empR        = mock->dispoVoiesRames[disp].dispoRame;
        new_dec_a.timeslotGraphSplited = mock->trajectoryStops[new_dec_a.empR].getDispoStop();
        new_dec_a.lastCreneau = {0,0};
        
        // Pénalité ancienne O(1) — séquence sans op_a
        Penalty oldPen = sol.penaltyPerMachine.count(new_dec_a.empV)
            ? sol.penaltyPerMachine.at(new_dec_a.empV) : Penalty{};

        unsigned int oldDiagCost = 0;
        unsigned int oldCostScheduled = 0;
        unsigned int newDiagCost = new_dec_a.rejected ? STFMockInstance::jobs[op_a]->getPoidsRejet() : 0;
        std::vector<std::pair<unsigned short, decision>> jobsEmpVA;
        for(auto& dec : neighbor.decisions)
        {
            if(!dec.second.excluded && dec.first != op_a)
            {
                if(dec.second.empV == new_dec_a.empV)
                {
                    jobsEmpVA.push_back({dec.first, dec.second});
                    oldCostScheduled += dec.second.lastCreneau.first * STFMockInstance::jobs[dec.first]->getPoidsRetard();
                }
            }
        }

        std::sort(jobsEmpVA.begin(), jobsEmpVA.end(), [&](auto& el1, auto& el2){
            auto cren1 = el1.second.lastCreneau;
            auto cren2 = el2.second.lastCreneau;
            return cren1.first < cren2.first;
        });

        std::uniform_int_distribution<unsigned int> distPos(0,jobsEmpVA.size());
        auto pos = distPos(randomEngine);
        
        auto seq = jobsEmpVA;
        if(pos == jobsEmpVA.size())
        {
            seq.insert(seq.end(), std::make_pair(op_a, new_dec_a));
        }
        else {
            seq.insert(seq.begin() + pos, std::make_pair(op_a, new_dec_a));
        }
        
        unsigned int newCost = 0;
        Penalty penA; bool feasA = true;
        auto res = checkSequence(seq, new_dec_a.empV, penA, feasA);
        
        if(!res) return std::nullopt;

        applySequenceResult(neighbor, seq, *res, newCost, penA, feasA);

        double oldFictive = sol.fictiveExcludedCosts.count(op_a)
            ? fictiveCostExcluded(op_a)
            : MAXIMUM_TIME_OFFSET * STFMockInstance::jobs[op_a]->getPoidsRetard();

        unsigned int oldCostExcluded = MAXIMUM_TIME_OFFSET * STFMockInstance::jobs[op_a]->getPoidsRetard();

        neighbor.penalty                       = sol.penalty - oldPen + penA;
        neighbor.penaltyPerMachine[new_dec_a.empV] = penA;
        neighbor.isFeasible                    = neighbor.penalty.isFeasible();
        neighbor.fictiveCost                   = (neighbor.fictiveCost - oldFictive - oldCostScheduled) + newCost;
        neighbor.fictiveExcludedCosts.erase(op_a);
        neighbor.diagCost                      = (neighbor.diagCost - oldDiagCost) + newDiagCost;
        neighbor.cost                          = (neighbor.cost - oldCostExcluded - oldCostScheduled) + newCost;
        neighbor.source                        = ESourceTrackPlan::SimAn;

        return neighbor;        
    }

    std::optional<SASolution> SimulatedAnnealing::move_move_WC(const SASolution& sol)
    {
        auto mock = sol.mock;
        if (!mock) return std::nullopt;

        std::vector<unsigned short> active_ops;
        for (auto& [op_id, dec] : sol.decisions)
            if (!dec.excluded) active_ops.push_back(op_id);
        if (active_ops.empty()) return std::nullopt;

        std::uniform_int_distribution<int> dist(0, (int)active_ops.size() - 1);
        unsigned short op_a = active_ops[dist(randomEngine)];

        std::vector<unsigned int> dispCandidate;
        const Decision& dec_a = sol.decisions.at(op_a);

        for (auto& disp : mock->jobDispoVoiesRames[op_a]) {
            if(dec_a.empV == mock->dispoVoiesRames[disp].dispoVoie)
                continue;

            auto dur = mock->dispoVoiesRames[disp].match.second - mock->dispoVoiesRames[disp].match.first;
            if(dur >= STFMockInstance::jobs[op_a]->getDuree())
            {
                dispCandidate.push_back(disp);
            }
            else if(dur >= STFMockInstance::jobs[op_a]->getDureeDiag() && sol.diagCost + STFMockInstance::jobs[op_a]->getPoidsRejet() <= configlib::Configuration::Global.EPSILON)
            {
                dispCandidate.push_back(disp);
            }
        }
        if(dispCandidate.empty()) return std::nullopt;

        std::uniform_int_distribution<int> cand_dist(0, (int)dispCandidate.size() - 1);
        auto disp = dispCandidate[cand_dist(randomEngine)];

        //Construire le voisin - try insert
        SASolution neighbor = sol;
        Decision& new_dec_a = neighbor.decisions[op_a];
        auto dur = mock->dispoVoiesRames[disp].match.second - mock->dispoVoiesRames[disp].match.first;

        new_dec_a.rejected = dur >= STFMockInstance::jobs[op_a]->getDuree() ? false : true,
        new_dec_a.voie        = mock->dispoVoiesRames[disp].voie;
        new_dec_a.site        = mock->dispoVoiesRames[disp].site;
        new_dec_a.empV        = mock->dispoVoiesRames[disp].dispoVoie;
        new_dec_a.empR        = mock->dispoVoiesRames[disp].dispoRame;
        new_dec_a.timeslotGraphSplited = mock->trajectoryStops[new_dec_a.empR].getDispoStop();
        new_dec_a.lastCreneau = {0,0};
        
        // Pénalités anciennes O(1) — machine de départ et machine d'arrivée
        Penalty oldPenSrc = sol.penaltyPerMachine.count(dec_a.empV)
            ? sol.penaltyPerMachine.at(dec_a.empV) : Penalty{};
        Penalty oldPenDst = sol.penaltyPerMachine.count(new_dec_a.empV)
            ? sol.penaltyPerMachine.at(new_dec_a.empV) : Penalty{};

        unsigned int oldCost = dec_a.lastCreneau.first*STFMockInstance::jobs[op_a]->getPoidsRetard();
        unsigned int oldDiagCost = dec_a.rejected ? STFMockInstance::jobs[op_a]->getPoidsRejet() : 0;
        unsigned int newDiagCost = new_dec_a.rejected ? STFMockInstance::jobs[op_a]->getPoidsRejet() : 0;

        std::vector<std::pair<unsigned short, decision>> jobsEmpVA;
        std::vector<std::pair<unsigned short, decision>> jobsEmpVB;

        for(auto& dec : neighbor.decisions)
        {
            if(!dec.second.excluded && dec.first != op_a)
            {
                if(dec.second.empV == new_dec_a.empV)
                {
                    jobsEmpVA.push_back({dec.first, dec.second});
                    oldCost += dec.second.lastCreneau.first * STFMockInstance::jobs[dec.first]->getPoidsRetard();
                }
            }
            if(!dec.second.excluded)
            {
                if(dec.second.empV == dec_a.empV)
                {
                    jobsEmpVB.push_back({dec.first, dec.second});
                    oldCost += dec.second.lastCreneau.first * STFMockInstance::jobs[dec.first]->getPoidsRetard();
                }
            }
        }

        std::sort(jobsEmpVA.begin(), jobsEmpVA.end(), [&](auto& el1, auto& el2){
            auto cren1 = el1.second.lastCreneau;
            auto cren2 = el2.second.lastCreneau;
            return cren1.first < cren2.first;
        });
        std::sort(jobsEmpVB.begin(), jobsEmpVB.end(), [&](auto& el1, auto& el2){
            auto cren1 = el1.second.lastCreneau;
            auto cren2 = el2.second.lastCreneau;
            return cren1.first < cren2.first;
        });

        unsigned int newCost = 0;
        Penalty penA; Penalty penB;
        bool feasA = true; bool feasB = true;
        //décale à gauche sur track de départ
        if(!jobsEmpVB.empty())
        {
            auto resB = checkSequence(jobsEmpVB, dec_a.empV, penB, feasB);
            applySequenceResult(neighbor, jobsEmpVB, *resB, newCost, penB, feasB);
        }

        std::uniform_int_distribution<unsigned int> distPos(0,jobsEmpVA.size());
        auto pos = distPos(randomEngine);
        
        auto seq = jobsEmpVA;
        if(pos == jobsEmpVA.size())
        {
            seq.insert(seq.end(), std::make_pair(op_a, new_dec_a));
        }
        else {
            seq.insert(seq.begin() + pos, std::make_pair(op_a, new_dec_a));
        }
        auto res = checkSequence(seq, new_dec_a.empV, penA, feasA);
        if(!res) return std::nullopt;
    
        applySequenceResult(neighbor, seq, *res, newCost, penA, feasA);

        neighbor.penalty                        = sol.penalty - oldPenSrc - oldPenDst + penB + penA;
        neighbor.penaltyPerMachine[dec_a.empV]  = penB;
        neighbor.penaltyPerMachine[new_dec_a.empV] = penA;
        neighbor.isFeasible                     = neighbor.penalty.isFeasible();
        neighbor.fictiveCost                    = (neighbor.fictiveCost - oldCost) + newCost;
        neighbor.diagCost                       = (neighbor.diagCost - oldDiagCost) + newDiagCost;
        neighbor.cost                           = (neighbor.cost      - oldCost) + newCost;
        neighbor.source                         = ESourceTrackPlan::SimAn;
        return neighbor;
    }

    std::optional<SASolution> SimulatedAnnealing::move_remove_WC(const SASolution& sol)
    {
        auto mock = sol.mock;
        if (!mock) return std::nullopt;

        std::vector<unsigned short> active_ops;
        for (auto& [op_id, dec] : sol.decisions)
            if (!dec.excluded) active_ops.push_back(op_id);
        if (active_ops.empty()) return std::nullopt;

        std::uniform_int_distribution<int> dist(0, (int)active_ops.size() - 1);
        unsigned short op_a = active_ops[dist(randomEngine)];
        //const Decision& dec_a = sol.decisions.at(op_a);

        //Construire le voisin - exclure a
        SASolution neighbor = sol;
        const Decision& dec_a = sol.decisions.at(op_a);
        Decision& new_dec_a = neighbor.decisions[op_a];

        new_dec_a.rejected = false,
        new_dec_a.excluded = true;
        new_dec_a.voie        = 0;
        new_dec_a.site        = 0;
        new_dec_a.empV        = 0;
        new_dec_a.empR        = 0;
        new_dec_a.timeslotGraphSplited = CreneauHoraire();
        new_dec_a.lastCreneau = {0,0};
        
        // Pénalité ancienne O(1) — inclut la contribution de op_a
        Penalty oldPen = sol.penaltyPerMachine.count(dec_a.empV)
            ? sol.penaltyPerMachine.at(dec_a.empV) : Penalty{};

        unsigned int oldCost = dec_a.lastCreneau.first * STFMockInstance::jobs[op_a]->getPoidsRetard();
        unsigned int oldDiagCost = dec_a.rejected ? STFMockInstance::jobs[op_a]->getPoidsRejet() : 0;

        std::vector<std::pair<unsigned short, decision>> jobsEmpVA;
        for(auto& dec : neighbor.decisions)
        {
            if(!dec.second.excluded)
            {
                if(dec.second.empV == dec_a.empV)
                {
                    jobsEmpVA.push_back({dec.first, dec.second});
                    oldCost += dec.second.lastCreneau.first * STFMockInstance::jobs[dec.first]->getPoidsRetard();
                }
            }
        }

        unsigned int remaining = 0.0;
        Penalty penA; bool feasA = true; 
        if(!jobsEmpVA.empty())
        {
            std::sort(jobsEmpVA.begin(), jobsEmpVA.end(), [&](auto& el1, auto& el2){
                auto cren1 = el1.second.lastCreneau;
                auto cren2 = el2.second.lastCreneau;
                return cren1.first < cren2.first;
            });
            auto resA = checkSequence(jobsEmpVA, dec_a.empV, penA, feasA);
            if(!resA) return std::nullopt;
            
            applySequenceResult(neighbor, jobsEmpVA, *resA, remaining, penA, feasA);
        }

        double fictivePenalty = fictiveCostExcluded(op_a);
        unsigned int excludedCost = MAXIMUM_TIME_OFFSET * STFMockInstance::jobs[op_a]->getPoidsRetard();

        neighbor.penalty                       = sol.penalty - oldPen + penA;
        neighbor.penaltyPerMachine[dec_a.empV] = penA;
        neighbor.isFeasible                    = neighbor.penalty.isFeasible();
        neighbor.fictiveExcludedCosts[op_a]    = fictivePenalty;
        neighbor.fictiveCost                   = (neighbor.fictiveCost - oldCost) + fictivePenalty + remaining;
        neighbor.diagCost                      = neighbor.diagCost - oldDiagCost;
        neighbor.cost                          = (neighbor.cost - oldCost) + excludedCost + remaining;
        neighbor.source                        = ESourceTrackPlan::SimAn;
        return neighbor;
    }

    std::optional<SASolution> SimulatedAnnealing::move_change_mode_WC(const SASolution& sol)
    {
        auto mock = sol.mock;
        if (!mock) return std::nullopt;

        std::vector<unsigned short> active_ops;
        for (auto& [op_id, dec] : sol.decisions)
            if (!dec.excluded) active_ops.push_back(op_id);
        if (active_ops.empty()) return std::nullopt;

        std::uniform_int_distribution<int> dist(0, (int)active_ops.size() - 1);
        unsigned short op_a = active_ops[dist(randomEngine)];
        const Decision& dec_a = sol.decisions.at(op_a);

        SASolution neighbor = sol;
        Decision& new_dec_a = neighbor.decisions[op_a];

        
        unsigned int oldDiagCost = dec_a.rejected ? STFMockInstance::jobs[op_a]->getPoidsRejet() : 0;
        unsigned int newDiagCost = dec_a.rejected ? 0 : STFMockInstance::jobs[op_a]->getPoidsRejet();
        if(dec_a.rejected)
        {
            new_dec_a.rejected = false;
        }
        else {
            if(neighbor.diagCost + STFMockInstance::jobs[op_a]->getPoidsRejet() <= configlib::Configuration::Global.EPSILON)
            {
                new_dec_a.rejected = true;
            }
            else {
                return std::nullopt;
            }
        }
        
        // Pénalité ancienne O(1)
        Penalty oldPen = sol.penaltyPerMachine.count(dec_a.empV)
        ? sol.penaltyPerMachine.at(dec_a.empV) : Penalty{};
        
        unsigned int oldCost = 0;
        std::vector<std::pair<unsigned short, decision>> jobsEmpVA;
        for(auto& dec : neighbor.decisions)
        {
            if(!dec.second.excluded)
            {
                if(dec.second.empV == dec_a.empV)
                {
                    jobsEmpVA.push_back({dec.first, dec.second});
                    oldCost += dec.second.lastCreneau.first * STFMockInstance::jobs[dec.first]->getPoidsRetard();
                }
            }
        }
        
        std::sort(jobsEmpVA.begin(), jobsEmpVA.end(), [&](auto& el1, auto& el2){
            auto cren1 = el1.second.lastCreneau;
            auto cren2 = el2.second.lastCreneau;
            return cren1.first < cren2.first;
        });
        unsigned int newCost = 0;

        Penalty penA; bool feasA = true;
        auto resA = checkSequence(jobsEmpVA, dec_a.empV, penA, feasA);
        if(!resA) return std::nullopt;
        
        applySequenceResult(neighbor, jobsEmpVA, *resA, newCost, penA, feasA);
    
        neighbor.penalty                       = sol.penalty - oldPen + penA;
        neighbor.penaltyPerMachine[dec_a.empV] = penA;
        neighbor.isFeasible                    = neighbor.penalty.isFeasible();
        neighbor.fictiveCost                   = (neighbor.fictiveCost - oldCost) + newCost;
        neighbor.diagCost                      = (neighbor.diagCost - oldDiagCost) + newDiagCost;
        neighbor.cost                          = (neighbor.cost      - oldCost) + newCost;
        neighbor.source                        = ESourceTrackPlan::SimAn;
        return neighbor;
    }

    std::optional<std::vector<std::pair<unsigned short, Decision>>> SimulatedAnnealing::checkSequence(const std::vector<std::pair<unsigned short, Decision>>& jobsDec, unsigned int machine)
    {
        auto machineDisp = STFMockInstance::machines[machine]->getDispo();
        unsigned short minBegin = machineDisp.getDebut().getRelativeDate();
        std::vector<std::pair<unsigned short, Decision>> res;
        for(auto& jobDec : jobsDec)
        {
            auto matchWithMachine = CreneauHoraire::checkSlotsCompatibility(jobDec.second.timeslotGraphSplited, machineDisp).second;
            minBegin = std::max(minBegin, matchWithMachine.first);
            auto newJobDec = jobDec;
            unsigned int duration = !jobDec.second.rejected ? STFMockInstance::jobs[jobDec.first]->getDuree() : STFMockInstance::jobs[jobDec.first]->getDureeDiag();
            newJobDec.second.lastCreneau = {minBegin, minBegin + duration};
            if(minBegin + duration > matchWithMachine.first + matchWithMachine.second)
                return std::nullopt;
            minBegin = minBegin + duration;
            res.push_back(newJobDec);
        }
        return std::optional<std::vector<std::pair<unsigned short, Decision>>>(res);
    }

    std::optional<std::vector<std::pair<unsigned short, Decision>>>
    SimulatedAnnealing::checkSequence(
        const std::vector<std::pair<unsigned short, Decision>>& jobsDec,
        unsigned int machine,
        Penalty& outPenalty, 
        bool&   outFeasible)
    {
        auto machineDisp = STFMockInstance::machines[machine]->getDispo();
        unsigned short minBegin = machineDisp.getDebut().getRelativeDate();
        std::vector<std::pair<unsigned short, Decision>> res;
        outFeasible = true;

        for (auto& jobDec : jobsDec)
        {
            auto matchWithMachine = CreneauHoraire::checkSlotsCompatibility(
                jobDec.second.timeslotGraphSplited, machineDisp).second;

            minBegin = std::max(minBegin, matchWithMachine.first);
            unsigned int duration = !jobDec.second.rejected
                ? STFMockInstance::jobs[jobDec.first]->getDuree()
                : STFMockInstance::jobs[jobDec.first]->getDureeDiag();

            unsigned int windowEnd = matchWithMachine.first + matchWithMachine.second;
            unsigned int jobEnd    = minBegin + duration;

            if (jobEnd > windowEnd)
            {
                if (!authorizeInfeasible)
                    return std::nullopt;

                // Planification forcée + pénalité
                outFeasible = false;
                double overrun   = static_cast<double>(jobEnd - windowEnd);
                outPenalty.add(EPenaltyType::TIME_WINDOW_OVERRUN, overrun);
            }

            auto newJobDec = jobDec;
            newJobDec.second.lastCreneau = {minBegin, jobEnd};
            minBegin = jobEnd;
            res.push_back(newJobDec);
        }
        for (size_t i = 1; i < res.size(); ++i) {
            if (res[i].second.lastCreneau.first < res[i-1].second.lastCreneau.second && res[i].second.lastCreneau.first >= res[i-1].second.lastCreneau.first) {
                std::cout << res[i].second.lastCreneau.first << " " << res[i].second.lastCreneau.second << " " << res[i-1].second.lastCreneau.first << " " << res[i-1].second.lastCreneau.second << std::endl;
                throw std::logic_error("Chevauchement détecté");
            }
        }
        return res;
    }    
}