simulation-backend/src/main/java/com/hivekion/room/bean/MoveRootTask.java

package com.hivekion.room.bean;

import cn.hutool.extra.spring.SpringUtil;
import com.alibaba.fastjson2.JSON;
import com.alibaba.fastjson2.JSONArray;
import com.alibaba.fastjson2.JSONObject;
import com.hivekion.Global;
import com.hivekion.common.MultiPointGeoPosition;
import com.hivekion.common.entity.ResponseCmdInfo;
import com.hivekion.enums.WsCmdTypeEnum;
import com.hivekion.room.func.TaskAction;
import com.hivekion.scenario.entity.ScenarioTask;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Map.Entry;
import java.util.NavigableMap;
import java.util.TreeMap;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.atomic.AtomicReference;
import lombok.Data;
import lombok.extern.slf4j.Slf4j;
import org.springframework.core.env.Environment;

/**
 * [类的简要说明]
 * <p>
 * [详细描述，可选]
 * <p>
 *
 * @author LiDongYU
 * @since 2025/7/22
 */
@Slf4j
public class MoveRootTask extends AbtParentTask implements TaskAction {

  /**
   * 速度 换算为100Km/小时
   */
  private final double SPEED = 27;
  /**
   * 距离和坐标的对应关系
   */
  private final TreeMap<Double, Coordinate> distanceInfoMap = new TreeMap<>();
  /**
   * 开始点坐标
   */
  private final AtomicReference<Double> startPoint = new AtomicReference<>();


  public MoveRootTask(ScenarioTask scenarioTask, String roomId) {
    super(scenarioTask, roomId);
  }


  @Override
  public void doSomeThing() {
    log.info("move task running");
    initPath(); //初始化路径
    updatePath(); //更新路径
  }

  /**
   * 初始化路径
   */
  private void initPath() {
    try {

      String url = SpringUtil.getBean(Environment.class).getProperty("path.planning.url");
      String params = url + "?"
          + "profile=car"
          + "&point=" + scenarioTask.getFromLat() + ","
          + scenarioTask.getFromLng()
          + "&point=" + scenarioTask.getToLat() + ","
          + scenarioTask.getToLng()
          + "&points_encoded=false"
          + "&algorithm=alternative_route&alternative_route.max_paths=3";
      //获取路线信息
      String result = webClient.get().uri(params)
          .retrieve()
          .bodyToMono(String.class)
          .block();

      JSONObject pointJson = JSON.parseObject(result);
      //获取路径点
      if (pointJson != null) {
        JSONObject pointsObj = pointJson.getJSONArray("paths").getJSONObject(0)
            .getJSONObject("points");
        JSONArray coordinates = pointsObj.getJSONArray("coordinates");
        //组装信息
        Map<String, Object> dataMap = new HashMap<>();
        dataMap.put("resourceId", scenarioTask.getResourceId());
        dataMap.put("points", coordinates);
        //推送路径任务
        Global.sendCmdInfoQueue.add(
            ResponseCmdInfo.create(WsCmdTypeEnum.PATH_INIT.getCode(), roomId,
                scenarioTask.getScenarioId(), dataMap));
        log.info("init::{}", JSON.toJSONString(coordinates));
        //计算各个点的累计距离和坐标的对应关系
        double beforeLng = Double.parseDouble(scenarioTask.getFromLng());
        double beforeLat = Double.parseDouble(scenarioTask.getFromLat());
        double total = 0;
        for (int i = 0; i < coordinates.size(); i++) {
          JSONArray coordinate = coordinates.getJSONArray(i);
          Double lng = coordinate.getDouble(0);

          Double lat = coordinate.getDouble(1);

          double distance = MultiPointGeoPosition.haversine(beforeLat, beforeLng, lat, lng);
          //当前总距离
          total = total + distance;
          //定义坐标对象
          Coordinate coordinateInfo = new Coordinate();
          coordinateInfo.setLat(lat);
          coordinateInfo.setLng(lng);

          //记录距离和数组列表直接的索引关系
          distanceInfoMap.put(total, coordinateInfo);

          beforeLng = lng;
          beforeLat = lat;

        }
        //设置第一个开始位置
        startPoint.set(distanceInfoMap.firstKey());
      }

    } catch (Exception e) {
      log.error("error::", e);
    }
  }

