package com.ard.tianyu.controller.wayline.terrain;

import com.ard.tianyu.domain.dto.wayline.ArdActionDTO;
import com.ard.tianyu.domain.dto.wayline.ArdPositionDTO;
import com.ard.tianyu.util.GeoTools;
import com.ard.tianyu.util.Vertex;
import com.ard.tianyu.domain.dto.wayline.ArdUavPositionDTO;
import com.ard.tianyu.domain.dto.wayline.ArdWaylineDTO;
import com.fasterxml.jackson.databind.ObjectMapper;
import org.springframework.stereotype.Component;
import javax.annotation.Resource;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;

/**
 * 仿地飞行规划器
 *
 * @author lsy
 * @date 2025/6/18
 */
@Component
public class TerrainFollowingPlanner {
    @Resource
    private GeoUtils geoUtils;

    private final double intervalMeter = 100.0; // 插值距离
    private final double smoothThreshold = 3.0; // 平滑阈值
    private static final ObjectMapper mapper = new ObjectMapper();

    // 算法主函数
    public List<ArdUavPositionDTO> interpolateWithTerrain(ArdWaylineDTO ardWaylineDTO) throws Exception {
        // 0. 验证源航线数据
        validateWaylineData(ardWaylineDTO);

        // 1. 先进行纯几何插值（不考虑高度）
        List<ArdUavPositionDTO> interpolatedPoints = pureInterpolation(ardWaylineDTO.getUavPositions());

        // 2. 然后统一处理地形高度
        return adjustAltitudeForTerrain(interpolatedPoints, ardWaylineDTO.getGlobalFlyHeight());
    }

    // 第一阶段：纯几何插值
    private List<ArdUavPositionDTO> pureInterpolation(List<ArdUavPositionDTO> originalPoints) {
        List<ArdUavPositionDTO> result = new ArrayList<>();

        for (int i = 0; i < originalPoints.size() - 1; i++) {
            ArdUavPositionDTO start = originalPoints.get(i);
            ArdUavPositionDTO end = originalPoints.get(i + 1);

            // 添加原始起点
            result.add(deepCopy(start, ArdUavPositionDTO.class));

            // 插值中间点
            result.addAll(interpolateBetween(start, end));
        }

        // 添加最后一个点
        result.add(deepCopy(originalPoints.get(originalPoints.size() - 1), ArdUavPositionDTO.class));

        return result;
    }

    // 第二阶段：地形高度调整
    private List<ArdUavPositionDTO> adjustAltitudeForTerrain(List<ArdUavPositionDTO> points, double flyHeight) throws IOException {

        double homeTerrainHeight = 0; // 记录起飞点地形高度作为基准海拔
        double lastFlightAltitude = flyHeight; // 记录上一个飞行高度

        for (int i = 0; i < points.size(); i++) {
            ArdUavPositionDTO point = points.get(i);
            double currentTerrainHeight = geoUtils.getHeight(
                    point.getUavPosition().getLongitude(),
                    point.getUavPosition().getLatitude()
            );
            if (i == 0) {
                homeTerrainHeight = currentTerrainHeight;
            }
            // 计算相对于起飞点的地形高度差
            double terrainHeightDiff = currentTerrainHeight - homeTerrainHeight;

            // 理论目标高度 = 设定高度 + 地形高度变化量
            double targetAltitude = flyHeight + terrainHeightDiff;

            // 平滑处理：如果高度变化不大，沿用上一个飞行高度
            if (i > 0 && Math.abs(targetAltitude - lastFlightAltitude) < smoothThreshold) {
                targetAltitude = lastFlightAltitude;
            }

            // 设置高度
            point.getUavPosition().setAltitude(targetAltitude);
            updateActionAltitude(point, currentTerrainHeight);

            lastFlightAltitude = targetAltitude; // 更新最后飞行高度
        }

        renumberWaypoints(points);
        return points;
    }

