"""A small VRU basic service for the phone emulator: VAM assembly with asn1tools from the
ETSI ASN.1 modules, the individual-VAM generation rules of ETSI TS 103 300-3 V2.3.1
clause 6.4 (items 1 to 4, Tables 16 and 17), the VBS PCI of Table 4, and a PoTi stand-in.

It is a test stand-in for the phone app's VBS, not a conformant VBS: no clustering,
no redundancy mitigation, no reception management, no motion prediction.
"""
from __future__ import annotations

import math
import time
from dataclasses import dataclass, field
from pathlib import Path
from typing import Optional

from . import messages as m

ITS_EPOCH_UNIX = 1072915200
LEAP_SECONDS_SINCE_2004 = 5  # TAI-UTC 37 s now, 32 s at the epoch (2017-01-01 was the last insertion)

# ETSI TS 103 300-3 V2.3.1 Table 16 / Table 17 recommended values
T_GEN_VAM_MIN_MS = 100
T_GEN_VAM_MAX_MS = 5000
T_GEN_VAM_LF_MIN_MS = 2000
MIN_POSITION_CHANGE_M = 4.0
MIN_SPEED_CHANGE_MPS = 0.5
MIN_ORIENTATION_CHANGE_DEG = 4.0

# TS 102 965 ITS-AID of the VRU service, TS 103 248 BTP port of VAM
ITS_AID_VRU = 638
BTP_PORT_VAM = 2018


def its_timestamp_ms(unix_seconds: Optional[float] = None) -> int:
    """TS 102 894-2 TimestampIts: TAI milliseconds since 2004-01-01T00:00:00Z."""
    if unix_seconds is None:
        unix_seconds = time.time()
    return int((unix_seconds - ITS_EPOCH_UNIX + LEAP_SECONDS_SINCE_2004) * 1000)


def default_asn1_directory() -> Path:
    here = Path(__file__).resolve()
    return here.parents[3] / 'external' / 'vanetza-idf' / 'asn1' / 'release2'


class Codec:
    """UPER codec for VAM (and CAM/DENM for the displays) from the submodule's ASN.1 modules."""

    def __init__(self, directory: Optional[Path] = None):
        import asn1tools
        directory = directory or default_asn1_directory()
        cdd = directory / 'TS102894-2v241-CDD.asn'
        self.vam = asn1tools.compile_files([cdd, directory / 'TS103300-3v231' / 'VAM-PDU-Descriptions.asn',
                                            directory / 'TS103300-3v231' / 'motorcyclist-special-container.asn'], 'uper')
        self.vam_type = self.vam.modules['VAM-PDU-Descriptions']['VAM']
        self.cam_type = None
        self.denm_type = None
        try:
            cam = asn1tools.compile_files([cdd, directory / 'TS103900v231-CAM.asn'], 'uper')
            self.cam_type = cam.modules['CAM-PDU-Descriptions']['CAM']
        except Exception:
            pass
        try:
            denm = asn1tools.compile_files([cdd, directory / 'TS103831v231-DENM.asn'], 'uper')
            self.denm_type = denm.modules['DENM-PDU-Description']['DENM']
        except Exception:
            pass

    def encode_vam(self, vam: dict) -> bytes:
        return self.vam_type.encode(vam)

    def decode_vam(self, octets: bytes) -> dict:
        return self.vam_type.decode(octets)

    def decode_by_port(self, port: int, octets: bytes):
        if port == BTP_PORT_VAM:
            return 'VAM', self.decode_vam(octets)
        if port == 2001 and self.cam_type:
            return 'CAM', self.cam_type.decode(octets)
        if port == 2002 and self.denm_type:
            return 'DENM', self.denm_type.decode(octets)
        return None, None


