#
# Copyright (c) 2021 Czech Technical University in Prague.
#
# This file is part of Roadmaptools 
# (see https://github.com/aicenter/roadmap-processing).
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
#
import math
import geojson
import codecs
import sys
import argparse
import time

from roadmaptools.printer import print_info
from roadmaptools.init import config


def compute_edge_curvatures(input_filename: str, output_filename: str):

    print_info('Computing average edge curvatures.')
    start_time = time.time()

    input_stream = open(input_filename, encoding='utf8')
    output_stream = open(output_filename, 'w')

    print_info("Loading geojson from: {}".format(input_filename))
    geojson_file = load_geojson(input_stream)

    print_info("Calculating curvature")
    geojson_out = get_geojson_with_curvature(geojson_file)

    print_info("Saving geojson to: {}".format(output_filename))
    save_geojson(geojson_out, output_stream)
    input_stream.close()
    output_stream.close()

    print_info('Curvature computation process finished. (%.2f secs)' % (time.time() - start_time))


def calculate_curvature(input_stream, output_stream):
    json_dict = load_geojson(input_stream)
    analyse_roads(json_dict)
    save_geojson(json_dict, output_stream)


def get_geojson_with_curvature(json_dict):
    analyse_roads(json_dict)
    return json_dict


def get_node(node):  # latlon
    return (node[1], node[0])


