# -*- coding: utf-8 -*-
import xml.etree.ElementTree as ET
import math
import time
import pandas as pd
import numpy as np
from scipy import special


def evaluate_elevation(s, a, b, c, d):
    elevation = a + b * s + c * s ** 2 + d * s ** 3
    return elevation


# 获得高程参数信息
def get_elevation(road):
    count = 0
    elevation_dict = dict()
    while 1:
        try:
            elevation = [elevation for elevation in road.iter('elevation')][count]
        except IndexError:
            break
        s = float(elevation.get('s'))
        a = float(elevation.get('a'))
        b = float(elevation.get('b'))
        c = float(elevation.get('c'))
        d = float(elevation.get('d'))
        elevation_dict[count] = {'s': s, 'a': a, 'b': b, 'c': c, 'd': d}
        count += 1
    return elevation_dict


# 获得超高程参数信息     # 暂不启用
def get_superelevation(road):
    count = 0
    superelevation_dict = dict()
    while 1:
        try:
            superelevation = [superelevation for superelevation in road.iter('superelevation')][count]
        except IndexError:
            break
        s = float(superelevation.get('s'))
        a = float(superelevation.get('a'))
        b = float(superelevation.get('b'))
        c = float(superelevation.get('c'))
        d = float(superelevation.get('d'))
        superelevation_dict[count] = {'s': s, 'a': a, 'b': b, 'c': c, 'd': d}
        count += 1
    return superelevation_dict


# 获得车道宽度参数
def get_width(lane):
    count = 0
    width_dict = dict()
    while 1:
        try:
            width = [width for width in lane.iter('width')][count]
        except IndexError:  # 可能出现有车道但没有车道宽度，返回0
            break
        s = float(width.get('sOffset'))
        a = float(width.get('a'))
        b = float(width.get('b'))
        c = float(width.get('c'))
        d = float(width.get('d'))
        width_dict[count] = {'sOffset': s, 'a': a, 'b': b, 'c': c, 'd': d}
        count += 1
    if width_dict:
        return width_dict
    else:  # 如果为空，增加一条0
        width_dict[count] = {'sOffset': 0, 'a': 0, 'b': 0, 'c': 0, 'd': 0}
        return width_dict


# 获取道路参考线偏移参数信息
def get_laneOffset(road):
    count = 0
    laneOffset_dict = dict()
    while 1:
        try:
            laneOffset = [laneOffset for laneOffset in road.iter('laneOffset')][count]
        except IndexError:  # 可能没有道路偏移
            break
        s = float(laneOffset.get('s'))
        a = float(laneOffset.get('a'))
        b = float(laneOffset.get('b'))
        c = float(laneOffset.get('c'))
        d = float(laneOffset.get('d'))
        laneOffset_dict[count] = {'s': s, 'a': a, 'b': b, 'c': c, 'd': d}
        count += 1
    if laneOffset_dict:
        return laneOffset_dict
    else:  # 如果为空
        return False


# 求点到线段距离
def dis_point_to_seg_line(p: [float, float], a: [float, float], b: [float, float]):
    a, b, p = np.array(a), np.array(b), np.array(p)
    d = np.divide(b - a, np.linalg.norm(b - a))
    s = np.dot(a - p, d)
    t = np.dot(p - b, d)
    h = np.maximum.reduce([s, t, 0])
    c = np.cross(p - a, d)
    return np.hypot(h, np.linalg.norm(c))


def cal_ele_avg(s_start, ele_start, s_end, ele_end, s):
    A = ele_end - ele_start
    B = s_start - s_end
    C = s_end * ele_start - s_start * ele_end

    if A == 0:
        result_ele = ele_start
    elif B == 0:
        result_ele = ele_start
    else:
        k = -1 * A / B
        b = -1 * C / B
        result_ele = k * (s + s_start) + b

    return result_ele


# 知参考线起点终点s和curvature坐标可求需计算点s的曲率result_curv
def cal_curv_avg(s_start, curv_start, s_end, curv_end, s):
    A = curv_end - curv_start
    B = s_start - s_end
    C = s_end * curv_start - s_start * curv_end

    if A == 0:  # line or arc, or spiral
        result_curv = curv_start
    elif B == 0:
        result_curv = curv_start
    else:
        k = -1 * A / B
        b = -1 * C / B
        result_curv = k * (s + s_start) + b

    return result_curv


def line_cal_pos(x_start, y_start, s_pos, hdg_start):
    s = s_pos
    dx = s * math.cos(hdg_start)
    dy = s * math.sin(hdg_start)
    return x_start + dx, y_start + dy


def arc_cal_pos(x_start, y_start, s_pos, hdg_start, curvature):
    c = curvature
    hdg_start = hdg_start - math.pi / 2
    s = s_pos

    a = 2 / c * math.sin(s * c / 2)
    alpha = (math.pi - s * c) / 2 - hdg_start

    dx = -1 * a * math.cos(alpha)
    dy = a * math.sin(alpha)
    return x_start + dx, y_start + dy


