yvan-lcc-bench/servo/src/main/java/com/galaxis/rcs/amr/AmrAgvItem.java

package com.galaxis.rcs.amr;

import com.galaxis.rcs.common.entity.RcsTaskPlan;
import com.galaxis.rcs.common.enums.*;
import com.galaxis.rcs.connector.amr.AmrDeviceConnector;
import com.galaxis.rcs.plan.PlanTaskSequence;
import com.galaxis.rcs.plan.path.PathUtils;
import com.galaxis.rcs.amr.receiveEntity.AmrHeartbeatMessage;
import com.galaxis.rcs.amr.receiveEntity.base.CurBatteryData;
import com.galaxis.rcs.amr.sendEntity.RcsConfigMessage;
import com.galaxis.rcs.amr.sendEntity.RcsSRMessage;
import com.galaxis.rcs.amr.sendEntity.RcsSetLocationMessage;
import com.google.common.collect.Queues;
import com.yvan.entity.AgvStatusVo;
import com.yvan.logisticsModel.ExecutorItem;
import com.yvan.logisticsModel.LogisticsRuntime;
import com.yvan.logisticsModel.StaticItem;
import lombok.Getter;
import lombok.SneakyThrows;
import lombok.extern.slf4j.Slf4j;
import org.clever.core.json.JsonWrapper;
import org.clever.data.redis.Redis;
import org.clever.data.redis.RedisAdmin;

import java.util.*;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.locks.LockSupport;

@Slf4j
public abstract class AmrAgvItem extends ExecutorItem {
    private static final int BLOCKING_QUEUE_CAPACITY = 100;
    private static final Redis redis = RedisAdmin.getRedis();

    // ip
    public volatile String ip;
    // agv名称
    public volatile String agvName;
    // agv类型
    public volatile String agvType;
    // agv型号
    public volatile String agvModel;
    // AMR功能型号
    public volatile String agvFnModel;
    // 电池信息
    public volatile CurBatteryData battery;
    // agv当前x坐标
    public volatile double x;
    // agv当前y坐标
    public volatile double y;
    // agv当前z坐标
    public volatile double z;
    // 当前所在站点的逻辑X坐标 Int32
    public volatile int logicX;
    // 当前所在站点的逻辑Y坐标 Int32
    public volatile int logicY;
    // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
    public volatile short direction;
    // agv当前转动角度值
    public volatile double orientation;

    private volatile boolean isPaused = false;

    private volatile PosDirection lastPausedPosition;

    // 任务模式
    @Getter
    private volatile AmrTaskMode __taskMode;


    // 执行中的任务
    public List<AmrDeviceTask> runningDeviceTaskList = new ArrayList<>();

    /**
     * 当前执行的任务规划列表
     */
    public volatile PlanTaskSequence planTaskSequence;

