package com.zy.asrs.planner;

import com.alibaba.fastjson.JSONArray;
import com.alibaba.fastjson.JSONObject;
import com.google.ortools.Loader;
import com.google.ortools.sat.BoolVar;
import com.google.ortools.sat.CircuitConstraint;
import com.google.ortools.sat.CpModel;
import com.google.ortools.sat.CpSolver;
import com.google.ortools.sat.CpSolverStatus;
import com.google.ortools.sat.CumulativeConstraint;
import com.google.ortools.sat.IntervalVar;
import com.google.ortools.sat.IntVar;
import com.google.ortools.sat.LinearExpr;
import org.springframework.stereotype.Component;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

@Component
public class PlannerOrtoolsSolverService {

    private static volatile boolean loaded = false;

    public JSONObject solve(JSONObject req) {
        ensureLoaded();

        JSONArray cranesIn = req.getJSONArray("cranes");
        JSONArray tasksIn = req.getJSONArray("tasks");
        JSONObject cfgIn = req.getJSONObject("config");

        if (cranesIn == null || cranesIn.isEmpty()) {
            throw new IllegalArgumentException("cranes is empty");
        }
        if (tasksIn == null || tasksIn.isEmpty()) {
            JSONObject out = new JSONObject();
            out.put("schedule", new JSONArray());
            out.put("objectiveValue", 0);
            return out;
        }

        List<String> craneCodes = new ArrayList<>();
        Map<String, JSONObject> craneByCode = new HashMap<>();
        for (int i = 0; i < cranesIn.size(); i++) {
            JSONObject c = cranesIn.getJSONObject(i);
            if (c == null) continue;
            String code = c.getString("code");
            if (code == null || code.trim().isEmpty()) continue;
            craneCodes.add(code);
            craneByCode.put(code, c);
        }
        if (craneCodes.isEmpty()) {
            throw new IllegalArgumentException("cranes is empty");
        }

        int outMinGapSec = optInt(cfgIn, "outMinGapSec", 0);
        int wMakespan = optInt(cfgIn, "wMakespan", 1000);
        int wPriorityEarlyFinish = optInt(cfgIn, "wPriorityEarlyFinish", 5);
        int wWaitTime = optInt(cfgIn, "wWaitTime", 1);
        int wOutTardiness = optInt(cfgIn, "wOutTardiness", 0);
        boolean enableOutDue = optBool(cfgIn, "enableOutDue", false);
        int outTaktSec = optInt(cfgIn, "outTaktSec", 20);
        double maxSolveSeconds = optDouble(cfgIn, "maxSolveSeconds", 5.0);
        int numSearchWorkers = optInt(cfgIn, "numSearchWorkers", 8);
        boolean logSearchProgress = optBool(cfgIn, "logSearchProgress", false);
        int horizonPaddingSec = optInt(cfgIn, "horizonPaddingSec", 3600);
        int conveyorStationTaskLimit = optInt(cfgIn, "conveyorStationTaskLimit", 0);
        int conveyorStationRunningTasks = optInt(cfgIn, "conveyorStationRunningTasks", 0);

        double conveyorDefaultSpeedMps = optDouble(req, "conveyorDefaultSpeedMps", 0.6);
        if (conveyorDefaultSpeedMps <= 0) conveyorDefaultSpeedMps = 0.6;

        List<Integer> taskIds = new ArrayList<>();
        Map<Integer, JSONObject> taskById = new HashMap<>();
        for (int i = 0; i < tasksIn.size(); i++) {
            JSONObject t = tasksIn.getJSONObject(i);
            if (t == null) continue;
            Integer taskId = t.getInteger("taskId");
            if (taskId == null || taskId <= 0) continue;
            taskIds.add(taskId);
            taskById.put(taskId, t);
        }
        if (taskIds.isEmpty()) {
            JSONObject out = new JSONObject();
            out.put("schedule", new JSONArray());
            out.put("objectiveValue", 0);
            return out;
        }

        Map<Integer, Integer> nodeByTaskId = new HashMap<>();
        int nodeIdx = 1;
        for (Integer taskId : taskIds) {
            nodeByTaskId.put(taskId, nodeIdx);
            nodeIdx++;
        }

        Map<Integer, Double> fromX = new HashMap<>();
        Map<Integer, Double> fromY = new HashMap<>();
        Map<Integer, Double> toX = new HashMap<>();
        Map<Integer, Double> toY = new HashMap<>();
        double minX = Double.POSITIVE_INFINITY;
        double maxX = Double.NEGATIVE_INFINITY;
        double minY = Double.POSITIVE_INFINITY;
        double maxY = Double.NEGATIVE_INFINITY;
        for (Integer taskId : taskIds) {
            JSONObject t = taskById.get(taskId);
            JSONObject fp = t == null ? null : t.getJSONObject("fromPos");
            JSONObject tp = t == null ? null : t.getJSONObject("toPos");
            double fx = optDouble(fp, "x", 0);
            double fy = optDouble(fp, "y", 0);
            double tx = optDouble(tp, "x", 0);
            double ty = optDouble(tp, "y", 0);
            fromX.put(taskId, fx);
            fromY.put(taskId, fy);
            toX.put(taskId, tx);
            toY.put(taskId, ty);
            minX = Math.min(minX, Math.min(fx, tx));
            maxX = Math.max(maxX, Math.max(fx, tx));
            minY = Math.min(minY, Math.min(fy, ty));
            maxY = Math.max(maxY, Math.max(fy, ty));
        }
        if (!Double.isFinite(minX)) {
            minX = 0;
            maxX = 0;
            minY = 0;
            maxY = 0;
        }

        Map<Integer, Integer> outConv = new HashMap<>();
        Map<String, Integer> baseDur = new HashMap<>();
        int maxReady = 0;
        int setupBoundPerTask = 0;
        for (String cc : craneCodes) {
            JSONObject c = craneByCode.get(cc);
            int ready = optInt(c, "readySec", 0);
            if (ready > maxReady) maxReady = ready;
            int bound = craneMoveTimeSecForDelta(c, Math.abs(maxX - minX), Math.abs(maxY - minY));
            setupBoundPerTask = Math.max(setupBoundPerTask, bound);
        }
        for (Integer taskId : taskIds) {
            JSONObject t = taskById.get(taskId);
            outConv.put(taskId, conveyorTimeSec(t, conveyorDefaultSpeedMps));
            for (String cc : eligibleCodes(t, craneCodes, craneByCode)) {
                baseDur.put(durKey(taskId, cc), craneBaseExecTimeSec(craneByCode.get(cc), t));
            }
        }

        int horizon = estimateHorizon(taskIds, taskById, craneCodes, baseDur, outConv, maxReady, horizonPaddingSec, craneByCode, setupBoundPerTask);

        CpModel m = new CpModel();

        Map<Integer, IntVar> startVars = new HashMap<>();
        Map<Integer, IntVar> endVars = new HashMap<>();
        Map<String, BoolVar> chosen = new HashMap<>();
        Map<String, Map<Integer, BoolVar>> presenceByCrane = new HashMap<>();
        Map<String, List<Integer>> eligibleTasksByCrane = new HashMap<>();
        for (String cc : craneCodes) {
            presenceByCrane.put(cc, new HashMap<>());
            eligibleTasksByCrane.put(cc, new ArrayList<>());
        }
        Map<Integer, IntVar> outEtaVars = new HashMap<>();
        List<IntervalVar> convIntervals = new ArrayList<>();

        for (Integer taskId : taskIds) {
            JSONObject t = taskById.get(taskId);
            IntVar s = m.newIntVar(0, horizon, "S_" + taskId);
            IntVar e = m.newIntVar(0, horizon, "E_" + taskId);
            startVars.put(taskId, s);
            endVars.put(taskId, e);

            List<String> eligible = eligibleCodes(t, craneCodes, craneByCode);
            List<BoolVar> presences = new ArrayList<>();
            for (String cc : eligible) {
                BoolVar p = m.newBoolVar("P_" + taskId + "_" + cc);
                presences.add(p);
                chosen.put(chosenKey(taskId, cc), p);
                presenceByCrane.get(cc).put(taskId, p);
                eligibleTasksByCrane.get(cc).add(taskId);

                int ready = optInt(craneByCode.get(cc), "readySec", 0);
                m.addGreaterOrEqual(s, ready).onlyEnforceIf(p);
            }
            m.addExactlyOne(presences.toArray(new BoolVar[0]));

            String taskType = optStr(t, "taskType", "");
            if ("OUT".equalsIgnoreCase(taskType)) {
                IntVar outEta = m.newIntVar(0, horizon, "OUT_" + taskId);
                outEtaVars.put(taskId, outEta);
                m.addEquality(outEta, LinearExpr.sum(new LinearExpr[]{
                        LinearExpr.term(e, 1),
                        LinearExpr.constant(outConv.get(taskId))
                }));

                int conv = outConv.get(taskId);
                IntVar sConv = m.newIntVar(0, horizon, "CS_" + taskId);
                IntVar eConv = m.newIntVar(0, horizon, "CE_" + taskId);
                m.addEquality(sConv, e);
                m.addEquality(eConv, LinearExpr.sum(new LinearExpr[]{
                        LinearExpr.term(sConv, 1),
                        LinearExpr.constant(conv)
                }));
                IntervalVar itvConv = m.newIntervalVar(sConv, LinearExpr.constant(conv), eConv, "CITV_" + taskId);
                convIntervals.add(itvConv);
            }
        }

        for (String cc : craneCodes) {
            JSONObject crane = craneByCode.get(cc);
            int ready = optInt(crane, "readySec", 0);
            Map<Integer, BoolVar> pMap = presenceByCrane.get(cc);
            List<Integer> eligibleTasks = eligibleTasksByCrane.get(cc);

            CircuitConstraint circuit = m.addCircuit();
            BoolVar loop0 = m.newBoolVar("LOOP0_" + cc);
            circuit.addArc(0, 0, loop0);

            for (Integer tid : taskIds) {
                int node = nodeByTaskId.get(tid);
                BoolVar p = pMap.get(tid);
                if (p != null) {
                    circuit.addArc(node, node, p.not());
                    m.addImplication(p, loop0.not());
                } else {
                    circuit.addArc(node, node, m.trueLiteral());
                }
            }

            for (Integer tid : eligibleTasks) {
                int nodeJ = nodeByTaskId.get(tid);
                BoolVar pJ = pMap.get(tid);

                BoolVar arc0j = m.newBoolVar("ARC_" + cc + "_0_" + nodeJ);
                circuit.addArc(0, nodeJ, arc0j);
                m.addImplication(arc0j, loop0.not());
                m.addImplication(arc0j, pJ);
                int setup0j = craneSetupFromInitSec(crane, fromX.get(tid), fromY.get(tid));
                int dJ = baseDur.get(durKey(tid, cc));
                m.addGreaterOrEqual(startVars.get(tid), ready).onlyEnforceIf(arc0j);
                m.addGreaterOrEqual(endVars.get(tid), LinearExpr.sum(new LinearExpr[]{
                        LinearExpr.term(startVars.get(tid), 1),
                        LinearExpr.constant(setup0j + dJ)
                })).onlyEnforceIf(arc0j);

                BoolVar arcj0 = m.newBoolVar("ARC_" + cc + "_" + nodeJ + "_0");
                circuit.addArc(nodeJ, 0, arcj0);
                m.addImplication(arcj0, loop0.not());
                m.addImplication(arcj0, pJ);
            }

            for (int i = 0; i < eligibleTasks.size(); i++) {
                int tidI = eligibleTasks.get(i);
                int nodeI = nodeByTaskId.get(tidI);
                BoolVar pI = pMap.get(tidI);
                for (int j = 0; j < eligibleTasks.size(); j++) {
                    if (i == j) continue;
                    int tidJ = eligibleTasks.get(j);
                    int nodeJ = nodeByTaskId.get(tidJ);
                    BoolVar pJ = pMap.get(tidJ);
                    BoolVar arc = m.newBoolVar("ARC_" + cc + "_" + nodeI + "_" + nodeJ);
                    circuit.addArc(nodeI, nodeJ, arc);
                    m.addImplication(arc, pI);
                    m.addImplication(arc, pJ);
                    int setup = craneMoveTimeSec(crane, toX.get(tidI), toY.get(tidI), fromX.get(tidJ), fromY.get(tidJ));
                    int dJ = baseDur.get(durKey(tidJ, cc));
                    m.addGreaterOrEqual(startVars.get(tidJ), endVars.get(tidI)).onlyEnforceIf(arc);
                    m.addGreaterOrEqual(endVars.get(tidJ), LinearExpr.sum(new LinearExpr[]{
                            LinearExpr.term(startVars.get(tidJ), 1),
                            LinearExpr.constant(setup + dJ)
                    })).onlyEnforceIf(arc);
                }
            }
        }

        if (conveyorStationTaskLimit > 0 && !convIntervals.isEmpty()) {
            int effCap = Math.max(0, conveyorStationTaskLimit - Math.max(0, conveyorStationRunningTasks));
            if (effCap > 0) {
                CumulativeConstraint cumul = m.addCumulative(effCap);
                for (IntervalVar itv : convIntervals) {
                    cumul.addDemand(itv, 1);
                }
            }
        }

        addOutboundOrderConstraints(m, taskIds, taskById, outEtaVars, outMinGapSec);

        IntVar makespan = m.newIntVar(0, horizon, "MAKESPAN");
        List<IntVar> ends = new ArrayList<>();
        for (Integer tid : taskIds) {
            ends.add(endVars.get(tid));
        }
        m.addMaxEquality(makespan, ends.toArray(new IntVar[0]));

        List<LinearExpr> obj = new ArrayList<>();
        obj.add(LinearExpr.term(makespan, wMakespan));
        if (wPriorityEarlyFinish > 0) {
            for (Integer tid : taskIds) {
                int pri = optInt(taskById.get(tid), "priority", 5);
                if (pri < 0) pri = 0;
                long coeff = (long) wPriorityEarlyFinish * (long) pri;
                if (coeff != 0) {
                    obj.add(LinearExpr.term(endVars.get(tid), coeff));
                }
            }
        }
        if (wWaitTime > 0) {
            long now = System.currentTimeMillis();
            for (Integer tid : taskIds) {
                long createTime = optLong(taskById.get(tid), "createTime", now);
                long waitSeconds = Math.max(0, (now - createTime) / 1000);
                long coeff = (long) wWaitTime * waitSeconds; // 等待时间越长，权重越大，越希望尽早完成
                // 这里我们希望尽早完成等待时间长的任务，所以将完成时间乘以等待时间作为惩罚项
                // 惩罚 = endVar * wWaitTime * waitSeconds
                if (coeff > 0) {
                    obj.add(LinearExpr.term(endVars.get(tid), coeff));
                }
            }
        }
        if (enableOutDue && wOutTardiness > 0) {
            IntVar tardSum = addOutboundDueTardiness(m, taskIds, taskById, outEtaVars, outTaktSec);
            obj.add(LinearExpr.term(tardSum, wOutTardiness));
        }
        m.minimize(LinearExpr.sum(obj.toArray(new LinearExpr[0])));

        CpSolver solver = new CpSolver();
        solver.getParameters().setMaxTimeInSeconds(maxSolveSeconds);
        solver.getParameters().setNumSearchWorkers(numSearchWorkers);
        solver.getParameters().setLogSearchProgress(logSearchProgress);

        CpSolverStatus status = solver.solve(m);
        if (status != CpSolverStatus.OPTIMAL && status != CpSolverStatus.FEASIBLE) {
            throw new IllegalStateException("No feasible solution");
        }

        JSONArray schedule = new JSONArray();
        for (Integer tid : taskIds) {
            JSONObject t = taskById.get(tid);
            int s = (int) solver.value(startVars.get(tid));
            int e = (int) solver.value(endVars.get(tid));

            String chosenCrane = null;
            for (String cc : eligibleCodes(t, craneCodes, craneByCode)) {
                BoolVar p = chosen.get(chosenKey(tid, cc));
                if (p != null && solver.booleanValue(p)) {
                    chosenCrane = cc;
                    break;
                }
            }
            if (chosenCrane == null) {
                throw new IllegalStateException("internal error: task " + tid + " no crane chosen");
            }
            int d = (int) (solver.value(endVars.get(tid)) - solver.value(startVars.get(tid)));

            JSONObject item = new JSONObject();
            item.put("craneCode", chosenCrane);
            item.put("taskId", tid);
            item.put("taskType", optStr(t, "taskType", ""));
            item.put("startSec", s);
            item.put("endSec", e);
            item.put("durationSec", d);
            item.put("priority", optInt(t, "priority", 5));
            if ("OUT".equalsIgnoreCase(optStr(t, "taskType", ""))) {
                IntVar outEta = outEtaVars.get(tid);
                if (outEta != null) {
                    item.put("outEtaSec", (int) solver.value(outEta));
                }
            }
            schedule.add(item);
        }

        schedule.sort(new Comparator<Object>() {
            @Override
            public int compare(Object o1, Object o2) {
                JSONObject a = (JSONObject) JSONObject.toJSON(o1);
                JSONObject b = (JSONObject) JSONObject.toJSON(o2);
                String ca = a.getString("craneCode");
                String cb = b.getString("craneCode");
                int cmp = (ca == null ? "" : ca).compareTo(cb == null ? "" : cb);
                if (cmp != 0) return cmp;
                return Integer.compare(a.getIntValue("startSec"), b.getIntValue("startSec"));
            }
        });

        JSONObject out = new JSONObject();
        out.put("schedule", schedule);
        out.put("objectiveValue", solver.objectiveValue());
        return out;
    }