class EulerSpiral:
    """ """

    def __init__(self, gamma):
        self._gamma = gamma

    @staticmethod
    def createFromLengthAndCurvature(length, curvStart, curvEnd):
        """Create an EulerSpiral from a given length with curveStart
        and curvEnd. This is how the OpenDrive format specifies
        EulerSpirals.

        Args:
          length: Length of EulerSpiral.
          curvStart: Curvature at start of EulerSpiral.
          curvEnd: Curvature at end of EulerSpiral.

        Returns:
          EulerSpiral: A new Clothoid.

        """
        # if length is zero, assume zero curvature
        if length == 0:
            return EulerSpiral(0)
        return EulerSpiral(1 * (curvEnd - curvStart) / length)

    def calc(self, s, x0=0, y0=0, kappa0=0, theta0=0):
        """

        Args:
          s:
          x0:  (Default value = 0)
          y0:  (Default value = 0)
          kappa0:  (Default value = 0)  curvStart
          theta0:  (Default value = 0)  heading
          gamma： 1 * (curvEnd - curvStart) / length

        Returns:

        """

        # Start
        C0 = x0 + 1j * y0

        if self._gamma == 0 and kappa0 == 0:
            # Straight line
            Cs = C0 + np.exp(1j * theta0 * s)

        elif self._gamma == 0 and kappa0 != 0:
            # Arc
            Cs = C0 + np.exp(1j * theta0) / kappa0 * (
                    np.sin(kappa0 * s) + 1j * (1 - np.cos(kappa0 * s))
            )

        else:
            # Fresnel integrals
            Sa, Ca = special.fresnel(
                (kappa0 + self._gamma * s) / np.sqrt(np.pi * np.abs(self._gamma))
            )
            Sb, Cb = special.fresnel(kappa0 / np.sqrt(np.pi * np.abs(self._gamma)))

            # Euler Spiral
            Cs1 = np.sqrt(np.pi / np.abs(self._gamma)) * np.exp(
                1j * (theta0 - kappa0 ** 2 / 2 / self._gamma)
            )
            Cs2 = np.sign(self._gamma) * (Ca - Cb) + 1j * Sa - 1j * Sb

            Cs = C0 + Cs1 * Cs2

        # Tangent at each point
        theta = self._gamma * s ** 2 / 2 + kappa0 * s + theta0

        return (Cs.real, Cs.imag, theta)


def spiral_cal_pos(x_start, y_start, s_pos, curvStart, curvEnd, hdg_start):
    _spiral = EulerSpiral.createFromLengthAndCurvature(s_pos, curvStart, curvEnd)
    (x, y, t) = _spiral.calc(s_pos, x_start, y_start, curvStart, hdg_start)

    x_start, y_start = x, y
    return x_start, y_start


def parampoly3_cal_pos(x_start, y_start, s_pos, length, pRange, aU, bU, cU, dU, aV, bV, cV, dV, hdg_start):
    pos = (s_pos / length) * pRange

    coeffsU = [aU, bU, cU, dU]
    coeffsV = [aV, bV, cV, dV]

    x = np.polynomial.polynomial.polyval(pos, coeffsU)
    y = np.polynomial.polynomial.polyval(pos, coeffsV)

    xrot = x * np.cos(hdg_start) - y * np.sin(hdg_start)
    yrot = x * np.sin(hdg_start) + y * np.cos(hdg_start)

    # Tangent is defined by derivation
    dCoeffsU = coeffsU[1:] * np.array(np.arange(1, len(coeffsU)))
    dCoeffsV = coeffsV[1:] * np.array(np.arange(1, len(coeffsV)))

    dx = np.polynomial.polynomial.polyval(pos, dCoeffsU)
    dy = np.polynomial.polynomial.polyval(pos, dCoeffsV)

    tangent = np.arctan2(dy, dx)
    hdg_end = hdg_start + tangent

    # self.start_position + np.array([xrot, yrot])

    return x_start + xrot, y_start + yrot, hdg_end


