import math


# 定义点
class MyPoint:
    def __init__(self, x, y):
        self.X = x
        self.Y = y

    @classmethod
    def makeOne(cls, pt):
        return MyPoint(pt.lng, pt.lat)

    def get_Manhattan_dist(self, other):
        return math.fabs(self.X - other.X) + math.fabs(self.Y - other.Y)

    def get_dist(self, other):
        return math.sqrt((self.X - other.X)*(self.X - other.X) + (self.Y - other.Y)*(self.Y - other.Y))


    def calc_left_pt(self, target_pt: 'MyPoint', left_dist: float):
        """计算中间插值点"""
        dist = self.get_dist(target_pt)
        mid_pt = MyPoint(0, 0)
        mid_pt.X = self.X + left_dist/dist*(target_pt.X - self.X)
        mid_pt.Y = self.Y + left_dist / dist * (target_pt.Y - self.Y)
        return mid_pt


# 定义一系列全局变量
PI = 3.1415926535897932384626433832795
PI_120D = 2.0943951023931954923084289221863
PI_90D = 1.5707963267948966192313216916398
PI_80D = 1.3962634015954636615389526147909
PI_30D = 0.52333333333


g_srcDir_to_angle_map = {'N': 180, 'NE': 225, 'E': 270, 'SE': 315, 'S': 0, 'SW': 45, 'W': 90, 'NW':135}