    private static void ensureLoaded() {
        if (loaded) return;
        synchronized (PlannerOrtoolsSolverService.class) {
            if (loaded) return;
            Loader.loadNativeLibraries();
            loaded = true;
        }
    }

    private static String chosenKey(int taskId, String cc) {
        return taskId + "|" + cc;
    }

    private static String durKey(int taskId, String cc) {
        return taskId + "|" + cc;
    }

    private static int estimateHorizon(List<Integer> taskIds,
                                       Map<Integer, JSONObject> taskById,
                                       List<String> craneCodes,
                                       Map<String, Integer> dur,
                                       Map<Integer, Integer> outConv,
                                       int maxReady,
                                       int padding,
                                       Map<String, JSONObject> craneByCode,
                                       int setupBoundPerTask) {
        int total = 0;
        for (Integer tid : taskIds) {
            JSONObject t = taskById.get(tid);
            List<String> elig = eligibleCodes(t, craneCodes, craneByCode);
            int best = Integer.MAX_VALUE;
            for (String cc : elig) {
                Integer d = dur.get(durKey(tid, cc));
                if (d != null && d < best) best = d;
            }
            if (best != Integer.MAX_VALUE) total += best;
            total += Math.max(0, setupBoundPerTask);
            Integer conv = outConv.get(tid);
            if (conv != null) total += conv;
        }
        return total + maxReady + Math.max(0, padding);
    }