# 由s起点终点、geometry起点航向角和车道信息，计算得到道路起始s和四个坐标点
def get_geometry_point(start_s, end_s, hdg_s, geometry, road):
    road_id = road.get('id')
    start_x = geometry['x_start']  # geometry段的起点坐标
    start_y = geometry['y_start']

    line_type = geometry['line_type']
    curv_start = geometry['curvStart']  # start_s的曲率
    curv_end = geometry['curvEnd']  # end_s的曲率
    hdg = geometry['hdg_start']

    s_start_pos = start_s - hdg_s  # 以hdg_s为参考起点，计算start_s处的s_pos
    paramPoly3_Coeff = {}

    if line_type == 'line':
        hdg_start = hdg
        x_start, y_start = line_cal_pos(start_x, start_y, s_start_pos, hdg)
    elif line_type == 'arc':
        hdg_start = hdg + (s_start_pos * curv_start)  # 计算s_pos处的航向角
        x_start, y_start = arc_cal_pos(start_x, start_y, s_start_pos, hdg, curv_start)
    elif line_type == 'spiral':
        hdg_start = hdg + s_start_pos * (curv_end - curv_start) * 0.5
        x_start, y_start = spiral_cal_pos(start_x, start_y, s_start_pos, curv_start, curv_end, hdg)
    elif line_type == 'paramPoly3':
        length = geometry['length']
        pRange = geometry['pRange']
        aU = geometry['aU']
        bU = geometry['bU']
        cU = geometry['cU']
        dU = geometry['dU']
        aV = geometry['aV']
        bV = geometry['bV']
        cV = geometry['cV']
        dV = geometry['dV']
        paramPoly3_Coeff = {'length': length, 'pRange': pRange, 'aU': aU, 'bU': bU, 'cU': cU, 'dU': dU,
                            'aV': aV, 'bV': bV, 'cV': cV, 'dV': dV}
        x_start, y_start, hdg_start = parampoly3_cal_pos(start_x, start_y, s_start_pos, length, pRange,
                                                         aU, bU, cU, dU, aV, bV, cV, dV, hdg)

    # 计算各段起点终点高程ele_start, ele_end
    elevation_dict = get_elevation(road)
    s_ele, a_ele, b_ele, c_ele, d_ele = 0, 0, 0, 0, 0
    for ele in elevation_dict:
        s_elevation = elevation_dict[ele]['s']
        if start_s >= s_elevation:
            s_ele = start_s - s_elevation
            a_ele = elevation_dict[ele]['a']
            b_ele = elevation_dict[ele]['b']
            c_ele = elevation_dict[ele]['c']
            d_ele = elevation_dict[ele]['d']
        else:
            break
    ele_start = evaluate_elevation(s_ele, a_ele, b_ele, c_ele, d_ele)

    s_ele, a_ele, b_ele, c_ele, d_ele = 0, 0, 0, 0, 0
    for ele in elevation_dict:
        s_elevation = elevation_dict[ele]['s']
        if end_s >= s_elevation:
            s_ele = end_s - s_elevation
            a_ele = elevation_dict[ele]['a']
            b_ele = elevation_dict[ele]['b']
            c_ele = elevation_dict[ele]['c']
            d_ele = elevation_dict[ele]['d']
        else:
            break
    ele_end = evaluate_elevation(s_ele, a_ele, b_ele, c_ele, d_ele)

    # 先计算出各分段start_s处的 t_left 和 t_right
    for lane_section in road.iter('laneSection'):  # 找到start_s 在哪个lanesection范围
        s_laneSec = float(lane_section.get('s'))
        if start_s >= s_laneSec:
            t_left = 0
            t_right = 0
            ss, aa, bb, cc, dd = 0, 0, 0, 0, 0
            for lane in lane_section.iter('lane'):
                if float(lane.get('id')) > 0:  # 左正右负
                    width_dict = get_width(lane)
                    for width in width_dict:
                        sOffset = width_dict[width]['sOffset']
                        if start_s >= s_laneSec + sOffset:
                            ss = start_s - (s_laneSec + sOffset)
                            aa = width_dict[width]['a']
                            bb = width_dict[width]['b']
                            cc = width_dict[width]['c']
                            dd = width_dict[width]['d']
                        else:
                            break
                    t_left += evaluate_elevation(ss, aa, bb, cc, dd)
                elif float(lane.get('id')) < 0:
                    width_dict = get_width(lane)
                    for width in width_dict:
                        sOffset = width_dict[width]['sOffset']
                        if start_s >= s_laneSec + sOffset:
                            ss = start_s - (s_laneSec + sOffset)
                            aa = width_dict[width]['a']
                            bb = width_dict[width]['b']
                            cc = width_dict[width]['c']
                            dd = width_dict[width]['d']
                        else:
                            break
                    t_right += evaluate_elevation(ss, aa, bb, cc, dd)
        else:
            break

    # 将laneoffset纳入考虑，调整t_left和t_right
    laneOffset_dict = get_laneOffset(road)
    if laneOffset_dict:
        sss, aaa, bbb, ccc, ddd = 0, 0, 0, 0, 0
        for laneOffset in laneOffset_dict:
            s_laneOffset = laneOffset_dict[laneOffset]['s']
            if start_s >= s_laneOffset:
                sss = start_s - s_laneOffset
                aaa = laneOffset_dict[laneOffset]['a']
                bbb = laneOffset_dict[laneOffset]['b']
                ccc = laneOffset_dict[laneOffset]['c']
                ddd = laneOffset_dict[laneOffset]['d']
            else:  # 超过s，则上一段的位置符合，利用保存的参数算出laneOffset
                break
        t_left += evaluate_elevation(sss, aaa, bbb, ccc, ddd)
        t_right -= evaluate_elevation(sss, aaa, bbb, ccc, ddd)

    # 四个顶点的坐标
    x1 = x_start - t_left * math.sin(hdg_start)
    y1 = y_start + t_left * math.cos(hdg_start)
    x2 = x_start + t_right * math.sin(hdg_start)
    y2 = y_start - t_right * math.cos(hdg_start)

    return {
        'road_id': road_id,
        's_start': start_s,
        's_end': end_s,
        'x_start': x_start,
        'y_start': y_start,
        'x1': x1,
        'y1': y1,
        'x2': x2,
        'y2': y2,
        # 'x_end': x_end,
        # 'y_end': y_end,
        'x3': 0,
        'y3': 0,
        'x4': 0,
        'y4': 0,
        'ele_start': ele_start,
        'ele_end': ele_end,
        'geometry': line_type,
        'paramPoly3': paramPoly3_Coeff,
        'hdg_start': hdg_start,
        # 'hdg_end': hdg_end,
        't_left': t_left,
        't_right': t_right
    }


