from typing import Tuple

import numpy as np

sin, cos, arcsin, arccos, atan2, sqrt = np.sin, np.cos, np.arcsin, np.arccos, np.arctan2, np.sqrt


def angle_to_vec(angle_in_deg):
    angle = np.deg2rad(angle_in_deg)
    return np.array([np.cos(angle), np.sin(angle)])


class PosLLA:
    EARTH_RADIUS = 6.317e6  # m

    def __init__(self, *args):
        """ 纬、经、高 """
        if len(args) == 2:
            self.lat = args[0]
            self.lon = args[1]
            self.height = 0
        else:
            self.lat, self.lon, self.height = np.array(args).flatten()

    @property
    def phi_(self):
        return np.deg2rad(self.lat)

    @property
    def lambda_(self):
        return np.deg2rad(self.lon)

    @property
    def ecef(self):
        return (self.EARTH_RADIUS + self.height) * self.vector

    @property
    def vector(self):
        return np.array([np.cos(self.phi_) * np.cos(self.lambda_),
                         np.cos(self.phi_) * np.sin(self.lambda_),
                         np.sin(self.phi_)])

    def tolist(self):
        """ 纬、经、高        """
        return [self.lat, self.lon, self.height]

    def distance_to(self, other):
        # todo: 调用多，导致太慢
        # return np.linalg.norm(self.ecef - other.ecef)
        return sphere_distance_haversine(self.phi_, self.lambda_, other.phi_, other.lambda_, self.EARTH_RADIUS)

    def distance_to_arc(self, arc: Tuple):
        lla_1, lla_2 = arc
        pole_vec = np.cross(lla_1.vector, lla_2.vector)
        sin_alpha = np.inner(self.vector, pole_vec / np.linalg.norm(pole_vec))
        dist = np.abs(np.arcsin(sin_alpha)) * self.EARTH_RADIUS
        return dist

    def distance_to_traj(self, pos, heading):
        ref_pos = self.away_from(pos, heading, 1e5)
        return self.distance_to_arc((pos, ref_pos))

    def azimuth_to(self, other):
        d_lambda = other.lambda_ - self.lambda_
        numerator = sin(d_lambda) * cos(other.phi_)
        denominator = cos(self.phi_) * sin(other.phi_) - sin(self.phi_) * cos(other.phi_) * cos(d_lambda)
        angle = atan2(numerator, denominator, )  # 直角系
        azimuth = angle
        return np.rad2deg(azimuth) % 360

    def __str__(self):
        return '(' + ', '.join(map(str, self.tolist())) + ')'

    def __repr__(self):
        return self.__str__()

    @classmethod
    def away_from(cls, other, azimuth, distance):
        azimuth = np.deg2rad(azimuth)
        delta = distance / cls.EARTH_RADIUS
        sin_phi = sin(other.phi_) * cos(delta) + cos(other.phi_) * sin(delta) * cos(azimuth)
        new_phi = arcsin(sin_phi)
        numerator = sin(azimuth) * sin(delta) * cos(other.phi_)
        denominator = cos(delta) - sin(other.phi_) * sin_phi
        new_lambda = other.lambda_ + atan2(numerator, denominator)
        radius = cls.EARTH_RADIUS + other.height
        return cls.from_polar(new_phi, new_lambda, radius)

    @classmethod
    def away_from_displacement(cls, other, heading, h_dist, v_dist):
        direction = np.rad2deg(atan2(v_dist, h_dist)) % 360
        distance = np.sqrt(h_dist ** 2 + v_dist ** 2)
        azimuth = 90 + heading - direction  # 沿船横算，要比船头多90度射角
        return cls.away_from(other, azimuth, distance)

    @classmethod
    def from_polar(cls, phi_, lambda_, radius):
        lon = np.rad2deg(lambda_)
        lat = np.rad2deg(phi_)
        h = max(radius - cls.EARTH_RADIUS, 0)
        return cls(lat, lon, h)

    @classmethod
    def from_ecef(cls, *args):
        ecef = np.array(args).flatten()
        radius = np.linalg.norm(ecef)
        phi_ = arcsin(ecef[2] / radius)
        lambda_ = atan2(ecef[1], ecef[0])
        return cls.from_polar(phi_, lambda_, radius)

    def to_dict(self):
        return {
            'Lat': self.lat, 'Lon': self.lon, 'Height': self.height,
        }


def sphere_distance_haversine(phi_1, lambda_1, phi_2, lambda_2, radius):
    """
    计算球面距离 Haversine公式
    https://dothinking.github.io/2017-03-09-%E7%90%83%E9%9D%A2%E8%B7%9D%E7%A6%BB%E4%B8%8E%E6%96%B9%E4%BD%8D%E8%A7%92%E5%85%AC%E5%BC%8F%E7%9A%84%E6%8E%A8%E5%AF%BC%EF%BC%9A%E5%90%91%E9%87%8F%E4%BB%A3%E6%95%B0%E6%B3%95/
    """
    d_phi, d_lambda = phi_2 - phi_1, lambda_2 - lambda_1
    a = sin(d_phi / 2) ** 2 + cos(phi_1) * cos(phi_2) * sin(d_lambda / 2) ** 2
    delta = 2 * atan2(sqrt(a), sqrt(1 - a), )
    return delta * radius