    private static List<String> eligibleCodes(JSONObject task, List<String> craneCodes, Map<String, JSONObject> craneByCode) {
        JSONArray arr = task.getJSONArray("eligibleCranes");
        if (arr == null || arr.isEmpty()) {
            return new ArrayList<>(craneCodes);
        }
        List<String> out = new ArrayList<>();
        for (int i = 0; i < arr.size(); i++) {
            String cc = arr.getString(i);
            if (cc != null && craneByCode.containsKey(cc)) {
                out.add(cc);
            }
        }
        if (out.isEmpty()) {
            throw new IllegalArgumentException("Task " + task.getInteger("taskId") + " has no eligible cranes after filtering");
        }
        return out;
    }

    private static int craneBaseExecTimeSec(JSONObject crane, JSONObject task) {
        JSONObject fromPos = task.getJSONObject("fromPos");
        JSONObject toPos = task.getJSONObject("toPos");
        double fromX = optDouble(fromPos, "x", 0);
        double fromY = optDouble(fromPos, "y", 0);
        double toX = optDouble(toPos, "x", 0);
        double toY = optDouble(toPos, "y", 0);
        int move = craneMoveTimeSec(crane, fromX, fromY, toX, toY);
        int pick = optInt(crane, "pickSec", 0);
        int drop = optInt(crane, "dropSec", 0);
        return move + pick + drop;
    }

