fdkk_meta/src/main/java/com/fdkk/fdkkmeta/util/kesar/GetRoute.java

package com.fdkk.fdkkmeta.util.kesar;

import cn.hutool.core.io.LineHandler;
import cn.hutool.core.io.file.FileReader;
import cn.hutool.core.util.ObjectUtil;
import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONArray;
import com.alibaba.fastjson.JSONObject;
import com.fdkk.fdkkmeta.domain.dto.RouteDto;
import com.fdkk.fdkkmeta.domain.po.RoutePO;
import lombok.extern.slf4j.Slf4j;

import java.util.*;

@Slf4j
public class GetRoute {


    private static final String inputFilePath = "E:\\test\\project\\age_laser\\routeMap.txt";


    private static final Double startX = 5.358192084229412;
    private static final Double startY = -7.905951689748807;
    private static final Double startZ = -1.3145928255248511;

    private static final Double endX = -2.143694831230416;
    private static final Double endY = -3.3754012098200965;
    private static final Double endZ = -1.1803865408990568;

    private static RoutePO minStartId = null;
    private static RoutePO minEndId = null;

    private static Node start = null;
    private static Node end = null;
    private static final AStar g_AStar = new AStar();


    /**
     * 读取文件，获取文件内容
     * target_freespace.json
     * <p>
     * list 是routeMap的结果集
     *
     * @return
     */
    public static JSONArray getRoute(String initPath, RouteDto dto) {
        try {
            Map<Integer, RoutePO> maps = GetRoute.readMap(initPath, dto);
            MapInfo info = new MapInfo(maps, GetRoute.start, GetRoute.end);
            List<Node> path = GetRoute.g_AStar.start(info);
            List<Integer> ids = new LinkedList<>();
            for (Node node : path) {
                ids.add(node.id);
            }
            GetRoute.log.info("棋盘获取路线点-所有点{}", JSON.toJSONString(ids));
            JSONArray jsonArray = GetRoute.convertFromPath(path, dto);
            List<Integer> ids2 = new LinkedList<>();
            for (Object o : jsonArray) {
                JSONObject a = JSON.parseObject(JSONObject.toJSON(o).toString());
                if (a.getInteger("id") != null) {
                    ids2.add(a.getInteger("id"));
                }
            }
            GetRoute.log.info(JSON.toJSONString(ids));
            GetRoute.log.info(JSON.toJSONString(ids2));
            return jsonArray;
        } catch (Exception e) {
            e.printStackTrace();
        }
        return null;
    }


    public static Map<Integer, RoutePO> readMap(String initPath, RouteDto dto)   {

        Double startX = dto.getS_location().getX();
        Double startY = dto.getS_location().getY();
        Double startZ = dto.getS_location().getZ();

        Double endX = dto.getE_location().getX();
        Double endY =  dto.getE_location().getY();
        Double endZ =  dto.getE_location().getZ();


        Coord _start = new Coord(startX, startY, startZ);
        Coord _end = new Coord(endX, endY, endZ);

        final Double[] startDistance = {1000.0};
        final Double[] endDistance = {1000.0};

        FileReader fileReader = new FileReader(initPath);
        Map<Integer, RoutePO> collect = new HashMap<>();
        fileReader.readLines((LineHandler) s -> {
            RoutePO po = JSON.toJavaObject(JSON.parseObject(s), RoutePO.class);
            Coord coord = new Coord(Double.valueOf(po.getX()), Double.valueOf(po.getY()), Double.valueOf(po.getZ()));
            Double _startDistance = GetRoute.g_AStar.calcH(_start, coord);
            if (_startDistance <= startDistance[0]) {
                GetRoute.minStartId = po;
                startDistance[0] = _startDistance;
            }
            Double _endDistance = GetRoute.g_AStar.calcH(_end, coord);
            if (_endDistance <= endDistance[0]) {
                GetRoute.minEndId = po;
                endDistance[0] = _endDistance;
            }
            collect.put(po.getId(), po);
        });
        GetRoute.log.info("数量： {}", collect.size());
        GetRoute.log.info("棋盘获取路线点-开始点{},{}", _start.x, _start.y);
        GetRoute.log.info("棋盘获取路线点-结束点{},{}", _end.x, _end.y);
        //  开始结束点一样就直接返回
        if (ObjectUtil.isNull(minStartId)){
            GetRoute.log.info("找不到开始点");
            return null;
        }
        if (ObjectUtil.isNull(minEndId)){
            GetRoute.log.info("找不到结束点");
            return null;
        }

        if (minStartId.getId()==minEndId.getId()){
            GetRoute.log.info("超过1.1米就找不到路径startDistance {},endDistance{}", startDistance[0], endDistance[0]);
            GetRoute.log.info("开始最近距离{} ", startDistance[0]);
            GetRoute.log.info("结束最近距离{}", endDistance[0]);
            return null;
        }

        Coord startVirtualCoord = new Coord(GetRoute.minStartId.getX(), GetRoute.minStartId.getY(), GetRoute.minStartId.getZ());
        Coord endVirtualCoord = new Coord(GetRoute.minEndId.getX(), GetRoute.minEndId.getY(), GetRoute.minEndId.getZ());
        Double H = GetRoute.g_AStar.calcH1(startVirtualCoord, endVirtualCoord);
        GetRoute.start = new Node(GetRoute.minStartId.getId(), 0.0, startVirtualCoord, null, 0.0, H, GetRoute.minStartId.getIds());
        GetRoute.end = new Node(GetRoute.minEndId.getId(), 0.0, endVirtualCoord, null, 0.0, 0.0, GetRoute.minEndId.getIds());
        GetRoute.log.info("开始最近距离{}", startDistance[0]);
        GetRoute.log.info("结束最近距离{}", endDistance[0]);
        GetRoute.log.info("开始最近点{}", GetRoute.minStartId);
        GetRoute.log.info("结束最近点{}", GetRoute.minEndId);

        return collect;
    }


    private static JSONArray convertFromPath(List<Node> mappath, RouteDto dto) {
        double defaultdistance = 0.36;
        if (mappath == null || mappath.size() == 0) {
            GetRoute.log.info("convertFromPath，mappath.size ，{}", mappath.size());
            return null;
        }

        JSONArray route = new JSONArray();

        //起点不在path上，path的第一个点对应的是格子

        JSONObject start = new JSONObject();
        start.put("id", GetRoute.start.id);
        start.put("location", GetRoute.start.coord);
        route.add(start);

        Double[] virtualEndPosition = new Double[3];
        for (int i = mappath.size() - 1; i >= 0; i--) {
            Node node = mappath.get(i);
           if (node.id!=GetRoute.start.id&&node.id!=GetRoute.end.id) {
               JSONObject item = new JSONObject();
               item.put("location", node.coord);
               item.put("id", node.id);
               route.add(item);
           }
        }
        JSONObject end = new JSONObject();
        end.put("id", GetRoute.end.id);
        end.put("location", GetRoute.end.coord);
        route.add(end);
        return route;
    }



}