#!/usr/bin/env python3
# PFEIFER - SLC - Modified by FrogAi for FrogPilot
import calendar
import json
import math
import numpy as np
import requests

from concurrent.futures import ThreadPoolExecutor
from datetime import datetime

import openpilot.system.sentry as sentry

from openpilot.common.conversions import Conversions as CV
from openpilot.common.realtime import DT_MDL
from openpilot.common.time import system_time_valid

from openpilot.frogpilot.common.frogpilot_utilities import calculate_bearing_offset, calculate_distance_to_point, is_url_pingable
from openpilot.frogpilot.common.frogpilot_variables import TO_RADIANS, State, params, params_cache, params_memory

FREE_MAPBOX_REQUESTS = 100_000

class SpeedLimitController:
  def __init__(self):
    self.executor = ThreadPoolExecutor(max_workers=1)

    self.calling_mapbox = False
    self.override_slc = False

    self.denied_target = 0
    self.map_speed_limit = 0
    self.mapbox_limit = 0
    self.next_speed_limit = 0
    self.overridden_speed = 0
    self.segment_distance = 0
    self.speed_limit_changed_timer = 0
    self.target = 0
    self.unconfirmed_speed_limit = 0

    self.previous_source = "None"
    self.source = "None"

    self.mapbox_requests = json.loads(params.get("MapBoxRequests") or "{}")
    self.mapbox_requests.setdefault("month", datetime.now().month)
    self.mapbox_requests.setdefault("total_requests", 0)
    self.mapbox_requests.setdefault("max_requests", FREE_MAPBOX_REQUESTS - (28 * 100))

    self.mapbox_host = "https://api.mapbox.com"
    self.mapbox_token = params_cache.get("MapboxSecretKey", encoding="utf8")

    self.previous_target = params.get_float("PreviousSpeedLimit")

  @property
  def experimental_mode(self):
    return self.frogpilot_toggles.slc_fallback_experimental_mode and self.target == 0

  @property
  def offset(self):
    if self.frogpilot_toggles.is_metric:
      offset_map = [
        (0, 8.1, self.frogpilot_toggles.speed_limit_offset1),      # 0–29 km/h
        (8.1, 13.6, self.frogpilot_toggles.speed_limit_offset2),   # 30–49
        (13.6, 16.4, self.frogpilot_toggles.speed_limit_offset3),  # 50–59
        (16.4, 21.9, self.frogpilot_toggles.speed_limit_offset4),  # 60–79
        (21.9, 27.5, self.frogpilot_toggles.speed_limit_offset5),  # 80–99
        (27.5, 33.1, self.frogpilot_toggles.speed_limit_offset6),  # 100–119
        (33.1, 38.9, self.frogpilot_toggles.speed_limit_offset7),  # 120–140
      ]
    else:
      offset_map = [
        (0, 11.2, self.frogpilot_toggles.speed_limit_offset1),     # 0–24 mph
        (11.2, 15.2, self.frogpilot_toggles.speed_limit_offset2),  # 25–34
        (15.2, 19.6, self.frogpilot_toggles.speed_limit_offset3),  # 35–44
        (19.6, 24.1, self.frogpilot_toggles.speed_limit_offset4),  # 45–54
        (24.1, 28.6, self.frogpilot_toggles.speed_limit_offset5),  # 55–64
        (28.6, 33.1, self.frogpilot_toggles.speed_limit_offset6),  # 65–74
        (33.1, 44.2, self.frogpilot_toggles.speed_limit_offset7),  # 75–99
      ]
    return next((offset for low, high, offset in offset_map if low < self.target < high), 0)

  def get_mapbox_speed_limit(self, gps_position, v_ego):
    try:
      if not gps_position or not self.mapbox_token:
        self.mapbox_limit = 0
        self.segment_distance = 0
        return

      if v_ego == 0:
        return

      if self.segment_distance > 0:
        self.segment_distance -= v_ego * DT_MDL
        return

      if self.calling_mapbox:
        self.segment_distance = v_ego
        return

      def make_request():
        try:
          self.calling_mapbox = True

          successful = False

          if not is_url_pingable(self.mapbox_host):
            return None

          if system_time_valid():
            current_month = datetime.now().month
            if current_month != self.mapbox_requests.get("month"):
              self.mapbox_requests.update({
                "month": current_month,
                "total_requests": 0,
                "max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(datetime.now().year, current_month)[1] * 100,
              })

          self.mapbox_requests["total_requests"] += 1
          params.put_nonblocking("MapBoxRequests", json.dumps(self.mapbox_requests))

          current_bearing = gps_position.get("bearing")
          current_latitude = gps_position.get("latitude")
          current_longitude = gps_position.get("longitude")

          future_latitude, future_longitude = calculate_bearing_offset(current_latitude, current_longitude, current_bearing, v_ego)

          url = f"{self.mapbox_host}/matching/v5/mapbox/driving/{current_longitude},{current_latitude};{future_longitude},{future_latitude}.json"
          mapbox_params = {
            "access_token": self.mapbox_token,
            "annotations": "maxspeed,distance",
            "geometries": "geojson",
            "overview": "full",
          }

          response = requests.get(url, params=mapbox_params, timeout=10)
          response.raise_for_status()

          successful = True

          return response.json()
        except requests.ConnectionError:
          print("ConnectionError while fetching Mapbox data...")
        except requests.HTTPError:
          print("HTTPError while fetching Mapbox data...")
        except requests.RequestException:
          print("RequestException while fetching Mapbox data...")
        except requests.Timeout:
          print("Timeout while fetching Mapbox data...")
        except Exception as error:
          sentry.capture_exception(error)
          print(f"Unexpected error in Mapbox request: {error}")
        finally:
          self.calling_mapbox = False

          if not successful:
            self.mapbox_limit = 0
            self.segment_distance = v_ego

            return None

      def complete_request(future):
        try:
          data = future.result()
          if data:
            if (matchings := data.get("matchings")) and matchings[0].get("legs"):
              annotations = matchings[0]["legs"][0].get("annotation", {})
              if (speed_data := annotations.get("maxspeed")) and (speed_limit_kph := speed_data[0].get("speed")):
                self.mapbox_limit = speed_limit_kph * CV.KPH_TO_MS
                self.segment_distance = annotations.get("distance", [v_ego])[0]
                return
        except Exception as error:
          sentry.capture_exception(error)
          print(f"Mapbox Callback Error: {error}")

          self.mapbox_limit = 0
          self.segment_distance = v_ego

      future = self.executor.submit(make_request)
      future.add_done_callback(complete_request)
    except Exception as error:
      sentry.capture_exception(error)
      print(f"Unexpected error in get_mapbox_speed_limit: {error}")

      self.calling_mapbox = False

      self.mapbox_limit = 0
      self.segment_distance = v_ego

  def handle_limit_change(self, controlsState, desired_source, desired_target, frogpilotCarState):
    print(f"\n[handle_limit_change] Called with source: {desired_source}, target: {desired_target}")
    self.speed_limit_changed_timer += DT_MDL
    print(f"Timer incremented: {self.speed_limit_changed_timer:.2f}s")

    speed_limit_accepted = (frogpilotCarState.accelPressed and controlsState.state == State.enabled) or params_memory.get_bool("SpeedLimitAccepted")
    speed_limit_denied = frogpilotCarState.decelPressed or (self.speed_limit_changed_timer >= 30)

    print(f"Speed limit accepted: {speed_limit_accepted}")
    print(f"Speed limit denied: {speed_limit_denied}")

    if speed_limit_accepted:
      print("✅ Speed limit accepted — applying target")
      self.overridden_speed = 0

      self.source = desired_source
      self.target = desired_target

      params_memory.remove("SpeedLimitAccepted")
      print(f"Target set to {self.target}, source set to {self.source}")

    elif speed_limit_denied:
      print("❌ Speed limit denied — saving denied target")
      self.denied_target = desired_target

      self.previous_source = desired_source
      self.previous_target = desired_target
      print(f"Denied target set to {self.denied_target}")

    elif desired_target < self.target and not self.frogpilot_toggles.speed_limit_confirmation_lower:
      print("⬇️ Lower limit accepted without confirmation")
      self.source = desired_source
      self.target = desired_target

    elif desired_target > self.target and not self.frogpilot_toggles.speed_limit_confirmation_higher:
      print("⬆️ Higher limit accepted without confirmation")
      self.source = desired_source
      self.target = desired_target

    else:
      print("❔ Limit change needs confirmation — marking as unconfirmed")
      self.source = "None"
      self.unconfirmed_speed_limit = desired_target
      print(f"Unconfirmed target stored: {self.unconfirmed_speed_limit}")

    if self.target != self.previous_target and self.target > 0 and not speed_limit_denied:
      print("🔁 Target changed — updating previous values")
      self.denied_target = 0

      self.previous_source = self.source
      self.previous_target = self.target

      params.put_float_nonblocking("PreviousSpeedLimit", self.target)
      print(f"Previous target updated: {self.previous_target}")


  def update_limits(self, controlsState, dashboard_speed_limit, frogpilotCarState, gps_position, navigation_speed_limit, v_cruise, v_ego):
    print("\n[update_limits] Updating limits...")
    self.update_map_speed_limit(gps_position, v_ego)

    limits = {
      "Dashboard": dashboard_speed_limit,
      "Map Data": self.map_speed_limit,
      "Navigation": navigation_speed_limit
    }
    filtered_limits = {source: limit for source, limit in limits.items() if limit}
    print(f"Filtered limits: {filtered_limits}")

    if self.frogpilot_toggles.speed_limit_priority_highest:
      desired_source = max(filtered_limits, key=filtered_limits.get, default="None")
      desired_target = filtered_limits.get(desired_source, 0)
      print(f"🔝 Highest priority source selected: {desired_source} = {desired_target}")

    elif self.frogpilot_toggles.speed_limit_priority_lowest:
      desired_source = min(filtered_limits, key=filtered_limits.get, default="None")
      desired_target = filtered_limits.get(desired_source, 0)
      print(f"🔽 Lowest priority source selected: {desired_source} = {desired_target}")

    elif filtered_limits:
      print("🧩 Using custom priority list...")
      for priority in [
        self.frogpilot_toggles.speed_limit_priority1,
        self.frogpilot_toggles.speed_limit_priority2,
        self.frogpilot_toggles.speed_limit_priority3
      ]:
        if priority in filtered_limits:
          desired_source = priority
          desired_target = filtered_limits[desired_source]
          print(f"🎯 Priority match found: {desired_source} = {desired_target}")
          break
      else:
        desired_source = "None"
        desired_target = 0
        print("⚠️ No match found in priority list")

    else:
      desired_source = "None"
      desired_target = 0
      print("🚫 No valid limits found from sources")

    if desired_target == 0:
      print("🗺️ Attempting Mapbox fallback...")
      if self.mapbox_requests["total_requests"] < self.mapbox_requests["max_requests"] and self.frogpilot_toggles.slc_mapbox_filler:
        self.get_mapbox_speed_limit(gps_position, v_ego)
        if self.mapbox_limit:
          desired_source = "Mapbox"
          desired_target = self.mapbox_limit
          print(f"Mapbox limit found: {desired_target}")

      if desired_target == 0:
        print("🧭 Trying fallback options...")
        if self.denied_target != self.previous_target > 0 and self.frogpilot_toggles.slc_fallback_previous_speed_limit:
          desired_source = self.previous_source
          desired_target = self.previous_target

          self.target = desired_target
          print(f"Fallback to previous target: {desired_target}")

        elif controlsState.enabled and self.frogpilot_toggles.slc_fallback_set_speed:
          desired_source = "None"
          desired_target = v_cruise
          print(f"Fallback to cruise speed: {v_cruise}")
    else:
      self.mapbox_limit = 0
      self.segment_distance = 0
      print("✅ Valid target selected — clearing Mapbox state")

    if abs(desired_target - self.previous_target) >= 1:
      print("🔁 Target significantly changed — calling handle_limit_change")
      self.handle_limit_change(controlsState, desired_source, desired_target, frogpilotCarState)
    elif desired_source != self.source and abs(desired_target - self.target) < 1:
      print("ℹ️ Source changed — updating source")
      self.source = desired_source
    else:
      self.speed_limit_changed_timer = 0
      self.unconfirmed_speed_limit = 0

  def update_override(self, carState, controlsState, v_cruise, v_ego):
    self.override_slc = self.overridden_speed > self.target + self.offset
    self.override_slc |= carState.gasPressed and v_ego > self.target + self.offset
    self.override_slc &= controlsState.enabled

    if self.override_slc:
      if self.frogpilot_toggles.speed_limit_controller_override_manual:
        if carState.gasPressed:
          self.overridden_speed = v_ego
        self.overridden_speed = float(np.clip(self.overridden_speed, self.target + self.offset, v_cruise))
      elif self.frogpilot_toggles.speed_limit_controller_override_set_speed:
        self.overridden_speed = v_cruise
    else:
      self.overridden_speed = 0

  def update_map_speed_limit(self, gps_position, v_ego):
    if not gps_position:
      return

    self.map_speed_limit = params_memory.get_float("MapSpeedLimit")

    next_map_speed_limit = json.loads(params_memory.get("NextMapSpeedLimit") or "{}")
    self.next_speed_limit = next_map_speed_limit.get("speedlimit", 0)

    if self.next_speed_limit:
      current_latitude = gps_position.get("latitude")
      current_longitude = gps_position.get("longitude")

      next_latitude = next_map_speed_limit.get("latitude")
      next_longitude = next_map_speed_limit.get("longitude")

      distance_to_upcoming = calculate_distance_to_point(current_latitude * TO_RADIANS, current_longitude * TO_RADIANS, next_latitude * TO_RADIANS, next_longitude * TO_RADIANS)

      if self.map_speed_limit < self.next_speed_limit:
        max_lookahead = self.frogpilot_toggles.map_speed_lookahead_higher * v_ego
      elif self.map_speed_limit > self.next_speed_limit:
        max_lookahead = self.frogpilot_toggles.map_speed_lookahead_lower * v_ego
      else:
        max_lookahead = 0

      if distance_to_upcoming < max_lookahead:
        self.map_speed_limit = self.next_speed_limit