    private static int craneSetupFromInitSec(JSONObject crane, double toX, double toY) {
        JSONObject initPos = crane.getJSONObject("initPos");
        if (initPos == null) {
            return 0;
        }
        double initX = optDouble(initPos, "x", Double.NaN);
        double initY = optDouble(initPos, "y", Double.NaN);
        if (Double.isNaN(initX) || Double.isNaN(initY)) {
            return 0;
        }
        return craneMoveTimeSec(crane, initX, initY, toX, toY);
    }

    private static int craneMoveTimeSecForDelta(JSONObject crane, double dx, double dy) {
        double vx = optDouble(crane, "vx", 1.0);
        double vy = optDouble(crane, "vy", 1.0);
        String moveMode = optStr(crane, "moveMode", "max");
        double tx = vx <= 0 ? 0 : Math.abs(dx) / vx;
        double ty = vy <= 0 ? 0 : Math.abs(dy) / vy;
        double t = "sum".equalsIgnoreCase(moveMode) ? (tx + ty) : Math.max(tx, ty);
        return (int) Math.round(t);
    }

    private static int craneMoveTimeSec(JSONObject crane, double fromX, double fromY, double toX, double toY) {
        return craneMoveTimeSecForDelta(crane, toX - fromX, toY - fromY);
    }