@dataclass
class PotiState:
    """What PoTi reports: WGS84 position with a 95 % confidence ellipse, kinematics, time."""
    timestamp_ms: int
    latitude: float
    longitude: float
    speed_mps: float = 0.0
    heading_deg: float = 0.0
    altitude_m: Optional[float] = None
    semi_major_m: float = 1.5
    semi_minor_m: float = 1.0
    orientation_deg: float = 0.0

    def poti_update(self) -> m.PotiUpdate:
        return m.PotiUpdate(
            timestamp_ms=self.timestamp_ms,
            latitude=int(round(self.latitude * 1e7)), longitude=int(round(self.longitude * 1e7)),
            semi_major_cm=min(65535, int(round(self.semi_major_m * 100))),
            semi_minor_cm=min(65535, int(round(self.semi_minor_m * 100))),
            orientation_deci_degree=int(round(self.orientation_deg * 10)) % 3600,
            altitude_cm=None if self.altitude_m is None else int(round(self.altitude_m * 100)),
            speed_cm_s=min(65535, int(round(self.speed_mps * 100))),
            heading_deci_degree=int(round(self.heading_deg * 10)) % 3600,
            pai=self.semi_major_m * 2 <= 40.0)  # itsGnPaiInterval / 2, EN 302 636-4-1 / TS 103 836-4-1 clause 8.4


class PotiSimulator:
    """Static position, or a bicycle riding a circle (radius r at speed v) for the triggers."""

    def __init__(self, latitude: float, longitude: float, speed_mps: float = 0.0, radius_m: float = 30.0,
                 altitude_m: Optional[float] = 12.0):
        self.center = (latitude, longitude)
        self.speed = speed_mps
        self.radius = radius_m
        self.altitude = altitude_m
        self.start = time.time()

    def state(self, now: Optional[float] = None) -> PotiState:
        now = time.time() if now is None else now
        if self.speed <= 0:
            return PotiState(its_timestamp_ms(now), self.center[0], self.center[1], 0.0, 0.0, self.altitude)
        angle = (now - self.start) * self.speed / self.radius  # rad, counter-clockwise
        lat = self.center[0] + math.degrees(self.radius * math.sin(angle) / 6371000.0)
        lon = self.center[1] + math.degrees(self.radius * math.cos(angle) / (6371000.0 * math.cos(math.radians(self.center[0]))))
        heading = (math.degrees(angle) * -1 + 0.0) % 360  # tangent of a counter-clockwise circle, clockwise from north
        return PotiState(its_timestamp_ms(now), lat, lon, self.speed, heading, self.altitude)


def haversine_m(a: PotiState, b: PotiState) -> float:
    r = 6371000.0
    p1, p2 = math.radians(a.latitude), math.radians(b.latitude)
    dp = p2 - p1
    dl = math.radians(b.longitude - a.longitude)
    h = math.sin(dp / 2) ** 2 + math.cos(p1) * math.cos(p2) * math.sin(dl / 2) ** 2
    return 2 * r * math.asin(math.sqrt(h))