# 射线法，判断p[x,y]是否在poly[[],[],[],[]]里
# 输入：需判断的点p，和多边形顶点坐标列表poly
# 输出：False、True
def is_in_poly(p, poly):
    """
    :param p: [x, y]
    :param poly: [[], [], [], [], ...]
    :return:
    """
    px, py = p
    is_in = False  # 预设不在

    for i, corner in enumerate(poly):  # corner是四个顶点
        next_i = i + 1 if i + 1 < len(poly) else 0

        x1, y1 = corner
        x2, y2 = poly[next_i]

        # if point is on vertex
        if (x1 == px and y1 == py) or (x2 == px and y2 == py):
            return True

        # find horizontal edges of polygon
        if min(y1, y2) < py <= max(y1, y2):
            x = x1 + (py - y1) * (x2 - x1) / (y2 - y1)
            if x == px:  # if point is on edge
                return True
            if x > px:  # intersect
                is_in = not is_in

    return is_in


# 计算可划分为多少个区域并生成dataframe
# 输入：xodr道路数据文件
# 输出：划分好的区域信息表格
def cal_area(xodr_path):
    tree = ET.parse(xodr_path)
    root = tree.getroot()
    columns_list = ['road_id', 's_start', 's_end', 'x1', 'y1', 'x2', 'y2', 'x3', 'y3', 'x4', 'y4',
                    'ele_start', 'ele_end', 'geometry', 'hdg_start', 'curvStart', 'curvEnd', 'paramPoly3']
    ele_df = pd.DataFrame(columns=columns_list)
    for road in root.iter('road'):
        # road_id = road.get('id')
        road_length = float(road.get('length'))
        geometry_dict = dict()
        road_section_dict = dict()
        lane_dict = dict()
        aU = 0
        bU = 0
        cU = 0
        dU = 0
        aV = 0
        bV = 0
        cV = 0
        dV = 0
        pRange = 0

        # 获取道路参考线信息
        count = 0
        for line in road.iter('geometry'):
            s = float(line.get('s'))
            x = float(line.get('x'))  # 东为正方向
            y = float(line.get('y'))  # 北为正方向
            hdg = float(line.get('hdg'))  # 正东为0 逆时针递增
            length = float(line.get('length'))
            count += 1
            geometry = line[0]
            line_type = geometry.tag

            if line_type == 'line':
                curv_start = 0
                curv_end = 0
            elif line_type == 'arc':
                curv_start = float(line.find('arc').get('curvature'))
                curv_end = curv_start
            elif line_type == 'spiral':
                curv_start = float(line.find('spiral').get('curvStart'))
                curv_end = float(line.find('spiral').get('curvEnd'))
            elif line_type == 'paramPoly3':
                aU = float(line.find('paramPoly3').get('aU'))
                bU = float(line.find('paramPoly3').get('bU'))
                cU = float(line.find('paramPoly3').get('cU'))
                dU = float(line.find('paramPoly3').get('dU'))
                aV = float(line.find('paramPoly3').get('aV'))
                bV = float(line.find('paramPoly3').get('bV'))
                cV = float(line.find('paramPoly3').get('cV'))
                dV = float(line.find('paramPoly3').get('dV'))
                pRange = (line.find('paramPoly3').get('pRange'))
                if pRange == 'normalized':
                    pRange = 1.0
                else:
                    pRange = length
                curv_start = 0
                curv_end = 0

            geometry_dict[count] = {
                's_start': s, 'x_start': x, 'y_start': y, 'x_end': 0, 'y_end': 0,
                'hdg_start': hdg, 'length': length, 'line_type': line_type,
                'curvStart': curv_start, 'curvEnd': curv_end, 'aU': aU, 'bU': bU, 'cU': cU, 'dU': dU,
                'aV': aV, 'bV': bV, 'cV': cV, 'dV': dV, 'pRange': pRange
            }

        # 获取区域分隔点高程
        count = 0
        for elevation_area in road.iter('elevation'):
            s = float(elevation_area.get('s'))
            a = float(elevation_area.get('a'))
            b = float(elevation_area.get('b'))
            c = float(elevation_area.get('c'))
            d = float(elevation_area.get('d'))
            road_section_dict[count] = {'s_start': s, 'a': a, 'b': b, 'c': c, 'd': d}
            count += 1

        # 将road_section的分隔点设置为上一段的终点
        for count, eve_section in road_section_dict.items():  # eve_section有什么用
            try:
                end_s = road_section_dict[count + 1]['s_start']
            except KeyError:
                end_s = road_length
            road_section_dict[count]['s_end'] = end_s

        # 用 s_laneSection 存laneoffset和lanesection需要分段的s
        s_lanes = []
        for lane_offset in road.iter('laneOffset'):
            start_s = float(lane_offset.get('s'))
            s_lanes.append(start_s)

        for lane_section in road.iter('laneSection'):
            s_laneSec = float(lane_section.get('s'))
            for lane in lane_section.iter('lane'):
                width_dict = get_width(lane)
                for width in width_dict:
                    sOffset = width_dict[width]['sOffset']
                    start_s = s_laneSec + sOffset
                    s_lanes.append(start_s)

        s_lanes = list(set(s_lanes))  # 去重
        s_lanes.sort()  # 对s排序

        count = 0
        for s_laneSec in s_lanes:
            lane_dict[count] = {'s_start': s_laneSec, 's_end': 0}
            count += 1

        # 将lane_dict的分隔点设为上一段的终点
        for index, lane_section in lane_dict.items():
            try:
                end_s = lane_dict[index + 1]['s_start']
            except KeyError:
                end_s = road_length
            lane_section['s_end'] = end_s
            lane_dict[index] = lane_section

        # 分类讨论
        for index_g, geometry in geometry_dict.items():
            geo_s_start = geometry['s_start']
            hdg_s_start = geometry['s_start']
            geo_s_end = geometry['s_start'] + geometry['length']  # length：沿参考线的里程
            line_type = geometry['line_type']
            c1 = geometry['curvStart']
            c2 = geometry['curvEnd']
            for index_l, lanes in lane_dict.items():
                if lanes['s_start'] <= geo_s_start < lanes['s_end']:
                    if lanes['s_end'] >= geo_s_end:
                        for section_index, road_section in road_section_dict.items():
                            s1 = road_section['s_start']
                            s2 = road_section['s_end']
                            # e1 = road_section['ele_start']
                            # e2 = road_section['ele_end']
                            if s1 <= geo_s_start <= s2:
                                if s2 >= geo_s_end:
                                    # ele_start = cal_ele_avg(s1, e1, s2, e2, geo_s_start)  # 计算geo_s_start处elevation
                                    # ele_end = cal_ele_avg(s1, e1, s2, e2, geo_s_end)  # 计算geo_s_end处elevation
                                    curv_start = cal_curv_avg(s1, c1, s2, c2, geo_s_start)
                                    curv_end = cal_curv_avg(s1, c1, s2, c2, geo_s_end)
                                    geometry['curvStart'] = curv_start
                                    geometry['curvEnd'] = curv_end
                                    # 算出分割区域的信息和顶点
                                    single_area_dict = get_geometry_point(geo_s_start, geo_s_end, hdg_s_start,
                                                                          geometry, road)
                                    # single_area_dict['ele_start'] = ele_start
                                    # single_area_dict['ele_end'] = ele_end
                                    # curvature根据参考线类型确定
                                    if line_type == 'line' or line_type == 'arc' or line_type == 'spiral':
                                        single_area_dict['curvStart'] = curv_start
                                        single_area_dict['curvEnd'] = curv_end
                                    else:
                                        pass
                                    ele_df = pd.concat([ele_df, pd.DataFrame([single_area_dict])], ignore_index=True)
                                    break
                                else:
                                    _ = geo_s_end
                                    geo_s_end = s2
                                    # ele_start = cal_ele_avg(s1, e1, s2, e2, geo_s_start)
                                    # ele_end = cal_ele_avg(s1, e1, s2, e2, geo_s_end)
                                    curv_start = cal_curv_avg(s1, c1, s2, c2, geo_s_start)
                                    curv_end = cal_curv_avg(s1, c1, s2, c2, geo_s_end)
                                    geometry['curvStart'] = curv_start
                                    geometry['curvEnd'] = curv_end
                                    single_area_dict = get_geometry_point(geo_s_start, geo_s_end, hdg_s_start,
                                                                          geometry, road)
                                    # single_area_dict['ele_start'] = ele_start
                                    # single_area_dict['ele_end'] = ele_end
                                    # curvature根据参考线类型确定
                                    if line_type == 'line' or line_type == 'arc' or line_type == 'spiral':
                                        single_area_dict['curvStart'] = curv_start
                                        single_area_dict['curvEnd'] = curv_end
                                    else:
                                        pass
                                    ele_df = pd.concat([ele_df, pd.DataFrame([single_area_dict])], ignore_index=True)
                                    geo_s_start = s2
                                    geo_s_end = _
                        break
                    else:
                        geo_s_end = lanes['s_end']
                        for section_index, road_section in road_section_dict.items():
                            s1 = road_section['s_start']
                            s2 = road_section['s_end']
                            # e1 = road_section['ele_start']
                            # e2 = road_section['ele_end']
                            if s1 <= geo_s_start <= s2:
                                if s2 >= geo_s_end:
                                    # ele_start = cal_ele_avg(s1, e1, s2, e2, geo_s_start)
                                    # ele_end = cal_ele_avg(s1, e1, s2, e2, geo_s_end)
                                    curv_start = cal_curv_avg(s1, c1, s2, c2, geo_s_start)
                                    curv_end = cal_curv_avg(s1, c1, s2, c2, geo_s_end)
                                    geometry['curvStart'] = curv_start
                                    geometry['curvEnd'] = curv_end
                                    single_area_dict = get_geometry_point(geo_s_start, geo_s_end, hdg_s_start,
                                                                          geometry, road)
                                    # single_area_dict['ele_start'] = ele_start
                                    # single_area_dict['ele_end'] = ele_end
                                    if line_type == 'line' or line_type == 'arc' or line_type == 'spiral':
                                        single_area_dict['curvStart'] = curv_start
                                        single_area_dict['curvEnd'] = curv_end
                                    else:
                                        pass
                                    ele_df = pd.concat([ele_df, pd.DataFrame([single_area_dict])], ignore_index=True)
                                    break
                                else:
                                    _ = geo_s_end
                                    geo_s_end = s2
                                    # ele_start = cal_ele_avg(s1, e1, s2, e2, geo_s_start)
                                    # ele_end = cal_ele_avg(s1, e1, s2, e2, geo_s_end)
                                    curv_start = cal_curv_avg(s1, c1, s2, c2, geo_s_start)
                                    curv_end = cal_curv_avg(s1, c1, s2, c2, geo_s_end)
                                    geometry['curvStart'] = curv_start
                                    geometry['curvEnd'] = curv_end
                                    single_area_dict = get_geometry_point(geo_s_start, geo_s_end, hdg_s_start,
                                                                          geometry, road)
                                    # single_area_dict['ele_start'] = ele_start
                                    # single_area_dict['ele_end'] = ele_end
                                    if line_type == 'line' or line_type == 'arc' or line_type == 'spiral':
                                        single_area_dict['curvStart'] = curv_start
                                        single_area_dict['curvEnd'] = curv_end
                                    else:
                                        pass
                                    ele_df = pd.concat([ele_df, pd.DataFrame([single_area_dict])], ignore_index=True)
                                    geo_s_start = s2
                                    geo_s_end = _
                        geo_s_start = lanes['s_end']
                        geo_s_end = geometry['s_start'] + geometry['length']

        # x3y3,x4y4通过下一段的x1y1,x2y2更新
        for index, polygon in ele_df.iterrows():
            if index < len(ele_df) - 1:
                if polygon['s_start'] == 0:
                    pass
                else:
                    ele_df.at[index - 1, 'x3'] = polygon['x1']
                    ele_df.at[index - 1, 'y3'] = polygon['y1']
                    ele_df.at[index - 1, 'x4'] = polygon['x2']
                    ele_df.at[index - 1, 'y4'] = polygon['y2']
            else:  # 最后一个section单独计算
                line_type = polygon['geometry']
                start_s = polygon['s_start']
                end_s = polygon['s_end']
                start_x = polygon['x_start']  # polygon段的起点坐标
                start_y = polygon['y_start']
                curv_start = polygon['curvStart']  # start_s的
                curv_end = polygon['curvEnd']  # end_s的曲率
                hdg = polygon['hdg_start']
                s_end_pos = end_s - start_s
                if line_type == 'line':
                    hdg_end = hdg
                    x_end, y_end = line_cal_pos(start_x, start_y, s_end_pos, hdg)
                elif line_type == 'arc':
                    hdg_end = hdg + (s_end_pos * curv_start)
                    x_end, y_end = arc_cal_pos(start_x, start_y, s_end_pos, hdg, curv_start)
                elif line_type == 'spiral':
                    hdg_end = hdg + s_end_pos * (curv_end - curv_start) * 0.5
                    x_end, y_end = spiral_cal_pos(start_x, start_y, s_end_pos, curv_start, curv_end, hdg)
                elif line_type == 'paramPoly3':
                    length = polygon['paramPoly3']['length']
                    pRange = polygon['paramPoly3']['pRange']
                    aU = polygon['paramPoly3']['aU']
                    bU = polygon['paramPoly3']['bU']
                    cU = polygon['paramPoly3']['cU']
                    dU = polygon['paramPoly3']['dU']
                    aV = polygon['paramPoly3']['aV']
                    bV = polygon['paramPoly3']['bV']
                    cV = polygon['paramPoly3']['cV']
                    dV = polygon['paramPoly3']['dV']
                    x_end, y_end, hdg_end = parampoly3_cal_pos(start_x, start_y, s_end_pos, length, pRange,
                                                               aU, bU, cU, dU, aV, bV, cV, dV, hdg)

                # 先计算出最后一段end_s处的 t_left 和 t_right
                for lane_section in road.iter('laneSection'):  # 找到start_s 在哪个lanesection范围
                    s_laneSec = float(lane_section.get('s'))
                    if end_s >= s_laneSec:
                        t_left = 0
                        t_right = 0
                        ss, aa, bb, cc, dd = 0, 0, 0, 0, 0
                        for lane in lane_section.iter('lane'):
                            if float(lane.get('id')) > 0:  # 左正右负
                                width_dict = get_width(lane)
                                for width in width_dict:
                                    sOffset = width_dict[width]['sOffset']
                                    if end_s >= s_laneSec + sOffset:
                                        ss = end_s - (s_laneSec + sOffset)
                                        aa = width_dict[width]['a']
                                        bb = width_dict[width]['b']
                                        cc = width_dict[width]['c']
                                        dd = width_dict[width]['d']
                                    else:
                                        break
                                t_left += evaluate_elevation(ss, aa, bb, cc, dd)
                            elif float(lane.get('id')) < 0:
                                width_dict = get_width(lane)
                                for width in width_dict:
                                    sOffset = width_dict[width]['sOffset']
                                    if end_s >= s_laneSec + sOffset:
                                        ss = end_s - (s_laneSec + sOffset)
                                        aa = width_dict[width]['a']
                                        bb = width_dict[width]['b']
                                        cc = width_dict[width]['c']
                                        dd = width_dict[width]['d']
                                    else:
                                        break
                                t_right += evaluate_elevation(ss, aa, bb, cc, dd)
                    else:
                        break
                # 将laneoffset纳入考虑，调整t_left和t_right
                laneOffset_dict = get_laneOffset(road)
                if laneOffset_dict:
                    sss, aaa, bbb, ccc, ddd = 0, 0, 0, 0, 0
                    for laneOffset in laneOffset_dict:
                        s_laneOffset = laneOffset_dict[laneOffset]['s']
                        if end_s >= s_laneOffset:
                            sss = end_s - s_laneOffset
                            aaa = laneOffset_dict[laneOffset]['a']
                            bbb = laneOffset_dict[laneOffset]['b']
                            ccc = laneOffset_dict[laneOffset]['c']
                            ddd = laneOffset_dict[laneOffset]['d']
                        else:  # 超过s，则上一段的位置符合，利用保存的参数算出laneOffset
                            break
                    t_left += evaluate_elevation(sss, aaa, bbb, ccc, ddd)
                    t_right -= evaluate_elevation(sss, aaa, bbb, ccc, ddd)

                # 终点两个顶点的坐标
                x3 = x_end - t_left * math.sin(hdg_end)
                y3 = y_end + t_left * math.cos(hdg_end)
                x4 = x_end + t_right * math.sin(hdg_end)
                y4 = y_end - t_right * math.cos(hdg_end)

                ele_df.at[index, 'x3'] = x3
                ele_df.at[index, 'y3'] = y3
                ele_df.at[index, 'x4'] = x4
                ele_df.at[index, 'y4'] = y4

    return ele_df