    private static int conveyorTimeSec(JSONObject task, double defaultSpeedMps) {
        String taskType = optStr(task, "taskType", "");
        if (!"OUT".equalsIgnoreCase(taskType)) {
            return 0;
        }
        JSONArray segs = task.getJSONArray("conveyorPath");
        if (segs != null && !segs.isEmpty()) {
            double total = 0.0;
            for (int i = 0; i < segs.size(); i++) {
                JSONObject seg = segs.getJSONObject(i);
                if (seg == null) continue;
                double len = optDouble(seg, "length_m", 0);
                double sp = optDouble(seg, "speed_mps", 0);
                if (len > 0 && sp > 0) total += len / sp;
            }
            return (int) Math.round(total);
        }
        JSONArray nav = task.getJSONArray("navigatePath");
        JSONArray edges = task.getJSONArray("pathEdges");
        if (nav != null && nav.size() >= 2 && edges != null && !edges.isEmpty()) {
            Map<String, Edge> edgeMap = new HashMap<>();
            for (int i = 0; i < edges.size(); i++) {
                JSONObject e = edges.getJSONObject(i);
                if (e == null) continue;
                int u = e.getIntValue("fromId");
                int v = e.getIntValue("toId");
                double len = optDouble(e, "length_m", 0);
                Double sp = e.containsKey("speed_mps") ? e.getDouble("speed_mps") : null;
                edgeMap.put(u + "->" + v, new Edge(len, sp));
            }
            double total = 0.0;
            for (int i = 0; i < nav.size() - 1; i++) {
                int u = nav.getIntValue(i);
                int v = nav.getIntValue(i + 1);
                Edge ed = edgeMap.get(u + "->" + v);
                if (ed == null) ed = edgeMap.get(v + "->" + u);
                if (ed == null) {
                    continue;
                }
                double sp = ed.speedMps != null && ed.speedMps > 0 ? ed.speedMps : defaultSpeedMps;
                if (ed.lengthM > 0 && sp > 0) total += ed.lengthM / sp;
            }
            return (int) Math.round(total);
        }
        return 0;
    }