    // 辅助方法：两点间插值
    private List<ArdUavPositionDTO> interpolateBetween(ArdUavPositionDTO start, ArdUavPositionDTO end) {
        List<ArdUavPositionDTO> interpolated = new ArrayList<>();
        // 计算两点间的距离
        Vertex startV=new Vertex( start.getUavPosition().getLatitude(), start.getUavPosition().getLongitude());
        Vertex endV=new Vertex( end.getUavPosition().getLatitude(), end.getUavPosition().getLongitude());
        double distance = GeoTools.DistanceSphere(startV,endV);
        // 如果距离小于插值间隔，则直接返回
        if (distance <= intervalMeter) return interpolated;
        // 计算插值步数
        int steps = (int) (distance / intervalMeter);
        for (int i = 1; i < steps; i++) {
            double ratio = i * intervalMeter / distance;
            double lat = interpolate(
                    start.getUavPosition().getLatitude(),
                    end.getUavPosition().getLatitude(),
                    ratio
            );
            double lon = interpolate(
                    start.getUavPosition().getLongitude(),
                    end.getUavPosition().getLongitude(),
                    ratio
            );

            ArdUavPositionDTO newPoint = deepCopy(start, ArdUavPositionDTO.class);
            newPoint.setType(1);
            newPoint.getUavPosition().setLatitude(lat);
            newPoint.getUavPosition().setLongitude(lon);
            interpolated.add(newPoint);
        }

        return interpolated;
    }

    /**
     * 更新航点动作中的地面高度
     *
     * @param point         航点对象
     * @param terrainHeight 当前地形高度
     */
    private void updateActionAltitude(ArdUavPositionDTO point, double terrainHeight) {
        if (point.getActionGroup() == null || point.getActionGroup().isEmpty()) {
            return;
        }

        for (ArdActionDTO action : point.getActionGroup()) {
            // 如果已有地面位置，则更新高度
            if (action.getGroundPosition() != null) {
                action.getGroundPosition().setLongitude(point.getUavPosition().getLongitude());
                action.getGroundPosition().setLatitude(point.getUavPosition().getLatitude());
                action.getGroundPosition().setAltitude(terrainHeight);
            }
            // 如果没有地面位置，则创建新的地面位置
            else {
                action.setGroundPosition(ArdPositionDTO.builder()
                        .longitude(point.getUavPosition().getLongitude())
                        .latitude(point.getUavPosition().getLatitude())
                        .altitude(terrainHeight)
                        .build());
            }
        }
    }

    /**
     * 重新编号航点和动作
     *
     * @param points 航点列表
     */
    private void renumberWaypoints(List<ArdUavPositionDTO> points) {
        // 航点重新编号
        for (int i = 0; i < points.size(); i++) {
            ArdUavPositionDTO point = points.get(i);
            point.setIndex(i + 1); // 通常从1开始编号

            // 动作重新编号
            if (point.getActionGroup() != null && !point.getActionGroup().isEmpty()) {
                int actionIndex = 1;
                for (ArdActionDTO action : point.getActionGroup()) {
                    action.setActionIndex(actionIndex++);
                }
            }
        }
    }


    /**
     * 校验航线数据有效性
     *
     * @param ardWaylineDTO 航线数据
     * @throws IllegalArgumentException 如果数据无效
     */
    private void validateWaylineData(ArdWaylineDTO ardWaylineDTO) {
        if (ardWaylineDTO == null) {
            throw new IllegalArgumentException("航线数据不能为null");
        }
        if (ardWaylineDTO.getUavPositions() == null || ardWaylineDTO.getUavPositions().size() < 2) {
            throw new IllegalArgumentException("航点数量必须大于等于2");
        }
        if (ardWaylineDTO.getGlobalFlyHeight() <= 0) {
            throw new IllegalArgumentException("全局飞行高度必须大于0");
        }
    }

    // 通用深拷贝
    public static <T> T deepCopy(T original, Class<T> clazz) {
        if (original == null) {
            return null;
        }
        try {
            byte[] bytes = mapper.writeValueAsBytes(original);
            return mapper.readValue(bytes, clazz);
        } catch (Exception e) {
            e.printStackTrace();
            return null;
        }
    }

    // 线性插值
    public double interpolate(double start, double end, double ratio) {
        return start + (end - start) * ratio;
    }
}