@dataclass
class VbsLite:
    """VRU-ACTIVE-STANDALONE VBS state for one bicyclist (VRU profile 2, sub-profile bicyclist)."""
    codec: Codec
    station_id: int
    t_gen_vam_ms: int = T_GEN_VAM_MIN_MS      # T_GenVam, from the management entity (DCC) in a real VBS
    fixed_period_ms: Optional[int] = None    # test override: one VAM every period regardless of the triggers
    active: bool = True                      # false between PREPARE and COMMIT of an identifier change
    last_vam: Optional[PotiState] = None
    last_vam_at_ms: int = 0
    last_lf_at_ms: int = -10 ** 9
    generated: int = 0
    security_profile: int = 2                # Table 4: SECURED or UNSECURED
    ssp: bytes = b'\x01'                     # SSP of the VRU ITS-AID carried in the authorization ticket
    traffic_class: int = 0xFF                # "same GN traffic class value as for the CAM": the station default
    lifetime: int = 0x05                     # GN maximum packet lifetime 1 s (Table 4: shall not exceed 1 000 ms)

    def due(self, now: PotiState) -> bool:
        if not self.active:
            return False
        elapsed = now.timestamp_ms - self.last_vam_at_ms
        if self.fixed_period_ms is not None:
            return self.last_vam is None or elapsed >= self.fixed_period_ms
        if self.last_vam is None:
            return True
        if elapsed < max(self.t_gen_vam_ms, T_GEN_VAM_MIN_MS):
            return False
        if elapsed > T_GEN_VAM_MAX_MS:
            return True  # item 1
        if haversine_m(now, self.last_vam) > MIN_POSITION_CHANGE_M:
            return True  # item 2
        if abs(now.speed_mps - self.last_vam.speed_mps) > MIN_SPEED_CHANGE_MPS:
            return True  # item 3
        delta = abs((now.heading_deg - self.last_vam.heading_deg + 180) % 360 - 180)
        return delta > MIN_ORIENTATION_CHANGE_DEG  # item 4

    def assemble(self, now: PotiState) -> bytes:
        """VAM per TS 103 300-3 clause 7.3: basic + HF container, LF container every T_GenVamLFMin."""
        include_lf = now.timestamp_ms - self.last_lf_at_ms >= T_GEN_VAM_LF_MIN_MS
        vam = {
            'header': {'protocolVersion': 3, 'messageId': 16, 'stationId': self.station_id},
            'vam': {
                'generationDeltaTime': now.timestamp_ms % 65536,
                'vamParameters': {
                    'basicContainer': {
                        'stationType': 2,  # cyclist
                        'referencePosition': {
                            'latitude': int(round(now.latitude * 1e7)),
                            'longitude': int(round(now.longitude * 1e7)),
                            'positionConfidenceEllipse': {
                                'semiMajorAxisLength': min(4094, int(round(now.semi_major_m * 100))),
                                'semiMinorAxisLength': min(4094, int(round(now.semi_minor_m * 100))),
                                'semiMajorAxisOrientation': int(round(now.orientation_deg * 10)) % 3600,
                            },
                            'altitude': ({'altitudeValue': max(-100000, min(800000, int(round(now.altitude_m * 100)))),
                                          'altitudeConfidence': 'alt-020-00'} if now.altitude_m is not None
                                         else {'altitudeValue': 800001, 'altitudeConfidence': 'unavailable'}),
                        },
                    },
                    'vruHighFrequencyContainer': {
                        'heading': {'value': int(round(now.heading_deg * 10)) % 3600, 'confidence': 50},
                        'speed': {'speedValue': min(16382, int(round(now.speed_mps * 100))), 'speedConfidence': 50},
                        'longitudinalAcceleration': {'longitudinalAccelerationValue': 0, 'longitudinalAccelerationConfidence': 102},
                    },
                },
            },
        }
        if include_lf:
            vam['vam']['vamParameters']['vruLowFrequencyContainer'] = {
                'profileAndSubprofile': ('bicyclistAndLightVruVehicle', 1),  # bicyclist
                'sizeClass': 1,  # low
            }
            self.last_lf_at_ms = now.timestamp_ms
        octets = self.codec.encode_vam(vam)
        self.last_vam = now
        self.last_vam_at_ms = now.timestamp_ms
        self.generated += 1
        return octets

    def btp_data_request(self, vam: bytes) -> m.BtpDataRequest:
        """The VBS networking PCI of TS 103 300-3 Table 4 for one VAM."""
        return m.BtpDataRequest(
            fl_sdu=vam, btp_type=1, destination_port=BTP_PORT_VAM, destination_port_info=0,
            gn_packet_transport_type=m.TransportType.SHB, gn_communication_profile=1,
            gn_security_profile=self.security_profile, gn_traffic_class=self.traffic_class,
            gn_maximum_packet_lifetime=self.lifetime, its_aid=ITS_AID_VRU, permissions=self.ssp)

    # TS 103 300-3 clause 5.3.5: stop on PREPARE, resume with a new StationId after COMMIT
    def prepare(self):
        self.active = False

    def commit(self, new_station_id: int):
        self.station_id = new_station_id
        self.active = True
        self.last_vam = None

    def abort(self):
        self.active = True