package com.yvan.logisticsModel;

import com.fasterxml.jackson.annotation.JsonIgnore;
import com.galaxis.rcs.common.entity.RcsTaskPlan;
import com.galaxis.rcs.common.enums.LCCDirection;
import com.galaxis.rcs.common.enums.PlanTaskType;
import com.galaxis.rcs.connector.cl2.Cl2DeviceConnector;
import com.galaxis.rcs.plan.PlanTaskSequence;
import com.google.common.collect.Queues;
import lombok.extern.slf4j.Slf4j;
import org.clever.core.Conv;
import org.clever.core.json.JsonWrapper;

import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.locks.LockSupport;

//0.4m/ss // a  max 1.2m/s
//90 = 3.5s cl2
//90 = 5s // cLX
@Slf4j
public class PtrAgvItem extends ExecutorItem {
    private final int BLOCKING_QUEUE_CAPACITY = 100;

    private final Cl2DeviceConnector cl2DeviceConnector = new Cl2DeviceConnector();

    // ip
    public String ip;
    // agv名称
    public String agvName;
    // agv类型
    public String agvType;
    // agv型号
    public String agvModel;
    // AMR功能型号
    public String agvFnModel;
    // agv电量
    public double agvSOC;
    // agv电池电压
    public double agvBatteryVoltage;
    // agv充电状态
    public boolean agvChargingStatus;
    // agv充电电流
    public double agvChargingCurrent;
    // agv放电电流
    public double agvDischargingCurrent;
    // agv电池温度
    public double agvBatteryTemperature;
    // agv当前x坐标
    public double x;
    // agv当前y坐标
    public double y;
    // agv当前z坐标
    public double z;
    // 当前所在站点的逻辑X坐标 Int32
    public int logicX;
    // 当前所在站点的逻辑Y坐标 Int32
    public int logicY;
    // 当前方向 UInt8 0: X轴正向 1: Y轴正向 2: X轴负向 3: Y轴负向 15: 未知方向
    public short direction;
    // agv当前转动角度值
    public double orientation;

    public LCCDirection getLCCDirection() {
        return switch (direction) {
            case 0 -> LCCDirection.RIGHT;
            case 1 -> LCCDirection.DOWN;
            case 2 -> LCCDirection.LEFT;
            case 3 -> LCCDirection.UP;
            default -> null;
        };
    }

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

    /**
     * 当前执行的任务规划列表
     */
    @JsonIgnore
    final BlockingQueue<RcsTaskPlan> planQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);

    @JsonIgnore
    final BlockingQueue<PtrAgvDeviceTask> deviceTaskQueue = Queues.newArrayBlockingQueue(BLOCKING_QUEUE_CAPACITY);


    /**
     * 连接器线程
     */
    private final PtrAgvConnectorThread connectorThread;

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

        if (messageStatus < 2) {
            return;
        }

        boolean needCompute = true;
        for (PtrAgvDeviceTask task : runningDeviceTaskList) {
            if (task.seqNo == seqNo) {
                task.taskGroupStatus = messageStatus;
                if (task.x == x && task.y == y) {
                    task.taskStatus = 4;
                }
            }
            if (task.taskGroupStatus < 3 /*|| task.taskStatus < 4*/) {
                needCompute = false;
            }
        }
        if (needCompute) {
            LockSupport.unpark(connectorThread);
        }
    }


    public void mapReady() {
        this.isMapReady = true;
        this.startConnector();
    }


    /**
     * 启动连接器线程
     */
    public void startConnector() {
        if (!connectorThread.isRunning()) {
            connectorThread.start();
            System.out.println("Connector started for executor: " + this.getId());
        }
    }

    /**
     * 停止连接器线程
     */
    public void stopConnector() {
        connectorThread.stop();
        System.out.println("Connector stopped for executor: " + this.getId());
    }

    private static final int speed = 1000;

    /**
     * 添加任务序列到当前执行器
     */
    public void appendSequence(PlanTaskSequence sequence) {
        if (sequence == null || sequence.taskList.isEmpty()) {
            return;
        }
        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());
        }


        // 生成移动报文
        List<PtrAgvDeviceTask> deviceTaskList = new ArrayList<>();
        List<Map<String, Object>> linkStore = null;
        // 检查 planList 是不是全都是我的任务
        for (RcsTaskPlan plan : sequence.taskList) {
            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 ? CDirection.db : CDirection.dt;
                    if ((d > direction && d - CDirection.dl != direction) || (d < direction && d + CDirection.dl != direction)) {
                        throw new RuntimeException("方向错误");
                    }

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

                } else {
                    throw new RuntimeException("无法识别的点位关系");
                }
                PtrAgvDeviceTask deviceTask = new PtrAgvDeviceTask();
                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.planTaskId = plan.getPlanTaskId();
                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 = CDirection.dr;

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

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

                } else if (r >= 225 && r < 315) {
                    direction = CDirection.db;
                }

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

                PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                deviceTask.operationType = COperationType.transplantLoadAndUnload;
                deviceTask.pickMode = CPickMode.load;
                //处理取货高度
                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 (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;
                        }
                    }
                }

            } else if (plan.getPlanType().equals(PlanTaskType.UNLOAD.toString())) {
                PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                deviceTask.operationType = COperationType.transplantLoadAndUnload;
                deviceTask.pickMode = CPickMode.unload;
                // 处理卸货高度
                StaticItem storeItem = runtime.getStaticItemById(endPointId);
                Map<String, Object> storeItemRaw = storeItem.dt;
                if (storeItemRaw.containsKey("bays") && storeItemRaw.containsKey("level")) {
                    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("levels");
                    deviceTask.goodsSlotHeight = (int) Math.round(levelHeight.get(plan.getTargetLevel()) * 1000);
                } else {
                    deviceTask.goodsSlotHeight = 1;
                }
                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;
                        }
                    }
                }

            } else if (plan.getPlanType().equals(PlanTaskType.CHARGE.toString())) {
                PtrAgvDeviceTask deviceTask = deviceTaskList.get(deviceTaskList.size() - 1);
                deviceTask.operationType = COperationType.charge;
                // 处理充电距离(车的充电口到充电器被压下后的距离、一般被压下20mm)
            }

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

        }
        // 添加结束任务
        PtrAgvDeviceTask deviceTaskEnd = new PtrAgvDeviceTask();
        deviceTaskEnd.isLastTask = true;
        deviceTaskList.add(deviceTaskEnd);

        planQueue.addAll(sequence.taskList);
        deviceTaskQueue.addAll(deviceTaskList);

        String json = JsonWrapper.toJson(deviceTaskList);
        log.info("deviceTaskList: {}", json);

        // TODO: 开启轮询线程，等待下一个待执行任务
    }

    public boolean isFree() {
        return (this.logisticsRuntime.isRunning() && this.deviceTaskQueue.isEmpty() && this.connectorThread.isRunning());
    }

    public PtrAgvItem(LogisticsRuntime logisticsRuntime, Map<String, Object> raw) {
        super(logisticsRuntime, raw);
        this.connectorThread = new PtrAgvConnectorThread(this, this.cl2DeviceConnector, logisticsRuntime);
    }


    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;
    }


    private static class COperationType {
        public static final short move = 0;
        public static final short load = 1;
        public static final short unpick = 2;
        public static final short charge = 3;
        public static final short transplantLoadAndUnload = 4;
        public static final short rollerLoadAndUnload = 5;
    }

    private 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;

    }


}