    private static void addOutboundOrderConstraints(CpModel m,
                                                   List<Integer> taskIds,
                                                   Map<Integer, JSONObject> taskById,
                                                   Map<Integer, IntVar> outEtaVars,
                                                   int minGap) {
        Map<String, List<GroupItem>> groups = new HashMap<>();
        for (Integer tid : taskIds) {
            JSONObject t = taskById.get(tid);
            if (!"OUT".equalsIgnoreCase(optStr(t, "taskType", ""))) continue;
            String g = t.getString("outGroup");
            Integer seq = t.getInteger("outSeq");
            if (g == null || seq == null) continue;
            groups.computeIfAbsent(g, k -> new ArrayList<>()).add(new GroupItem(seq, tid));
        }
        for (Map.Entry<String, List<GroupItem>> e : groups.entrySet()) {
            List<GroupItem> items = e.getValue();
            Collections.sort(items, new Comparator<GroupItem>() {
                @Override
                public int compare(GroupItem a, GroupItem b) {
                    return Integer.compare(a.seq, b.seq);
                }
            });
            for (int i = 0; i < items.size() - 1; i++) {
                int aId = items.get(i).taskId;
                int bId = items.get(i + 1).taskId;
                IntVar aEta = outEtaVars.get(aId);
                IntVar bEta = outEtaVars.get(bId);
                if (aEta != null && bEta != null) {
                    m.addGreaterOrEqual(bEta, LinearExpr.sum(new LinearExpr[]{
                            LinearExpr.term(aEta, 1),
                            LinearExpr.constant(minGap)
                    }));
                }
            }
        }
    }