  private void updatePath() {

    ScheduledExecutorService schedule = Executors.newScheduledThreadPool(
        1);
    schedule.scheduleWithFixedDelay(() -> {

      try {
        if (this.getRoomStatus()) {

          long duringTime = getDuringTime();
          log.info("duringTime::{}", duringTime);
          //跑动距离
          double distance = duringTime * SPEED;
          //获取大与此距离的第一个路线点key
          Entry<Double, Coordinate> endPoint = distanceInfoMap.ceilingEntry(distance);
          //ws数据
          List<double[]> dataList = new ArrayList<>();
          HashMap<Object, Object> dataMap = new HashMap<>();
          dataMap.put("resourceId", scenarioTask.getResourceId());
          dataMap.put("points", dataList);

          if (Double.compare(distance, endPoint.getKey()) < 0) {
            //获取小于最大值的第一个key
            Double lowerKey = distanceInfoMap.lowerKey(endPoint.getKey());
          //  log.info("distance::{},lowerKey::{},endPoint{}",distance,lowerKey,endPoint.getKey());
            //获取从上一个开始节点到lowKey的数据
            NavigableMap<Double, Coordinate> subPathMap = distanceInfoMap.subMap(startPoint.get(),
                true, lowerKey, true);
            for (Double key : subPathMap.keySet()) {
              Coordinate coordinate = subPathMap.get(key);
              dataList.add(new double[]{coordinate.getLng(), coordinate.getLat()});
            }
            double diff =distance -  lowerKey  ;

            //插入值
            double[] insertPoints = MultiPointGeoPosition.pointAlong(
                distanceInfoMap.get(lowerKey).getLat(), distanceInfoMap.get(lowerKey).getLng(),
                endPoint.getValue().getLat(), endPoint.getValue().getLng(), diff);



            dataList.add(new double[]{insertPoints[1], insertPoints[0]});

            Coordinate coordinate = new  Coordinate();
            coordinate.setLat(insertPoints[0]);
            coordinate.setLng(insertPoints[1]);
            distanceInfoMap.put(distance, coordinate);
            startPoint.set(distance);


            Global.sendCmdInfoQueue.add(
                ResponseCmdInfo.create(WsCmdTypeEnum.PATH_UPDATE.getCode(), roomId,
                    scenarioTask.getScenarioId(), dataMap));

          } else if (Double.compare(distance, endPoint.getKey()) == 0) {
            NavigableMap<Double, Coordinate> subPathMap = distanceInfoMap.subMap(startPoint.get(),
                true, endPoint.getKey(), true);
            for (Double key : subPathMap.keySet()) {
              Coordinate coordinate = subPathMap.get(key);
              dataList.add(new double[]{coordinate.getLng(), coordinate.getLat()});
            }

            startPoint.set(endPoint.getKey());
            Global.sendCmdInfoQueue.add(
                ResponseCmdInfo.create(WsCmdTypeEnum.PATH_UPDATE.getCode(), roomId,
                    scenarioTask.getScenarioId(), dataMap));

          } else {
            //完成路径
            Global.sendCmdInfoQueue.add(
                ResponseCmdInfo.create(WsCmdTypeEnum.PATH_FINISHED.getCode(), roomId,
                    scenarioTask.getScenarioId(), dataMap));
          }



        }
      } catch (Exception e) {
        log.error("error::", e);
      }


    }, 0, 1, TimeUnit.SECONDS);

    //房间统一管理定时器；房间关闭后，定时器销毁
    addScheduledExecutorServiceRefenceToRoom(schedule);
  }


}

@Data
class Coordinate {

  double lng;
  double lat;

}