    /**
     * 当前执行的设备任务列表
     */
    public final BlockingQueue<AmrDeviceTask> deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);

    final AmrDeviceConnector amrDeviceConnector = new AmrDeviceConnector(this.runtime);

    public final AmrMessageHandler amrMessageHandler;

    /**
     * 连接器线程
     */
    public AmrConnectorThread connectorThread;

    public AmrAgvItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
        super(logisticsRuntime, raw);
        this.connectorThread = new AmrConnectorThread(this, this.amrDeviceConnector, logisticsRuntime);
        this.amrMessageHandler = logisticsRuntime.amrMessageHandler;
    }

    @Override
    public boolean isRunning() {
        return connectorThread.isRunning();
    }

    public abstract RcsConfigMessage getConfig();


    @Override
    public void start() {
        this.amrMessageHandler.registeHeartBeatSet(this);

        // 查询当前状态
        requestCurrentStatus();
        this.isRunning = true;

        this.startConnector();
    }

    @Override
    public void stop() {
        // 停止连接器线程
        stopConnector();

        this.amrMessageHandler.unregisteHeartBeatSet(this);

        // 清理任务序列
        if (planTaskSequence != null) {
            planTaskSequence = null;
        }
        // 清理设备任务队列
        deviceTaskQueue.clear();
        // 清理运行中的设备任务列表
        runningDeviceTaskList.clear();
        // 更新Redis状态
        updateRedisStatus();
    }

    public synchronized void dispatchTask(PlanTaskSequence taskSequence) {
        if (!isFree()) {
            if (!this.runtime.isRunning()) {
                throw new IllegalStateException("runtime is not running!");
            }
            if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) {
                throw new IllegalStateException("AGV is busy with an existing task sequence");
            }
            if (!deviceTaskQueue.isEmpty()) {
                throw new IllegalStateException("AGV has pending device tasks in the queue");
            }
            if (!isOnline) {
                throw new IllegalStateException("AGV is not online and cannot accept new tasks");
            }

            throw new IllegalStateException("AGV is not free to accept new tasks");
        }

        this.planTaskSequence = taskSequence;
        buildPlanToDeviceTask();
        this.runtime.eventManager.firePlanTaskSequenceAcceptEvent(this, taskSequence);
        connectorThread.resumeProcessing();
    }

    public synchronized void pauseTask() {
        if (planTaskSequence == null) {
            throw new IllegalStateException("No active task to pause");
        }

        isPaused = true;
        lastPausedPosition = new PosDirection(logicX, logicY, PathUtils.getDirectionByArmDirection(direction));

        // 发送停止指令
        RcsSRMessage stopMsg = new RcsSRMessage(this.runtime);
        stopMsg.SeqNo = amrMessageHandler.getNewSeqNo();
        stopMsg.OperationCode = 0; // 停止
        stopMsg.StopX = logicX;
        stopMsg.StopY = logicY;

        try {
            amrMessageHandler.sendCmdSR(this.getId(), stopMsg);
        } catch (Exception e) {
            log.error("Failed to send stop command to AGV {}", this.getId(), e);
        }

        this.runtime.eventManager.firePlanTaskSequencePauseEvent(this, planTaskSequence);
    }

    public synchronized void resumeTask() {
        if (!isPaused) {
            throw new IllegalStateException("Task is not paused");
        }

        // 检查当前位置是否与暂停位置一致
        if (Math.abs(logicX - lastPausedPosition.logicX()) > 1 ||
            Math.abs(logicY - lastPausedPosition.logicY()) > 1 ||
            PathUtils.getDirectionByArmDirection(direction) != lastPausedPosition.direction()) {
            // 需要返回暂停位置
            throw new RuntimeException("AGV position has changed since pause, cannot resume task safely");
        }

        isPaused = false;
        connectorThread.resumeProcessing();
        this.runtime.eventManager.firePlanTaskSequenceResumeEvent(this, planTaskSequence);
    }

    @SneakyThrows
    public synchronized void cancelTask() {
        // 发送取消指令
        amrMessageHandler.sendCmdCancelTask(this.getId(), this.connectorThread.getCurrentTaskSeqNo());

        if (planTaskSequence != null) {
            planTaskSequence = null;
        }
        if (!deviceTaskQueue.isEmpty()) {
            deviceTaskQueue.clear();
        }
        // todo 取消运行中的设备任务
        runningDeviceTaskList.clear();
        // 唤醒连接器线程
        LockSupport.unpark(connectorThread);
        this.runtime.eventManager.firePlanTaskSequenceCancelEvent(this, planTaskSequence);
    }

    @SneakyThrows
    public void setPositionAndDirection(int x, int y, short direction) {
        RcsSetLocationMessage setLoc = new RcsSetLocationMessage(amrMessageHandler.getNewSeqNo());
        setLoc.X = (short) x;
        setLoc.Y = (short) y;
        setLoc.Direction = direction;

        amrMessageHandler.sendCmdSetLocation(this.getId(), setLoc);
    }

    @SneakyThrows
    public void requestCurrentStatus() {
        amrMessageHandler.sendCmdQueryStatus(this.getId());
    }

    public boolean isFree() {
        // return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning());
        if (!this.runtime.isRunning()) {
            return false;
        }
        if (planTaskSequence != null && !planTaskSequence.isAllCompleted()) {
            return false;
        }
        if (!deviceTaskQueue.isEmpty()) {
            return false;
        }
        if (this.isPaused) {
            return false;
        }
        // if (this.taskMode != AmrTaskMode.AMR_FREE_MODE) {
        //     return false;
        // }
        return this.isOnline;
    }

    public void taskCompleted(int logicX, int logicY, short direction, int taskStatus) {

        updatePosition(logicX, logicY, direction);
        // 查找当前分组任务
        for (AmrDeviceTask task : runningDeviceTaskList) {
            task.taskGroupStatus = taskStatus;
            if (taskStatus == 4) {
                if (task.taskStatus != 4) {
                    this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task);
                    task.taskStatus = 4;
                }

                // 更新计划任务
                List<RcsTaskPlan> planTaskList = planTaskSequence.taskList.stream().filter(pt -> task.movePlanTaskId.equals(pt.getPlanTaskId()) || task.planTaskIdSet.contains(pt.getPlanTaskId())).toList();
                for (RcsTaskPlan planTask : planTaskList) {
                    if (PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) {
                        continue;
                    }

                    planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
                    this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask);
                }
            }
        }

        if (planTaskSequence != null && planTaskSequence.isAllCompleted()) {
            this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence);
            this.runningDeviceTaskList.clear();
            planTaskSequence = null;
        }
        LockSupport.unpark(connectorThread);
    }

    public void updatePosition(int logicX, int logicY, short direction) {
        int oldX = this.logicX;
        int oldY = this.logicY;
        short oldDirection = this.direction;

        this.logicX = logicX;
        this.logicY = logicY;
        this.direction = direction;

        LCCDirection oldLccDirection = PathUtils.getDirectionByArmDirection(oldDirection);
        LCCDirection newLccDirection = PathUtils.getDirectionByArmDirection(direction);

        // 更新Redis
        updateRedisStatus();

        // 触发位置变化事件
        if (oldX != logicX || oldY != logicY) {
            this.runtime.eventManager.firePosChangedEvent(this,
                new PosDirection(logicX, logicY, newLccDirection),
                new PosDirection(oldX, oldY, oldLccDirection));
        }

        if (oldDirection != direction) {
            this.runtime.eventManager.fireDirectionChangedEvent(this, newLccDirection, oldLccDirection);
        }

        boolean needCompute = false;

        // 从 runningDeviceTaskList 里面，找到完成到什么阶段
        // 比如 (1,2) -> (2,2) -> (3,2) ,  如果 updatePosition=3,2 ，那么前2个任务都要完成
        int finishTargetIndex = -1;
        if (this.runningDeviceTaskList != null && !this.runningDeviceTaskList.isEmpty() &&
            this.planTaskSequence != null && !this.planTaskSequence.isEmpty()) {

            for (int i = 0; i < runningDeviceTaskList.size(); i++) {
                AmrDeviceTask task = runningDeviceTaskList.get(i);
                if (task.checkLogicX == logicX && task.checkLogicY == logicY && task.direction == this.direction) {
                    if (task.taskStatus < 4) {
                        finishTargetIndex = i;
                    }
                    break;
                }
            }

            if (finishTargetIndex >= 0) {
                needCompute = true;
                // 标记前面的任务都完成了
                for (int i = 0; i <= finishTargetIndex; i++) {
                    AmrDeviceTask task = runningDeviceTaskList.get(i);
                    task.taskStatus = 4; // 标记为完成
                    this.runtime.eventManager.fireDeviceTaskCompleteEvent(this, task);

                    // 更新计划任务
                    RcsTaskPlan planTask = planTaskSequence.getByPlanTaskId(task.movePlanTaskId);
                    if (planTask != null && !PlanTaskStatus.FINISHED.toString().equals(planTask.getPlanTaskStatus())) {
                        planTask.setPlanTaskStatus(PlanTaskStatus.FINISHED.toString());
                        this.runtime.eventManager.firePlanTaskCompleteEvent(this, planTaskSequence, planTask);
                    }
                }

                if (planTaskSequence.isAllCompleted()) {
                    this.runtime.eventManager.firePlanTaskSequenceCompleteEvent(this, planTaskSequence);
                    this.runningDeviceTaskList.clear();
                    planTaskSequence = null;
                }
            }
        }

        // BannerUtils.printConfig(log, "updatePosition", new String[]{
        //     "logicX: " + logicX,
        //     "logicY: " + logicY,
        //     "direction: " + direction,
        //     "finishTargetIndex: " + finishTargetIndex,
        //     "runningDeviceSize:" + (this.runningDeviceTaskList == null ? "null" : this.runningDeviceTaskList.size()),
        //     "planTask:" + (this.planTaskSequence == null ? "null" :
        //         ("\n" + Joiner.on("\n").join((List) this.planTaskSequence.toPrettyMap().get("items")))
        //     )
        // });

        if (needCompute && this.runningDeviceTaskList.size() > 0) {
            int index = this.runningDeviceTaskList.size() - 1;
            AmrDeviceTask task = this.runningDeviceTaskList.get(index);
            if (task.groupEndPoint != task.endPoint) {
                LockSupport.unpark(connectorThread);
            }
        }
    }

    /**
     * 更新设备任务状态 暂时没有处理任务取消相关的状态
     *
     * @param seqNo
     * @param x
     * @param y
     * @param messageStatus
     */
    public void updateDeviceTaskStatus(int seqNo, int x, int y, int messageStatus) {
        // 更新任务状态逻辑
        if (messageStatus < 2) {
            return;
        }

        // 任务完成逻辑，在地标检查里
    }

    public void updateTaskMode(int taskMode) {
        this.setTaskMode(AmrTaskMode.fromValue(taskMode));
    }

    private void setTaskMode(AmrTaskMode taskMode) {
        var originalMode = this.__taskMode;
        this.__taskMode = taskMode;
        this.runtime.eventManager.fireModeChangeEvent(this, taskMode, originalMode);
        updateRedisStatus();
    }

    @SneakyThrows
    public void updateRedisStatus() {
        String statusKey = getRedisKey("status");
        var state = this.getState();
        var statusMap = new JsonWrapper(state).getInnerMap();
        redis.hPutAll(statusKey, statusMap);
    }

    public void handleHeartbeat(AmrHeartbeatMessage heartbeat) {
        // 更新在线状态
        String aliveKey = getRedisKey("alive");
        redis.vSet(aliveKey, this.runtime.serverId);
        redis.kExpire(aliveKey, 5); // 5秒过期

        // 更新状态信息
        if (this.battery == null) {
            this.battery = new CurBatteryData();
        }
        this.battery.SOC = heartbeat.Battery;
        this.battery.setTemperature(heartbeat.Temperature.Battery);

        // 检查低电量
        if (this.battery.SOC < 20) {
            this.runtime.eventManager.fireLowBatteryEvent(this);
        }

        updateRedisStatus();
    }

    public void handleOnlineEvent() {
        isOnline = true;
        this.runtime.eventManager.fireOnlineEvent(this);
        requestCurrentStatus();
    }

    public void handleOfflineEvent() {
        isOnline = false;
        this.runtime.eventManager.fireOfflineEvent(this);
    }

    public String getTaskStatus() {
        if (planTaskSequence == null) return "IDLE";
        if (isPaused) return "PAUSED";
        return "EXECUTING";
    }


    /**
     * 启动连接器线程
     */
    public void startConnector() {
        if (!connectorThread.isRunning()) {
            connectorThread.start();
        }
    }

    /**
     * 停止连接器线程
     */
    public void stopConnector() {
        connectorThread.stop();
    }

    private static final int speed = 1000;

    /**
     * 添加任务序列到当前执行器
     */
    protected void buildPlanToDeviceTask() {
        PlanTaskSequence sequence = this.planTaskSequence;
        LogisticsRuntime runtime = sequence.logisticsRuntime;

        short direction = this.direction;

        // 获取当前设备点位(逻辑点位)
        StaticItem startPoint = runtime.getStaticItemByLogicXY(this.logicX, this.logicY);
        if (startPoint == null) {
            log.error("Cl2DeviceConnector robotMove: executorItem={}, task={}, agv当前点位为空 地图上没有标记", this.getId(), sequence.bizTask.getBizTaskId());
        }
        StaticItem groupStartPoint = startPoint;

        Set<Long> rotationPlanTaskIdSet = new HashSet<>();

        // 生成移动报文
        List<AmrDeviceTask> deviceTaskList = new ArrayList<>();
        List<Map<String, Object>> linkStore = null;
        // 检查 planList 是不是全都是我的任务
        for (int i = 0; i < sequence.taskList.size(); i++) {
            RcsTaskPlan plan = sequence.taskList.get(i);
            String endPointId = plan.getTargetId();

            if (plan.getPlanType().equals(PlanTaskType.MOVE.toString()) || plan.getPlanType().equals(PlanTaskType.MOVE_BACKWARD.toString())) {
                // 获取目标点信息
                StaticItem pointItem = runtime.getStaticItemById(endPointId);
                linkStore = (List<Map<String, Object>>) pointItem.dt.get("linkStore");
                int d = -1;
                if (startPoint.logicX == pointItem.logicX && startPoint.logicY != pointItem.logicY) {
                    d = pointItem.logicY >= startPoint.logicY ? AmrAgvItem.CDirection.db : AmrAgvItem.CDirection.dt;
                    if ((d > direction && d - AmrAgvItem.CDirection.dl != direction) || (d < direction && d + AmrAgvItem.CDirection.dl != direction)) {
                        throw new RuntimeException("方向错误");
                    }

                } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX != pointItem.logicX) {
                    d = pointItem.logicX >= startPoint.logicX ? AmrAgvItem.CDirection.dr : AmrAgvItem.CDirection.dl;
                    if ((d > direction && d - AmrAgvItem.CDirection.dl != direction) || (d < direction && d + AmrAgvItem.CDirection.dl != direction)) {
                        throw new RuntimeException("方向错误");
                    }
                    // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());

                } else if (startPoint.logicY == pointItem.logicY && startPoint.logicX == pointItem.logicX) {
                    d = direction;
                    // distance += Math.abs(pointItem.getTransformationX() - startPoint.getTransformationX());

                } else {
                    throw new RuntimeException("无法识别的点位关系");
                }
                AmrDeviceTask deviceTask = new AmrDeviceTask();
                deviceTask.x = pointItem.logicX;
                deviceTask.y = pointItem.logicY;
                deviceTask.speed = d == direction ? (speed) : (-speed);
                deviceTask.direction = direction;
                deviceTask.pickMode = 0;
                deviceTask.startPoint = startPoint;
                deviceTask.endPoint = pointItem;
                deviceTask.bizTaskId = plan.getBizTaskId();
                deviceTask.movePlanTaskId = plan.getPlanTaskId();
                deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
                rotationPlanTaskIdSet.clear();
                // 行走任务完成后，检查用的字段
                deviceTask.checkLogicX = pointItem.logicX;
                deviceTask.checkLogicY = pointItem.logicY;
                deviceTaskList.add(deviceTask);
                // 设置新的起点
                startPoint = pointItem;

            } else if (plan.getPlanType().equals(PlanTaskType.ROTATION.toString())) {

                float r = plan.getTargetRotation().floatValue();
                while (r > 360) {
                    r -= 360;
                }
                while (r < 0) {
                    r += 360;
                }

                if (r >= 315 || r < 45) {
                    direction = AmrAgvItem.CDirection.dr;

                } else if (r >= 45 && r < 135) {
                    direction = AmrAgvItem.CDirection.dt;

                } else if (r >= 135 && r < 225) {
                    direction = AmrAgvItem.CDirection.dl;

                } else if (r >= 225 && r < 315) {
                    direction = AmrAgvItem.CDirection.db;
                }
                rotationPlanTaskIdSet.add(plan.getPlanTaskId());

            } else if (plan.getPlanType().equals(PlanTaskType.LOAD.toString())) {

                if (deviceTaskList.isEmpty()) {
                    AmrDeviceTask deviceTask = new AmrDeviceTask();
                    deviceTask.x = startPoint.logicX;
                    deviceTask.y = startPoint.logicY;
                    deviceTask.speed = speed;
                    deviceTask.direction = direction;
                    deviceTask.pickMode = 0;
                    deviceTask.startPoint = startPoint;
                    deviceTask.endPoint = startPoint;
                    deviceTask.bizTaskId = plan.getBizTaskId();
                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
                    rotationPlanTaskIdSet.clear();
                    // 行走任务完成后，检查用的字段
                    deviceTask.checkLogicX = startPoint.logicX;
                    deviceTask.checkLogicY = startPoint.logicY;
                    deviceTaskList.add(deviceTask);
                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
                }
                AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                deviceTask.operationType = AmrAgvItem.COperationType.transplantLoadAndUnload;
                deviceTask.pickMode = AmrAgvItem.CPickMode.load;
                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
                //处理取货高度
                StaticItem storeItem = runtime.getStaticItemById(endPointId);
                Map<String, Object> storeItemRaw = storeItem.dt;
                if (storeItemRaw.containsKey("bays")) {
                    List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
                    Map<String, Object> bay = bays.get(plan.getTargetBay());
                    List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
                } else {
                    deviceTask.goodsSlotHeight = 1;
                }
                if (sequence.getUnloadHeight() > 0d) {
                    deviceTask.goodsSlotHeight = (int) Math.round(sequence.getLoadHeight() * 1000);
                    log.info("option.loadHeight = {}", deviceTask.goodsSlotHeight);
                }
                if (linkStore != null) {
                    for (Map<String, Object> store : linkStore) {
                        if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) {
                            short d = 0;
                            switch (store.get("direction").toString()) {
                                case "up":
                                    d = 1;
                                    break;
                                case "right":
                                    d = 2;
                                    break;
                                case "down":
                                    d = 3;
                                    break;
                                case "left":
                                    d = 0;
                                    break;
                            }
                            deviceTask.goodsSlotDirection = d;
                        }
                    }
                }

                // 标记任务分组结束
                deviceTask.isGroupEnd = true;
                deviceTask.groupEndPoint = deviceTask.endPoint;
                deviceTask.groupStartPoint = groupStartPoint;
                groupStartPoint = deviceTask.endPoint;

            } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
                if (deviceTaskList.isEmpty()) {
                    AmrDeviceTask deviceTask = new AmrDeviceTask();
                    deviceTask.x = startPoint.logicX;
                    deviceTask.y = startPoint.logicY;
                    deviceTask.speed = speed;
                    deviceTask.direction = direction;
                    deviceTask.pickMode = 0;
                    deviceTask.startPoint = startPoint;
                    deviceTask.endPoint = startPoint;
                    deviceTask.bizTaskId = plan.getBizTaskId();
                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
                    rotationPlanTaskIdSet.clear();
                    // 行走任务完成后，检查用的字段
                    deviceTask.checkLogicX = startPoint.logicX;
                    deviceTask.checkLogicY = startPoint.logicY;
                    deviceTaskList.add(deviceTask);
                    linkStore = (List<Map<String, Object>>) startPoint.dt.get("linkStore");
                }
                AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                deviceTask.operationType = AmrAgvItem.COperationType.transplantLoadAndUnload;
                deviceTask.pickMode = AmrAgvItem.CPickMode.unload;
                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
                // 处理卸货高度
                StaticItem storeItem = runtime.getStaticItemById(endPointId);
                Map<String, Object> storeItemRaw = storeItem.dt;
                if (storeItemRaw.containsKey("bays")) {
                    List<Map<String, Object>> bays = (List<Map<String, Object>>) storeItemRaw.get("bays");
                    Map<String, Object> bay = bays.get(plan.getTargetBay());
                    List<Double> levelHeight = (List<Double>) bay.get("levelHeight");
                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
                } else {
                    deviceTask.goodsSlotHeight = 1;
                }
                if (sequence.getUnloadHeight() > 0d) {
                    deviceTask.goodsSlotHeight = (int) Math.round(sequence.getUnloadHeight() * 1000);
                    log.info("option.unloadHeight = {}", deviceTask.goodsSlotHeight);
                }
                if (linkStore != null) {
                    for (Map<String, Object> store : linkStore) {
                        if (store.get("item").equals(plan.getTargetId()) && store.get("level").equals(plan.getTargetLevel()) && store.get("bay").equals(plan.getTargetBay()) && store.get("cell").equals(plan.getTargetCell())) {
                            short d = 0;
                            switch (store.get("direction").toString()) {
                                case "up":
                                    d = 1;
                                    break;
                                case "right":
                                    d = 2;
                                    break;
                                case "down":
                                    d = 3;
                                    break;
                                case "left":
                                    d = 0;
                                    break;
                            }
                            deviceTask.goodsSlotDirection = d;
                        }
                    }
                }
                // 标记任务分组结束
                deviceTask.isGroupEnd = true;
                deviceTask.groupEndPoint = deviceTask.endPoint;
                deviceTask.groupStartPoint = groupStartPoint;
                groupStartPoint = deviceTask.endPoint;

            } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
                if (deviceTaskList.isEmpty()) {
                    AmrDeviceTask deviceTask = new AmrDeviceTask();
                    deviceTask.x = startPoint.logicX;
                    deviceTask.y = startPoint.logicY;
                    deviceTask.speed = speed;
                    deviceTask.direction = direction;
                    deviceTask.pickMode = 0;
                    deviceTask.startPoint = startPoint;
                    deviceTask.endPoint = startPoint;
                    deviceTask.bizTaskId = plan.getBizTaskId();
                    deviceTask.movePlanTaskId = plan.getPlanTaskId();
                    deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
                    rotationPlanTaskIdSet.clear();
                    // 行走任务完成后，检查用的字段
                    deviceTask.checkLogicX = startPoint.logicX;
                    deviceTask.checkLogicY = startPoint.logicY;
                    deviceTaskList.add(deviceTask);
                }
                AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                deviceTask.operationType = AmrAgvItem.COperationType.charge;
                deviceTask.planTaskIdSet.add(plan.getPlanTaskId());
                // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm)
                deviceTask.chargeDirection = 2;
                deviceTask.chargeLocation = 200;
                // 标记任务分组结束
                deviceTask.isGroupEnd = true;
                deviceTask.groupEndPoint = deviceTask.endPoint;
                deviceTask.groupStartPoint = groupStartPoint;
                groupStartPoint = deviceTask.endPoint;
            }

            if (!plan.getExecutorId().equals(this.getId())) {
                throw new RuntimeException("plan not belong executor:" + this.getId() + ", " + plan.getExecutorId());
            }

        }

        if (deviceTaskList.isEmpty()) {
            AmrDeviceTask deviceTask = new AmrDeviceTask();
            deviceTask.x = startPoint.logicX;
            deviceTask.y = startPoint.logicY;
            deviceTask.speed = speed;
            deviceTask.direction = direction;
            deviceTask.pickMode = 0;
            deviceTask.startPoint = startPoint;
            deviceTask.endPoint = startPoint;
            // 行走任务完成后，检查用的字段
            deviceTask.checkLogicX = startPoint.logicX;
            deviceTask.checkLogicY = startPoint.logicY;
            deviceTaskList.add(deviceTask);
        }

        // 标记任务分组结束
        AmrDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
        deviceTask.groupEndPoint = deviceTask.endPoint;
        deviceTask.groupStartPoint = groupStartPoint;
        deviceTask.isGroupEnd = true;
        // 最后一个规划任务为旋转时需要添加一个endDirection
        if (rotationPlanTaskIdSet.size() > 0) {
            if (deviceTask.operationType <= AmrAgvItem.COperationType.move) {
                deviceTask.operationType = AmrAgvItem.COperationType.move;
                deviceTask.pickMode = AmrAgvItem.CPickMode.normal;
            }
            deviceTask.endDirection = direction;
            deviceTask.planTaskIdSet.addAll(rotationPlanTaskIdSet);
            if (deviceTask.movePlanTaskId == null) {
                deviceTask.movePlanTaskId = 0L;
            }
            rotationPlanTaskIdSet.clear();
        }

        // 反向标记任务组
        int lastIndex = deviceTaskList.size() - 1;

        for (int i = deviceTaskList.size() - 1; i >= 0; i--) {
            AmrDeviceTask d = deviceTaskList.get(i);
            if (d.operationType > 0) {
                lastIndex = i;
                break;
            }
        }


        for (int i = deviceTaskList.size() - 1; i >= 0; i--) {
            AmrDeviceTask d = deviceTaskList.get(i);
            if (d.isGroupEnd && d.operationType > AmrAgvItem.COperationType.move) {
                lastIndex = i;
            } else {
                d.operationType = deviceTaskList.get(lastIndex).operationType;
                d.pickMode = deviceTaskList.get(lastIndex).pickMode;
                d.groupStartPoint = deviceTaskList.get(lastIndex).groupStartPoint;
                d.groupEndPoint = deviceTaskList.get(lastIndex).groupEndPoint;
                d.goodsSlotHeight = deviceTaskList.get(lastIndex).goodsSlotHeight;
                d.goodsSlotDirection = deviceTaskList.get(lastIndex).goodsSlotDirection;
                d.chargeDirection = deviceTaskList.get(lastIndex).chargeDirection;
                d.chargeLocation = deviceTaskList.get(lastIndex).chargeLocation;
            }
        }
        handelDeviceTask(deviceTaskList);
    }

    abstract public void handelDeviceTask(List<AmrDeviceTask> deviceTaskList);

    public boolean isSamePosition(PosDirection startPos) {
        return this.logicX == startPos.logicX() && this.logicY == startPos.logicY() &&
            PathUtils.getDirectionByArmDirection(this.direction) == startPos.direction();
    }

    private static class CDirection {
        private static final short dr = 0;
        private static final short db = 1;
        private static final short dl = 2;
        private static final short dt = 3;
    }


    public static class COperationType {
        // 移动
        public static final short move = 0;
        // 载货
        public static final short load = 1;
        // 卸货
        public static final short unload = 2;
        // 充电
        public static final short charge = 3;
        // 移载取放货
        public static final short transplantLoadAndUnload = 4;
        // 滚轮取放货
        public static final short rollerLoadAndUnload = 5;
    }

    public static class CPickMode {
        // 无动作
        public static final short normal = 0;
        // 载货
        public static final short load = 1;
        // 卸货
        public static final short unload = 2;
        // 调高
        public static final short adjustHeight = 3;
        // 调高准备取货
        public static final short adjustHeightToLoad = 5;
        // 调高准备卸货
        public static final short adjustHeightToUnload = 6;
    }


    /**
     * 从 AMR 方向转换为 LCC 方向枚举
     *
     * @return
     */
    public LCCDirection getLCCDirection() {
        return switch (direction) {
            case 0 -> LCCDirection.RIGHT;
            case 1 -> LCCDirection.DOWN;
            case 2 -> LCCDirection.LEFT;
            case 3 -> LCCDirection.UP;
            default -> null;
        };
    }

    public short getAmrDirection(LCCDirection lccDirection) {
        return switch (lccDirection) {
            case RIGHT -> 0;
            case DOWN -> 1;
            case LEFT -> 2;
            case UP -> 3;
            default -> -1; // 未知方向
        };
    }

    private String getRedisKey(String type) {
        return String.format("lcc:%s:%s:device:%s:%s",
            runtime.projectUuid, runtime.envId, this.getId(), type);
    }

    public AgvStatusVo getState() {
        var ptr = this;

        var state = new AgvStatusVo();
        state.setId(ptr.id);
        state.setType(ptr.getT());
        state.setIsOnline(ptr.isOnline);
        state.setIsSystemManaged(ptr.isSystemManaged);

        state.setX(ptr.x);
        state.setY(ptr.y);
        state.setZ(ptr.z);
        state.setLogicX(ptr.logicX);
        state.setLogicY(ptr.logicY);

        state.setDirection(PathUtils.getDirectionByArmDirection(ptr.direction));
        state.setOrientation(ptr.orientation);
        state.setSoc(ptr.battery == null ? -1 : ptr.battery.SOC);
        state.setMode(ptr.get__taskMode());
        state.setTaskStatus(ptr.getTaskStatus());

        state.setIsBlocked(ptr.isBlocked);
        if (ptr.planTaskSequence != null) {
            state.setTaskCompleted(ptr.planTaskSequence.completedCount());
            state.setTaskTotalCount(ptr.planTaskSequence.taskTotalCount());
            state.setBizTaskId(ptr.planTaskSequence.bizTask.getBizTaskId());
            state.setBizTaskType(BizTaskType.fromString(ptr.planTaskSequence.bizTask.getBizType()));
            state.setBizTaskStatus(BizTaskStatus.fromString(ptr.planTaskSequence.bizTask.getBizTaskStatus()));
            state.setBizTaskFrom(ptr.planTaskSequence.bizTask.getTaskFrom());
            state.setBizTaskTo(ptr.planTaskSequence.bizTask.getTaskTo());
            state.setBizLpn(ptr.planTaskSequence.bizTask.getLpn());

            state.setLoadBasLocationVo(ptr.planTaskSequence.loadBasLocationVo);
            state.setUnloadBasLocationVo(ptr.planTaskSequence.unloadBasLocationVo);
        }

        return state;
    }
}