    private static IntVar addOutboundDueTardiness(CpModel m,
                                                 List<Integer> taskIds,
                                                 Map<Integer, JSONObject> taskById,
                                                 Map<Integer, IntVar> outEtaVars,
                                                 int taktSec) {
        Map<String, List<GroupItem>> groups = new HashMap<>();
        for (Integer tid : taskIds) {
            JSONObject t = taskById.get(tid);
            if (!"OUT".equalsIgnoreCase(optStr(t, "taskType", ""))) continue;
            String g = t.getString("outGroup");
            Integer seq = t.getInteger("outSeq");
            if (g == null || seq == null) continue;
            if (!outEtaVars.containsKey(tid)) continue;
            groups.computeIfAbsent(g, k -> new ArrayList<>()).add(new GroupItem(seq, tid));
        }
        List<IntVar> tardVars = new ArrayList<>();
        for (Map.Entry<String, List<GroupItem>> e : groups.entrySet()) {
            List<GroupItem> items = e.getValue();
            Collections.sort(items, new Comparator<GroupItem>() {
                @Override
                public int compare(GroupItem a, GroupItem b) {
                    return Integer.compare(a.seq, b.seq);
                }
            });
            for (int idx = 0; idx < items.size(); idx++) {
                int tid = items.get(idx).taskId;
                IntVar outEta = outEtaVars.get(tid);
                int due = idx * taktSec;
                IntVar tard = m.newIntVar(0, 1_000_000_000, "TARD_" + e.getKey() + "_" + tid);
                m.addGreaterOrEqual(tard, LinearExpr.sum(new LinearExpr[]{
                        LinearExpr.term(outEta, 1),
                        LinearExpr.constant(-due)
                }));
                m.addGreaterOrEqual(tard, 0);
                tardVars.add(tard);
            }
        }
        if (tardVars.isEmpty()) {
            IntVar z = m.newIntVar(0, 0, "TARD_SUM_ZERO");
            m.addEquality(z, 0);
            return z;
        }
        IntVar s = m.newIntVar(0, 1_000_000_000, "TARD_SUM");
        m.addEquality(s, LinearExpr.sum(tardVars.toArray(new IntVar[0])));
        return s;
    }

    private static int optInt(JSONObject o, String k, int def) {
        if (o == null) return def;
        try {
            Integer v = o.getInteger(k);
            return v == null ? def : v;
        } catch (Exception e) {
            return def;
        }
    }

    private static double optDouble(JSONObject o, String k, double def) {
        if (o == null) return def;
        try {
            Double v = o.getDouble(k);
            return v == null ? def : v;
        } catch (Exception e) {
            return def;
        }
    }

    private static String optStr(JSONObject o, String k, String def) {
        if (o == null) return def;
        try {
            String v = o.getString(k);
            return v == null ? def : v;
        } catch (Exception e) {
            return def;
        }
    }

    private static boolean optBool(JSONObject o, String k, boolean def) {
        if (o == null) return def;
        try {
            Object v = o.get(k);
            if (v == null) return def;
            if (v instanceof Boolean) return (Boolean) v;
            String s = String.valueOf(v).trim().toUpperCase();
            if ("Y".equals(s) || "TRUE".equals(s) || "1".equals(s)) return true;
            if ("N".equals(s) || "FALSE".equals(s) || "0".equals(s)) return false;
            return def;
        } catch (Exception e) {
            return def;
        }
    }

    private static double optDouble(JSONObject o, String k, Double def) {
        return optDouble(o, k, def == null ? 0.0 : def);
    }

    private static long optLong(JSONObject o, String k, long def) {
        if (o == null) return def;
        try {
            Long v = o.getLong(k);
            return v == null ? def : v;
        } catch (Exception e) {
            return def;
        }
    }

    private static final class Edge {
        final double lengthM;
        final Double speedMps;
        Edge(double lengthM, Double speedMps) {
            this.lengthM = lengthM;
            this.speedMps = speedMps;
        }
    }

    private static final class GroupItem {
        final int seq;
        final int taskId;
        GroupItem(int seq, int taskId) {
            this.seq = seq;
            this.taskId = taskId;
        }
    }
}