package org.sxkj.common.geo.utils;

import lombok.Data;

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

/**
 * 云台角度计算
 */
public class GimbalAngles {

	/**
	 * 根据云台角度计算当前 地图上的角度 0-360
	 *
	 * @param options
	 * @return
	 */
	public static double yawAngle(GimbalOptions options) {
		double gimbalYaw = options != null && options.getGimbalYaw() != null ? options.getGimbalYaw() : 0;
		double attitudeHead = options != null && options.getAttitudeHead() != null ? options.getAttitudeHead() : 0;

		double yaw;
		if (gimbalYaw > 180) {
			yaw = gimbalYaw;
		} else {
			yaw = gimbalYaw - attitudeHead;
		}

		double result = 0;
		if (yaw < 0) {
			result = yaw + 360;
		} else if (yaw > 0) {
			result = yaw;
		}

		if ((yaw > -2 && yaw < 2) || (int) attitudeHead == (int) gimbalYaw) {
			result = attitudeHead < 0 ? 180 + (180 + attitudeHead) : attitudeHead;
		}

		return result ;
	}




	// Pitch angle calculation
	private static double pitchAngle(GimbalOptions options) {
		double gimbalPitch = options != null && options.getGimbalPitch() != null ? options.getGimbalPitch() : 0;
		return gimbalPitch;
	}

	// Example options class
	@Data
	public static class GimbalOptions {
		private Double gimbalYaw;
		private Double attitudeHead;
		private Double gimbalPitch;

	}

	/**
	 * 无人机转向 需要加多少度减多少度
	 */
	public static final Map<String, Integer> DRONE_DIRECTION_ANGLES = new HashMap<>();

	static {
		DRONE_DIRECTION_ANGLES.put("左", -90);
		DRONE_DIRECTION_ANGLES.put("右", 90);
		DRONE_DIRECTION_ANGLES.put("前", 0);
		DRONE_DIRECTION_ANGLES.put("后", 180);

	}

}