package com.ruoyi.device.hiksdk.service.impl;

import java.util.HashMap;
import java.util.Map;

/**
 * @ClassName: MapCoordinateToPTZ
 * @Description:
 * @Author: Administrator
 * @Date: 2023年02月08日 8:19
 * @Version: 1.0
 **/


public class MapCoordinateToPTZ {

    public static void main(String[] args) {
        double height = 90;
        double zoom=5;
        String cameraGps = "125.160097,46.566593";
        String targetGps = "125.156863,46.566648";
        double cameraLongitude = Double.parseDouble(cameraGps.split(",")[0]);
        double cameraLatitude = Double.parseDouble(cameraGps.split(",")[1]);
        double targetLongitude = Double.parseDouble(targetGps.split(",")[0]);
        double targetLatitude = Double.parseDouble(targetGps.split(",")[1]);
        Map<String, Object> map = GpsToPTZ(height,zoom, cameraLongitude, cameraLatitude, targetLongitude, targetLatitude);
        System.out.println(map);
        double angley = Math.atan2((targetLongitude-cameraLongitude),(targetLatitude-cameraLatitude)); //与Y轴的夹角
        System.out.println(angley*(180/Math.PI));
    }

    /**
     * @描述 gps坐标映射ptz
     * @参数 [height球机高度, anglePInit球机初始水平角度, x1球机经度, y1球机纬度, x2目标经度, y2目标纬度]
     * @返回值 java.util.Map<java.lang.String, java.lang.Object>
     * @创建人 刘苏义
     * @创建时间 2023/2/8 15:47
     * @修改人和其它信息
     */
    public static Map<String, Object> GpsToPTZ(double height,double Zoom, double x1, double y1, double x2, double y2) {
        //定义球机云台转动角度变量
        double angle = 0;//定义正右
        // 计算球机云台转动的角度
        // 通过正切公式计算出来的角度分正负，正数为逆时针旋转角度，负数为顺时针宣传角度
        angle = Math.atan2(x2 - x1,y2 - y1) * 180 / Math.PI;
        // 输出球机云台需要旋转的夹角
        System.out.println("球机需要水平旋转的夹角为：" + (angle) + "°");
        if (angle < 0) {
            angle = (360 + angle);
        }
        int pAngle = new Double(Math.round(angle)).intValue();
        System.out.println("当前P值为：" + pAngle + "°");
        // 计算球机云台俯仰的角度
        double distance = distance(x1, y1, x2, y2);
        System.out.println("两点距离：" + distance + "m");
        angle = (Math.atan(distance / height) * 180) / Math.PI;
        System.out.println("球机需要垂直旋转的夹角为：" + (angle) + "°");
        //云台水平时T值为0，向上为正值，向下为负值，且有上下限正负45度。
        //T值正常计算公式：向上为角度正值，向下为360+（角度负值）。
        //按实际使用情况，基本上排除向上正值的情况（可视范围内的坐标不会与相机水平）
        //通过正切公式计算出来角度，在相机里这个角度实际为负值，因此直接360-夹角就得到相机的T值
        //由于相机的T值向下的下限最大360-45=315度，也就是说计算出来的夹角必须大于315，小于415的也就必须时315
        double tValue = 360 - angle;
        if (tValue <= 315) {
            tValue = 315;
        }
        int tAngle = new Double(Math.round(tValue)).intValue();
        System.out.println("当前T值为：" + tAngle + "°");
        Map<String, Object> ptzMap = new HashMap<>();
        ptzMap.put("p", pAngle);
        ptzMap.put("t", tAngle);
        ptzMap.put("z", Zoom);
        return ptzMap;
    }

    /**
     * @描述 通过2个经纬度计算距离
     * @参数 [lat1, lng1, lat2, lng2]
     * @返回值 double
     * @创建人 刘苏义
     * @创建时间 2023/2/8 15:40
     * @修改人和其它信息
     */
    private static final double EARTH_RADIUS_WGS84 = 6378137.0;
    private static double distance(double lat1, double lng1, double lat2, double lng2) {
        double radLat1 = Math.toRadians(lat1);
        double radLat2 = Math.toRadians(lat2);
        double a = radLat1 - radLat2;
        double b = Math.toRadians(lng1) - Math.toRadians(lng2);
        double s = 2 * Math.asin(Math.sqrt(Math.pow(Math.sin(a / 2), 2) + Math.cos(radLat1) * Math.cos(radLat2) * Math.pow(Math.sin(b / 2), 2)));
        return Math.round(s * EARTH_RADIUS_WGS84);
    }

}