# 获得高程
# 输入：高程参数，需计算点的xy坐标
# 输出：xy点对应的高程
def cal_elevation(ele_df, x, y):
    ele_flag = False
    for index, polygon in ele_df.iterrows():
        p1 = [polygon['x1'], polygon['y1']]
        p2 = [polygon['x2'], polygon['y2']]
        p3 = [polygon['x3'], polygon['y3']]
        p4 = [polygon['x4'], polygon['y4']]
        # line_type = polygon['geometry']

        # 如果在多边形内，获得参考线起点x0y0和终点x1y1，然后计算得出目标点的s和高程
        if is_in_poly([x, y], [p1, p2, p4, p3]):
            x0 = (polygon['x1'] + polygon['x2']) / 2
            y0 = (polygon['y1'] + polygon['y2']) / 2
            x1 = (polygon['x3'] + polygon['x4']) / 2
            y1 = (polygon['y3'] + polygon['y4']) / 2

            # 直线、弧线、螺旋线、参数三次多项式
            A = y1 - y0
            B = x0 - x1
            C = x1 * y0 - x0 * y1
            if A == 0:
                result_x = x
                result_y = y0
            elif B == 0:
                result_x = x0
                result_y = y
            else:
                k = -1 * A / B
                b = -1 * C / B
                k0 = 1 / -k
                b0 = y - k0 * x
                result_x = (b0 - b) / (k - k0)
                result_y = (k * b0 - k * b) / (k - k0) + b

            result_s = dis_point_to_seg_line([result_x, result_y], p1, p2)  # 点到直线的距离，计算s
            result_ele = cal_ele_avg(polygon['s_start'], polygon['ele_start'], polygon['s_end'], polygon['ele_end'],
                                     result_s)

            ele_flag = True
            break  # 找到第一段就

        # 双指针共同搜索
        back = len(ele_df) - index -1
        if index >= back:
            break

        polygon = ele_df.iloc[back]
        p1 = [polygon['x1'], polygon['y1']]
        p2 = [polygon['x2'], polygon['y2']]
        p3 = [polygon['x3'], polygon['y3']]
        p4 = [polygon['x4'], polygon['y4']]

        if is_in_poly([x, y], [p1, p2, p4, p3]):
            x0 = (polygon['x1'] + polygon['x2']) / 2
            y0 = (polygon['y1'] + polygon['y2']) / 2
            x1 = (polygon['x3'] + polygon['x4']) / 2
            y1 = (polygon['y3'] + polygon['y4']) / 2

            # 直线、弧线、螺旋线、参数三次多项式
            A = y1 - y0
            B = x0 - x1
            C = x1 * y0 - x0 * y1
            if A == 0:
                result_x = x
                result_y = y0
            elif B == 0:
                result_x = x0
                result_y = y
            else:
                k = -1 * A / B
                b = -1 * C / B
                k0 = 1 / -k
                b0 = y - k0 * x
                result_x = (b0 - b) / (k - k0)
                result_y = (k * b0 - k * b) / (k - k0) + b

            result_s = dis_point_to_seg_line([result_x, result_y], p1, p2)  # 点到直线的距离，计算s
            result_ele = cal_ele_avg(polygon['s_start'], polygon['ele_start'], polygon['s_end'], polygon['ele_end'],
                                     result_s)

            ele_flag = True
            break  # 找到第一段就

    if ele_flag:
        return result_ele
    else:
        return 0