class GeoFunc:

    @classmethod
    def calc_vec_radian(cls, pA: MyPoint, pB: MyPoint):
        dx = pB.X - pA.X
        dy = pB.Y - pA.Y
        if dx == 0:
            if dy > 0:
                return PI_90D
            elif dy < 0:
                return -1 * PI_90D
            else:
                # 两点重合，暂未处理这种异常情况
                return 0

        if dy == 0:
            if dx > 0:
                return 0

            elif dx < 0:
                return PI

            else:
                # 两点重合，暂未处理这种异常情况
                return 0

        dK = dy / dx
        dA = math.atan(dK)
        # 如果在第二项限
        if (pB.Y > pA.Y and dA < 0):
            dA += PI

        # 如果在第三项限
        if (pB.Y < pA.Y and dA > 0):
            dA -= PI

        return dA

    @classmethod
    def calc_turn_radian(cls, pA: MyPoint, pB: MyPoint, pC: MyPoint, pD: MyPoint):
        """
        计算AB->CD转弯的弧度。右转为正值，左转为负值
        :param pA:
        :param pB:
        :param pC:
        :param pD:
        :return: [-PI, PI]
        """
        fa = GeoFunc.calc_vec_radian(pA, pB)
        fb = GeoFunc.calc_vec_radian(pC, pD)
        fSign = fa * fb
        if fSign >= 0:
            return fa - fb

        else:
            ft = math.fabs(fa - fb)
            if ft >= PI:
                r = 2 * PI - ft
                if fa - fb < 0:
                    return r
                else:
                    return -1 * r
            else:
                return fa - fb

    @classmethod
    def radian2angle(cls, radian):
        """将弧度转换为角度"""
        return radian * 180 / PI

    @classmethod
    def judge_turn_type(cls, turn_angle):
        turn_type = -1
        if math.fabs(turn_angle) <= 30:
            turn_type = 0  # straight
        elif -150 < turn_angle < -30:
            turn_type = 1  # turn left
        elif 30 < turn_angle < 150:
            turn_type = 2  # turn left
        else:  # means: abs(turn_angle) >= 150
            turn_type = 3  # turn back

        return turn_type

    @classmethod
    def calc_turn_type(cls, pA, pB, pC, pD):
        turn_radian = GeoFunc.calc_turn_radian(pA, pB, pC, pD)
        turn_angle = GeoFunc.radian2angle(turn_radian)
        return GeoFunc.judge_turn_type(turn_angle)

    @classmethod
    def calc_turn_type_ex(cls, pA, pB, pC, pD):
        """
        计算AB->CD的转向类型，附加偏差角度
        @param pA:
        @param pB:
        @param pC:
        @param pD:
        @return: turn_type, diff
        """
        turn_radian = GeoFunc.calc_turn_radian(pA, pB, pC, pD)
        turn_angle = GeoFunc.radian2angle(turn_radian)
        turn_type = GeoFunc.judge_turn_type(turn_angle)
        diff = 0
        if turn_type == 0:
            diff = abs(abs(turn_angle))
        elif turn_type == 3:
            diff = abs(180 - abs(turn_angle))
        else:
            diff = abs(90 - abs(turn_angle))
        return turn_type, diff


    # 根据向量角度，判定来源方向
    @classmethod
    def vecAngle_to_srcDir(cls, vec_angle):
        srcDir = ''
        if math.fabs(vec_angle) <= 22.5:
            srcDir = 'W'
        elif 22.5 < vec_angle <= 67.5:
            srcDir = 'SW'
        elif 67.5 < vec_angle <= 112.5:
            srcDir = 'S'
        elif 112.5 < vec_angle <= 157.5:
            srcDir = 'SE'
        elif math.fabs(vec_angle) > 157.5:
            srcDir = 'E'
        elif -157.5 <= vec_angle <= -112.5:
            srcDir = 'NE'
        elif -112.5 < vec_angle <= -67.5:
            srcDir = 'N'
        elif -67.5 < vec_angle <= -22.5:
            srcDir = 'NW'

        return srcDir

    @classmethod
    def calc_srcDir(cls, pA: MyPoint, pB: MyPoint):
        vec_radian = GeoFunc.calc_vec_radian(pA, pB)
        vec_angle = GeoFunc.radian2angle(vec_radian)
        return GeoFunc.vecAngle_to_srcDir(vec_angle)

    @classmethod
    def calc_pole_angle(cls, pA: MyPoint, pB: MyPoint):
        """
        计算gps角度：以正北为0度，顺时针方向角度增大
        :param pA:
        :param pB:
        :return: [0, 360)
        """
        vec_radian = GeoFunc.calc_vec_radian(pA, pB)
        vec_angle = GeoFunc.radian2angle(vec_radian)
        pole_angle = (90 - vec_angle + 360) % 360
        return pole_angle

    @classmethod
    def diff_pole_angle(cls, a, b):
        """
        计算两个gps方位角的差值
        :param a:
        :param b:
        :return:
        """
        diff = abs(a -b)
        if diff > 180:
            diff = 360 - diff
        return diff

    @classmethod
    def calc_srcDir_tail(cls, pt_list):
        """
        计算点串末段的车流来向
        :param pt_list:
        :return:
        """
        num = len(pt_list)
        if num < 2:
            return 'unknown'
        pB = MyPoint.makeOne(pt_list[num - 1])
        cur_pA_index = num - 2
        while True:
            pA = MyPoint.makeOne(pt_list[cur_pA_index])
            if cur_pA_index <= 0 or pA.get_Manhattan_dist(pB) >= 0.0005:
                return GeoFunc.calc_srcDir(pA, pB)
            cur_pA_index -= 1
            # 保底措施
            if cur_pA_index < 0:
                break
        # 不应该走到的分支
        return 'unknown'

    @classmethod
    def calc_srcDir_head(cls, pt_list):
        """
        计算点串起始段的车流来向
        :param pt_list:
        :return:
        """
        num = len(pt_list)
        if num < 2:
            return 'unknown'
        pA = MyPoint.makeOne(pt_list[0])
        cur_pB_index = 1
        while True:
            pB = MyPoint.makeOne(pt_list[cur_pB_index])
            if cur_pB_index >= num-1 or pA.get_Manhattan_dist(pB) >= 0.0005:
                return GeoFunc.calc_srcDir(pA, pB)
            cur_pB_index += 1
        # 不应该走到的分支
        return 'unknown'


    @classmethod
    def calc_angle_head(cls, pt_list):
        """
        计算点串起始段的车流来向
        :param pt_list:
        :return:
        """
        num = len(pt_list)
        if num < 2:
            return 'unknown'
        pA = MyPoint.makeOne(pt_list[0])
        cur_pB_index = 1
        while True:
            pB = MyPoint.makeOne(pt_list[cur_pB_index])
            if cur_pB_index >= num-1 or pA.get_Manhattan_dist(pB) >= 0.0005:
                return GeoFunc.calc_pole_angle(pA, pB)
            cur_pB_index += 1
        # 不应该走到的分支
        return 'unknown'

    @classmethod
    def flip_gps_angle(self, original_angle):
        flipped_angle = (original_angle + 180) % 360
        return flipped_angle

    @classmethod
    def calc_angle_tail(cls, pt_list):
        """
        计算点串末段的车流来向
        :param pt_list:
        :return:
        """
        num = len(pt_list)
        if num < 2:
            return 'unknown'
        pB = MyPoint.makeOne(pt_list[num - 1])
        cur_pA_index = num - 2
        while True:
            pA = MyPoint.makeOne(pt_list[cur_pA_index])
            if cur_pA_index <= 0 or pA.get_Manhattan_dist(pB) >= 0.0005:
                return GeoFunc.calc_pole_angle(pA, pB)
            cur_pA_index -= 1
            # 保底措施
            if cur_pA_index < 0:
                break
        # 不应该走到的分支
        return 'unknown'


    @classmethod
    def diff_srcDir(cls, a: str, b: str):
        """
        计算两个车流来向的角度差
        :param a:
        :param b:
        :return: 0/45/90/135/180
        """
        if a == b:
            return 0

        angle_a = g_srcDir_to_angle_map[a]
        angle_b = g_srcDir_to_angle_map[b]
        diff_angle = abs(angle_a -  angle_b)
        if diff_angle > 180:
            diff_angle = 360 - diff_angle
        return diff_angle

    @classmethod
    def pick_path_head(cls, pt_list) -> (MyPoint, MyPoint):
        """
        从点序列中，提取满足50米长度的开头的一段
        :param pt_list: Pt的数组
        :return: pA, pB
        """
        num = len(pt_list)
        if num < 2:
            return None, None
        if num == 2:
            return MyPoint.makeOne(pt_list[0]), MyPoint.makeOne(pt_list[1])
        pA = MyPoint.makeOne(pt_list[0])
        cur_pB_index = 1
        while True:
            pB = MyPoint.makeOne(pt_list[cur_pB_index])
            if cur_pB_index >= num-1 or pA.get_dist(pB) >= 0.0005:
                return pA, pB
            cur_pB_index += 1
            # 保底措施
            if cur_pB_index > num-1:
                break
        # 不应该走到的分支
        return None, None

    @classmethod
    def pick_path_tail(cls, pt_list) -> (MyPoint, MyPoint):
        """
        从点序列中，提取满足50米长度的末尾的一段
        :param pt_list: Pt的数组
        :return: pA, pB
        """
        num = len(pt_list)
        if num < 2:
            return None, None
        if num == 2:
            return MyPoint.makeOne(pt_list[0]), MyPoint.makeOne(pt_list[1])
        pB = MyPoint.makeOne(pt_list[num - 1])
        cur_pA_index = num - 2
        while True:
            pA = MyPoint.makeOne(pt_list[cur_pA_index])
            if cur_pA_index <= 0 or pA.get_dist(pB) >= 0.0005:
                return pA, pB
            cur_pA_index -= 1
            # 保底措施
            if cur_pA_index < 0:
                break
        # 不应该走到的分支
        return None, None


    @classmethod
    def calc_earth_dist(cls, x1, y1, x2, y2):
        """
        计算两个经纬度坐标之间的球面距离
        """
        _PI_  = 3.14159265358979323846264
        EARTHS_RADIUS = 6371008.8
        RAD_TO_DEG = 180.0 / _PI_
        DEG_TO_RAD = _PI_ / 180.0
        radLat1 = y1 * DEG_TO_RAD
        radLat2 = y2 * DEG_TO_RAD
        a = radLat1 - radLat2
        b = (x1 - x2) * DEG_TO_RAD
        s = 2 * math.asin(math.sqrt(pow(math.sin(a / 2), 2) + math.cos(radLat1) * math.cos(radLat2) * pow(math.sin(b / 2), 2)))
        return s * EARTHS_RADIUS


if __name__ == '__main__':
    # for a in g_srcDir_to_angle_map.keys():
    #     for b in g_srcDir_to_angle_map.keys():
    #         diff_angle = GeoFunc.diff_srcDir(a, b)
    #         print('%s %s => %d' % (a, b, diff_angle))
    geo = '98.327637,27.994401,118.366699,41.508577'
    ss = geo.split(',')
    x1 = float(ss[0])
    y1 = float(ss[1])
    x2 = float(ss[2])
    y2 = float(ss[3])
    dist = GeoFunc.calc_earth_dist(x1, y1, x2, y2)
    print(dist)