package com.zy.acs.manager.common.utils;

import com.alibaba.fastjson.JSON;
import com.zy.acs.framework.common.Cools;
import com.zy.acs.manager.core.constant.MapDataConstant;
import com.zy.acs.manager.core.domain.VehicleFootprint;
import com.zy.acs.manager.manager.entity.AgvModel;

import java.util.*;

/**
 * Created by vincent on 8/7/2024
 */
public class MapDataUtils {

    public static Double[][][] preComputeCdaMatrix(String[][] cdaStrMatrix) {
        int rows = cdaStrMatrix.length;
        int cols = cdaStrMatrix[0].length;

        Double[][][] cdaMatrix = new Double[rows][cols][2];
        for (int i = 0; i < rows; i++) {
            for (int j = 0; j < cols; j++) {
                List<Double> cda = MapDataUtils.parseCdaNode(cdaStrMatrix[i][j]);
                cdaMatrix[i][j][0] = cda.get(0);
                cdaMatrix[i][j][1] = cda.get(1);
            }
        }

        return cdaMatrix;
    }

    public static List<String> parseWaveNode(String waveNodeStr) {
        List<String> waveNodeList = new ArrayList<>();
        if (Cools.isEmpty(waveNodeStr)) {
            return waveNodeList;
        }
        return JSON.parseArray(waveNodeStr, String.class);
    }

    public static List<Double> parseCdaNode(String cdaNodeStr) {
        if (Cools.isEmpty(cdaNodeStr)) {
            return new ArrayList<>();
        }
        return JSON.parseArray(cdaNodeStr, Double.class);
    }

    public static List<String> hasOtherWave(List<String> list, String vehicle) {
        if (Cools.isEmpty(list)) {
            return list;
        }
        list.removeIf(next -> next.equals(vehicle));
        return list;
    }

    public static String generateWaveNode(String originStr, String waveNode) {
        List<String> originNode = JSON.parseArray(originStr, String.class);
        Set<String> set = new HashSet<>(originNode);
        set.add(waveNode);
        return JSON.toJSONString(set);
    }

    public static Double getVehicleWaveSafeDistance(Number mm) {
        return getVehicleWaveSafeDistance(mm, null);
    }

    public static Double getVehicleWaveSafeDistance(Number mm, Double factor) {
        if (mm == null) {
            throw new IllegalArgumentException("Invalid map length: " + mm);
        }
        double val = mm.doubleValue();
        if (val <= 0) {
            throw new IllegalArgumentException("Invalid map length: " + mm);
        }
        factor = Optional.ofNullable(factor).orElse(MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR);
        return val * factor;
    }

    public static VehicleFootprint buildFootprint(AgvModel agvModel) {
        if (null == agvModel) {
            throw new IllegalArgumentException("AgvModel is null");
        }
        if (agvModel.getHeadOffset() == null || agvModel.getHeadOffset() <= 0) {
            throw new IllegalArgumentException("Invalid head offset: " + agvModel.getHeadOffset());
        }
        if (agvModel.getTailOffset() == null || agvModel.getTailOffset() <= 0) {
            throw new IllegalArgumentException("Invalid tail offset: " + agvModel.getTailOffset());
        }
        if (agvModel.getWidth() == null || agvModel.getWidth() <= 0) {
            throw new IllegalArgumentException("Invalid width: " + agvModel.getWidth());
        }

        double head = agvModel.getHeadOffset();
        double tail = agvModel.getTailOffset();
        double halfWidth = (double) agvModel.getWidth() / 2;

        return new VehicleFootprint(head, tail, halfWidth);
    }

}