if __name__ == '__main__':
    # x, y = -263.18, -58.70
    # x, y = -268.31, -59.51
    # x, y = -262.22, -56.56
    # x, y = -965.71, 1111.93
    # x, y = -954.95, 1102.95
    # x, y = 5813.42, 438.44
    # x, y = 5597.10, 1006.45
    # x, y = 5581.83, 1009.99
    # x, y = 5571.28, 1010.88
    # x, y = 5437.80, 829.45
    # x, y = 5449.90, 853.85

    # x, y = -261.75, -57.11
    # x, y = -267.91, -61.77
    # x, y = -240.85, -173.19
    # x, y = - 237.87, -173.16
    # x, y = 5803.37, 430.64
    # x, y = 5811.24, 435.88
    # x, y = 5816.87, 440.53
    # x, y = 5812.1,443.89
    # x, y = 5806.17, 437.40
    # x, y = 5799.90, 433.08
    # x, y = -1066.83, 1280.84
    # x, y = -1049.69, 1232.90
    # x, y = -1012.39, 1160.46
    # x, y = -809.62, 686.23
    # x, y = -962.05, 1187.23
    # x, y = -396.51, -446.58

    # x, y = -262.59, -59.86
    # x, y = -236.43, -173.84
    # x, y = -236.34, -178.19
    # x, y = 5809.46, 437.56
    # x, y = 5818.15, 445.58
    # x, y = 5580.43, 1017.73
    # x, y = 5453.30, 853.82
    # x, y = -1097.90, 1307.23
    # x, y = -858.63, 771.05

    # taiehqiao0211_lw.xodr
    x, y = -255.89, -53.07
    # x, y = -262.60, -52.09
    # x, y = -229.25, -168.76
    # x, y = -142.65, -115.74
    # x, y = -192.30, -22.96
    # x, y = -254.14, -54.55
    # x, y = 156.45, 40.58
    # x, y = 139.39, 30.61
    # x, y = 268.23, 84.35
    # x, y = 163.38, 15.58
    # x, y = -106.99, -26.86
    # x, y = -103.33, -12.92

    T1_run = time.time()

    # xodr_path = r'C:\Users\cicv\PycharmProjects\pythonProject\Elevation_cal/yangli.xodr'
    # xodr_path = r'C:\Users\cicv\PycharmProjects\pythonProject\Elevation_cal/anting_new_town.xodr'
    # xodr_path = r'C:\Users\cicv\PycharmProjects\pythonProject\Elevation_cal/taiheqiao_1227_4.xodr'
    xodr_path = r'/home/lxj/Documents/data/jiuzhonglu/jiuzhonglu_0308_1.xodr'
    # xodr_path = r'C:\Users\cicv\PycharmProjects\pythonProject\Elevation_cal\test_taiehqiao_0310.xodr'

    elevation_df = cal_area(xodr_path)

    # 将df输出到csv文件
    elevation_df.to_csv(r'/home/lxj/Documents/data/jiuzhonglu/jiuzhonglu.csv',index=False)

    T2_run = time.time()
    z = cal_elevation(elevation_df, x, y)
    T3_run = time.time()
    print("The elevation of (x:", x, ", y:", y, ") is:", z)
    print('cal_area() run time:%s s' % ((T2_run - T1_run)))
    print('cal_elevation() run time:%s s' % ((T3_run - T2_run)))
