from typing import Dict, List

from shapely.geometry import Polygon, Point

from pyrate.plan.geometry.polygon import CartesianPolygon
from pyrate.plan.nearplanner.cost_functions import *
from pyrate.plan.nearplanner.evaluated_timing_frame import EvaluatedTimingFrame
from pyrate.plan.nearplanner.holders import EstimationParameters
from pyrate.plan.nearplanner.holders import OptimizationParameters
from pyrate.plan.nearplanner.obstacle import Obstacle
from pyrate.plan.nearplanner.optimizer import Optimizer

estimation_param = EstimationParameters()
optimization_param = OptimizationParameters(estimation_param)

optimization_param.verbose = False


def create_context(
    position: Point,
    goal: Point,
    wind: Tuple[float, float],
    obstacles: Dict[str, Polygon],
    costfunction_obstacles_width: float = 40,
    costfunction_obstacles_scale: float = 0.02,
) -> Optimizer:
    fct = CostFunctionExp(scale=costfunction_obstacles_scale, safety_dist=costfunction_obstacles_width)

    cartesian_polys: Dict[str, CartesianPolygon] = {
        i: CartesianPolygon.from_shapely(k) for i, k in obstacles.items()
    }

    obstacle_dict: Dict[str, Obstacle] = {
        i: Obstacle(poly, np.array([0, 0]), cost_function=fct) for i, poly in cartesian_polys.items()
    }

    context = Optimizer(wind_information=(0, 0), obstacles={}, position=Point(0, 0))

    context.wind_speed, context.wind_angle = wind[0], np.deg2rad(wind[1])

    context.position = position
    context.goal = goal

    context.on_reset_obstacles(obstacle_dict)

    return context


# sueden nach norden -> wind (25 m/s, 0)
def generate_route(
    position: Point,
    goal: Point,
    wind: Tuple[float, float],
    obstacles: Dict[str, Polygon],
    costfunction_obstacles_width: float = 40,
    costfunction_obstacles_scale: float = 0.02,
) -> Tuple[Optional[EvaluatedTimingFrame], Optional[List[EvaluatedTimingFrame]]]:
    """Function that generates a route in the first of the tuple return value.
    Second value contains a list of timing frames created during optimization.
    """

    context = create_context(
        position=position,
        goal=goal,
        wind=wind,
        obstacles=obstacles,
        costfunction_obstacles_width=costfunction_obstacles_width,
        costfunction_obstacles_scale=costfunction_obstacles_scale,
    )

    return context.optimize(goal=goal, optimization_parameters=optimization_param)


# poly = Polygon([(-2, 1), (-0.1, 3), (3, 3), (2,1)])
# poly = Polygon([(-80, 10), (-50, 30), (30, 30), (20,20), (0,20)])

# poly_ = Polygon([(1000,1000),(1010,1010),(1000,1020)])
# poly2 = Polygon([(-50, 70), (50, 70), (50, 80), (-50,80)])

# ob_set = {"0": poly2, "1": poly_, "2": poly}
# ob_set_2 = {"0": poly_}
# print("START")

# route = [Point(0,0), Point(0,1), Point(0,2), Point(2,2), Point(2,1), Point(-2,2), Point(0,10)]
# _ = np.array([shapely_point_to_ndarray(p) for p in route])
# print(_)
# frame = TimingFrame(CartesianRoute.from_numpy(_))
# print(frame)
# frame2 = frame.remove_single_cycles()

# print(frame2)
# print("_"*10)
# print(frame.remove_single_cycles())