def calc_meet_heading(pos_a: PosLLA, heading_a, speed_a, pos_b: PosLLA, speed_b):
    azimuth_a2b = pos_a.azimuth_to(pos_b)
    theta_a = azimuth_a2b - heading_a
    theta_b = np.arcsin(speed_a * sin(theta_a) / speed_b)
    azimuth_b2a = pos_b.azimuth_to(pos_a)
    heading_b = azimuth_b2a - theta_b
    return heading_b


def get_pos_inbound(pos_a: PosLLA, pos_b: PosLLA, radius_a, heading_b):
    distance = pos_a.distance_to(pos_b)
    theta_b = pos_b.azimuth_to(pos_a) - heading_b
    pedal_line = np.abs(np.sin(np.deg2rad(theta_b))) * distance
    if pedal_line > radius_a:
        return None
    else:
        travel_distance = np.cos(np.deg2rad(theta_b)) * distance - np.sqrt(radius_a ** 2 - pedal_line) ** 2
        inbound_pos = PosLLA.away_from(pos_b, heading_b, travel_distance)
        return inbound_pos


def get_pos_meet(pos_a: PosLLA, pos_b: PosLLA, speed_a, speed_b, heading_b):
    distance = pos_a.distance_to(pos_b)
    azimuth2 = pos2.azimuth_to(pos1)
    theta2_rad = np.deg2rad(azimuth2 - heading_b)
    sin_theta1 = speed_a * sin(theta2_rad) / speed_b
    if np.abs(sin_theta1) > 1:  # 不可能相遇
        return None
    else:
        theta1_rad = np.arcsin(sin_theta1)
        t = distance / (speed_a * cos(theta1_rad) + speed_b * cos(theta2_rad))
        return PosLLA.away_from(pos_b, heading_b, speed_b * t)


def get_meet_time_inbound(pos1: PosLLA, pos2: PosLLA, speed1, speed2, heading2):
    distance = pos1.distance_to(pos2)
    azimuth2 = pos2.azimuth_to(pos1)
    theta2_rad = np.deg2rad(azimuth2 - heading2)
    sin_theta1 = speed2 * sin(theta2_rad) / speed1
    if np.abs(sin_theta1) > 1:  # 不可能相遇
        return None
    else:
        theta1_rad = np.arcsin(sin_theta1)
        t = distance / (speed1 * cos(theta1_rad) + speed2 * cos(theta2_rad))
        return t


def get_meet_time_outbound(pos1: PosLLA, pos2: PosLLA, speed1, speed2, heading2, radius1):
    distance = pos1.distance_to(pos2)
    azimuth2 = pos2.azimuth_to(pos1)
    theta2_rad = np.deg2rad(azimuth2 - heading2)
    goujing = np.abs(sin(theta2_rad) * distance)

    travel_distance = distance * cos(theta2_rad) - np.sqrt(radius1 ** 2 - goujing ** 2)
    travel_time = travel_distance / speed2
    inbound_pos = PosLLA.away_from(pos2, heading2, travel_distance)

    inbound_time = get_meet_time_inbound(pos1, inbound_pos, speed1, speed2, heading2)
    return travel_time + inbound_time


if __name__ == '__main__':
    pos1 = PosLLA(0, 30, 0)
    pos2 = PosLLA(0, 60, 0)
    print(pos1.distance_to(pos2), '=', np.deg2rad(30) * PosLLA.EARTH_RADIUS)

    pos1 = PosLLA(30, 30, 0)
    pos2 = PosLLA(30, 60, 0)
    print(pos1.distance_to(pos2), '<', np.deg2rad(30) * PosLLA.EARTH_RADIUS)

    print(pos1)

    heading = 0
    h_dist = 100e3
    v_dist = 0

    pos1 = PosLLA(0, 30, 0)
    pos2 = PosLLA(0, 60, 0)
    pos3 = PosLLA.away_from(pos1, 90, 3307573)
    print(pos3, '=', pos2)

    pos2 = PosLLA.away_from_displacement(pos1, heading, h_dist, v_dist)
    print(pos2)
    print(pos2.distance_to(pos1), pos1.distance_to(pos2))

    print(pos2.azimuth_to(pos1), pos1.azimuth_to(pos2))

    for distance in [100e3, -100e3, ]:
        pos3 = PosLLA.away_from(pos2, 90, distance)
        azimuth = pos2.azimuth_to(pos1)
        print(pos3.distance_to_traj(pos2, azimuth), pos3.distance_to_arc((pos2, pos1)))

    vector = pos3.vector
    pos4 = PosLLA.from_ecef(vector)
    print(pos4)
    print(pos3)