def get_distance_between_coords(point1, point2):
    """
    Computes the distance between to GPS coordinates.
    :param point1:
    :param point2:
    :return: distance between two GPS coordinates in meters
    """
    R = 6371000
    lat1 = math.radians(point1[0])
    lat2 = math.radians(point2[0])
    lat = math.radians(point2[0] - point1[0])
    lon = math.radians(point2[1] - point1[1])

    a = math.sin(lat / 2) * math.sin(lat / 2) + math.cos(lat1) * math.cos(lat2) * math.sin(lon / 2) * math.sin(lon / 2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
    distance = R * c
    return distance


#        C
#       /\
#    c /  \ a
#     /    \
#   A ----- B
#       b
def calculate_angle_in_degree(a, b, c):
    if a + c > b:  # check if it is a triangle
        angle = math.acos((a ** 2 + c ** 2 - b ** 2) / (2 * a * c))  # in radians
    else:
        angle = 0
    result = abs(180 - math.degrees(angle))
    return result


def get_length(coords):
    length = 0
    for i in range(0, len(coords) - 1):
        point1 = get_node(coords[i])
        point2 = get_node(coords[i + 1])
        length += get_distance_between_coords(point1, point2)
    return length


def get_curvature(coords):
    if len(coords) < 3:
        return [0, 0]  # no curvature on edge
    else:
        total_curvature = 0
        max_curvature = -1
        length_of_edge = 0
        for i in range(0, len(coords) - 2):
            point_a = get_node(coords[i])
            point_b = get_node(coords[i + 1])
            point_c = get_node(coords[i + 2])

            length_c = get_distance_between_coords(point_a, point_b)
            length_a = get_distance_between_coords(point_b, point_c)
            length_b = get_distance_between_coords(point_c, point_a)

            # k = 0.5 * (length_b+length_a+length_c)
            # area = math.sqrt(k*(k-length_a)*(k-length_b)*(k-length_c))
            # radius = (length_b*length_a*length_c)/(4*area)
            angle = calculate_angle_in_degree(length_a, length_b, length_c)
            distance = length_c + length_a
            curvature = angle / distance
            if curvature > max_curvature:
                max_curvature = curvature
            total_curvature += angle
            length_of_edge += distance

        return [total_curvature / length_of_edge, max_curvature]


def load_geojson(in_stream):
    json_dict = geojson.load(in_stream)
    return json_dict


def analyse_roads(json_dict):
    for item in json_dict['features']:
        cur = get_curvature(item['geometry']['coordinates'])
        item['properties']['curvature'] = cur[0]
        item['properties']['max_curvature'] = cur[1]


def save_geojson(json_dict, out_stream):
    geojson.dump(json_dict, out_stream)


def get_args():
    parser = argparse.ArgumentParser()
    parser.add_argument('-i', dest="input", type=str, action='store', help='input file')
    parser.add_argument('-o', dest="output", type=str, action='store', help='output file')
    return parser.parse_args()


# EXAMPLE OF USAGE
if __name__ == '__main__':
    args = get_args()
    input_stream = sys.stdin
    output_stream = sys.stdout

    if args.input is not None:
        input_stream = codecs.open(args.input, encoding='utf8')
    if args.output is not None:
        output_stream = codecs.open(args.output, 'w')

    calculate_curvature(input_stream, output_stream)
    input_stream.close()
    output_stream.close()

#
# Copyright (c) 2021 Czech Technical University in Prague.
#
# This file is part of Roadmaptools 
# (see https://github.com/aicenter/roadmap-processing).
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
#
import math
import geojson
import codecs
import sys
import argparse
import time

from roadmaptools.printer import print_info
from roadmaptools.init import config


def compute_edge_curvatures(input_filename: str, output_filename: str):

    print_info('Computing average edge curvatures.')
    start_time = time.time()

    input_stream = open(input_filename, encoding='utf8')
    output_stream = open(output_filename, 'w')

    print_info("Loading geojson from: {}".format(input_filename))
    geojson_file = load_geojson(input_stream)

    print_info("Calculating curvature")
    geojson_out = get_geojson_with_curvature(geojson_file)

    print_info("Saving geojson to: {}".format(output_filename))
    save_geojson(geojson_out, output_stream)
    input_stream.close()
    output_stream.close()

    print_info('Curvature computation process finished. (%.2f secs)' % (time.time() - start_time))


def calculate_curvature(input_stream, output_stream):
    json_dict = load_geojson(input_stream)
    analyse_roads(json_dict)
    save_geojson(json_dict, output_stream)


def get_geojson_with_curvature(json_dict):
    analyse_roads(json_dict)
    return json_dict


def get_node(node):  # latlon
    return (node[1], node[0])


def get_distance_between_coords(point1, point2):
    """
    Computes the distance between to GPS coordinates.
    :param point1:
    :param point2:
    :return: distance between two GPS coordinates in meters
    """
    R = 6371000
    lat1 = math.radians(point1[0])
    lat2 = math.radians(point2[0])
    lat = math.radians(point2[0] - point1[0])
    lon = math.radians(point2[1] - point1[1])

    a = math.sin(lat / 2) * math.sin(lat / 2) + math.cos(lat1) * math.cos(lat2) * math.sin(lon / 2) * math.sin(lon / 2)
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
    distance = R * c
    return distance


#        C
#       /\
#    c /  \ a
#     /    \
#   A ----- B
#       b
def calculate_angle_in_degree(a, b, c):
    if a + c > b:  # check if it is a triangle
        angle = math.acos((a ** 2 + c ** 2 - b ** 2) / (2 * a * c))  # in radians
    else:
        angle = 0
    result = abs(180 - math.degrees(angle))
    return result


def get_length(coords):
    length = 0
    for i in range(0, len(coords) - 1):
        point1 = get_node(coords[i])
        point2 = get_node(coords[i + 1])
        length += get_distance_between_coords(point1, point2)
    return length


def get_curvature(coords):
    if len(coords) < 3:
        return [0, 0]  # no curvature on edge
    else:
        total_curvature = 0
        max_curvature = -1
        length_of_edge = 0
        for i in range(0, len(coords) - 2):
            point_a = get_node(coords[i])
            point_b = get_node(coords[i + 1])
            point_c = get_node(coords[i + 2])

            length_c = get_distance_between_coords(point_a, point_b)
            length_a = get_distance_between_coords(point_b, point_c)
            length_b = get_distance_between_coords(point_c, point_a)

            # k = 0.5 * (length_b+length_a+length_c)
            # area = math.sqrt(k*(k-length_a)*(k-length_b)*(k-length_c))
            # radius = (length_b*length_a*length_c)/(4*area)
            angle = calculate_angle_in_degree(length_a, length_b, length_c)
            distance = length_c + length_a
            curvature = angle / distance
            if curvature > max_curvature:
                max_curvature = curvature
            total_curvature += angle
            length_of_edge += distance

        return [total_curvature / length_of_edge, max_curvature]


def load_geojson(in_stream):
    json_dict = geojson.load(in_stream)
    return json_dict


def analyse_roads(json_dict):
    for item in json_dict['features']:
        cur = get_curvature(item['geometry']['coordinates'])
        item['properties']['curvature'] = cur[0]
        item['properties']['max_curvature'] = cur[1]


def save_geojson(json_dict, out_stream):
    geojson.dump(json_dict, out_stream)


def get_args():
    parser = argparse.ArgumentParser()
    parser.add_argument('-i', dest="input", type=str, action='store', help='input file')
    parser.add_argument('-o', dest="output", type=str, action='store', help='output file')
    return parser.parse_args()


# EXAMPLE OF USAGE
if __name__ == '__main__':
    args = get_args()
    input_stream = sys.stdin
    output_stream = sys.stdout

    if args.input is not None:
        input_stream = codecs.open(args.input, encoding='utf8')
    if args.output is not None:
        output_stream = codecs.open(args.output, 'w')

    calculate_curvature(input_stream, output_stream)
    input_stream.close()
    output_stream.close()

# # Copyright (c) 2021 Czech Technical University in Prague. # # This file is part of Roadmaptools # (see https://github.com/aicenter/roadmap-processing). # # This program is free software: you can redistribute it and/or modify # it under the terms of the GNU Lesser General Public License as published by # the Free Software Foundation, either version 3 of the License, or # (at your option) any later version. # # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the # GNU Lesser General Public License for more details. # # You should have received a copy of the GNU Lesser General Public License # along with this program. If not, see <http://www.gnu.org/licenses/>. # # latlon Computes the distance between to GPS coordinates.     :param point1:     :param point2:     :return: distance between two GPS coordinates in meters #        C #       /\ #    c /  \ a #     /    \ #   A ----- B #       b # check if it is a triangle # in radians # no curvature on edge # k = 0.5 * (length_b+length_a+length_c) # area = math.sqrt(k*(k-length_a)*(k-length_b)*(k-length_c)) # radius = (length_b*length_a*length_c)/(4*area) # EXAMPLE OF USAGE