#!/usr/bin/env python3
"""Create a spreadsheet-friendly summary for FOPEN NC linked result directories."""

import argparse
import csv
import math
import os
import sys
from dataclasses import dataclass
from pathlib import Path


EARTH_RADIUS_M = 6378137.0


@dataclass
class EgoRow:
    scene_index: int
    timestamp_s: float
    pose: tuple
    lat: float
    lon: float
    alt: float
    raw: list


def parse_float(value):
    if value is None:
        return None
    value = str(value).strip()
    if not value:
        return None
    try:
        return float(value)
    except ValueError:
        return None


def parse_int(value):
    number = parse_float(value)
    if number is None:
        return None
    try:
        return int(number)
    except (TypeError, ValueError):
        return None


def read_rows(path):
    with open(path, newline="", encoding="utf-8") as f:
        sample = f.read(4096)
        f.seek(0)
        delimiter = "\t" if "\t" in sample else ","
        return list(csv.reader(f, delimiter=delimiter))


def scene_id_base(name):
    return name[:-6] if name.endswith("-index") else name


def scene_id_to_seconds(scene_id):
    base = scene_id_base(scene_id)
    if "_" not in base:
        return None
    try:
        sec, frac = base.split("_", 1)
        return float(f"{sec}.{frac}")
    except ValueError:
        return None


def scene_label(scene_id):
    base = scene_id_base(scene_id)
    if "_" not in base:
        return base
    sec, frac = base.split("_", 1)
    try:
        sec_mod = int(sec) % 10000
    except ValueError:
        return base
    tenth = (frac + "0")[0]
    return f"{sec_mod:04d}.{tenth}"


def fmt(value, digits=9):
    if value is None:
        return ""
    if isinstance(value, int):
        return str(value)
    if isinstance(value, float):
        if math.isnan(value) or math.isinf(value):
            return ""
        return f"{value:.{digits}g}"
    return str(value)


def first_header_index(header, name, occurrence=1):
    seen = 0
    for idx, value in enumerate(header):
        if value == name:
            seen += 1
            if seen == occurrence:
                return idx
    return None


def parse_elevations_histogram(path, threshold_m):
    rows = read_rows(path)
    if len(rows) < 2:
        return {}, "empty_elevation_histogram"

    agl_m = parse_float(rows[1][0] if rows[1] else None)
    hist = []
    for row in rows[1:]:
        if len(row) < 3:
            continue
        elevation = parse_float(row[1])
        fraction_lower = parse_float(row[2])
        if elevation is None or fraction_lower is None:
            continue
        hist.append((elevation, fraction_lower))

    if not hist:
        return {"agl_m": agl_m}, "no_histogram_bins"

    hist.sort(key=lambda item: item[0])
    fraction_lower_at_threshold = None
    if threshold_m <= hist[0][0]:
        fraction_lower_at_threshold = hist[0][1]
    elif threshold_m >= hist[-1][0]:
        fraction_lower_at_threshold = hist[-1][1]
    else:
        for (e0, f0), (e1, f1) in zip(hist, hist[1:]):
            if e0 <= threshold_m <= e1:
                if e1 == e0:
                    fraction_lower_at_threshold = f1
                else:
                    alpha = (threshold_m - e0) / (e1 - e0)
                    fraction_lower_at_threshold = f0 + alpha * (f1 - f0)
                break

    forestation_fraction = None
    if fraction_lower_at_threshold is not None:
        forestation_fraction = max(0.0, min(1.0, 1.0 - fraction_lower_at_threshold))

    return {
        "agl_m": agl_m,
        "hist_min_elev_m": hist[0][0],
        "hist_max_elev_m": hist[-1][0],
        "fraction_lower_at_threshold": fraction_lower_at_threshold,
        "forestation_fraction": forestation_fraction,
    }, ""


def parse_egomotion(path, scene_seconds):
    rows = read_rows(path)
    if not rows:
        return {}, "empty_egomotion"

    header = rows[0]
    idx_scene = first_header_index(header, "#")
    idx_timestamp = first_header_index(header, "timestamp")
    idx_x = first_header_index(header, "x(m)")
    idx_y = first_header_index(header, "y(m)")
    idx_z = first_header_index(header, "z(m)")
    idx_a = first_header_index(header, "a(rad)")
    idx_tilt = first_header_index(header, "tilt(rad)")
    idx_roll = first_header_index(header, "roll(rad)")
    idx_lat = first_header_index(header, "lat", 1)
    idx_lon = first_header_index(header, "long", 1)
    idx_alt = first_header_index(header, "alt", 1)
    required = [idx_scene, idx_timestamp, idx_x, idx_y, idx_z, idx_a, idx_tilt, idx_roll, idx_lat, idx_lon, idx_alt]
    if any(idx is None for idx in required):
        return {}, "missing_egomotion_columns"

    max_idx = max(required)
    ego_rows = []
    for row in rows[1:]:
        if len(row) <= max_idx:
            continue
        scene_index = parse_int(row[idx_scene])
        timestamp_s = parse_float(row[idx_timestamp])
        pose = tuple(parse_float(row[idx]) for idx in (idx_x, idx_y, idx_z, idx_a, idx_tilt, idx_roll))
        lat = parse_float(row[idx_lat])
        lon = parse_float(row[idx_lon])
        alt = parse_float(row[idx_alt])
        if scene_index is None or timestamp_s is None or any(v is None for v in pose):
            continue
        if lat is None or lon is None:
            continue
        ego_rows.append(EgoRow(scene_index, timestamp_s, pose, lat, lon, alt, row))

    if not ego_rows:
        return {}, "no_numeric_egomotion_rows"

    ref = None
    for row in ego_rows:
        if all(v == 0.0 for v in row.pose):
            ref = row
            ref_source = "zero_pose"
            break

    warnings = []
    if ref is None:
        if scene_seconds is not None:
            ref = min(ego_rows, key=lambda r: abs(r.timestamp_s - scene_seconds))
            ref_source = "nearest_timestamp"
            warnings.append("zero_pose_not_found")
        else:
            ref = ego_rows[len(ego_rows) // 2]
            ref_source = "middle_row"
            warnings.append("zero_pose_not_found")

    first = ego_rows[0]
    last = ego_rows[-1]
    segment_heading_deg = bearing_deg(first.lat, first.lon, last.lat, last.lon)
    heading_deg, heading_source, heading_from, heading_to, heading_warning = local_heading(ego_rows, ref)
    if heading_warning:
        warnings.append(heading_warning)
    image_x_axis_deg = None if heading_deg is None else (heading_deg + 180.0) % 360.0

    return {
        "ego_row_count": len(ego_rows),
        "first": first,
        "last": last,
        "ref": ref,
        "ref_source": ref_source,
        "segment_heading_deg": segment_heading_deg,
        "heading_source": heading_source,
        "heading_from": heading_from,
        "heading_to": heading_to,
        "flight_heading_deg": heading_deg,
        "image_x_axis_deg": image_x_axis_deg,
    }, ";".join(warnings)


def bearing_deg(lat1, lon1, lat2, lon2):
    if None in (lat1, lon1, lat2, lon2):
        return None
    if lat1 == lat2 and lon1 == lon2:
        return None
    phi1 = math.radians(lat1)
    phi2 = math.radians(lat2)
    dlam = math.radians(lon2 - lon1)
    x = math.sin(dlam) * math.cos(phi2)
    y = math.cos(phi1) * math.sin(phi2) - math.sin(phi1) * math.cos(phi2) * math.cos(dlam)
    return (math.degrees(math.atan2(x, y)) + 360.0) % 360.0


def local_heading(ego_rows, ref):
    by_scene_index = {row.scene_index: row for row in ego_rows}
    before = by_scene_index.get(ref.scene_index - 1)
    after = by_scene_index.get(ref.scene_index + 1)
    source = "ref_pm1_scene_index"
    warning = ""

    if before is None or after is None:
        try:
            pos = next(i for i, row in enumerate(ego_rows) if row is ref)
        except StopIteration:
            pos = -1
        before = ego_rows[pos - 1] if pos > 0 else None
        after = ego_rows[pos + 1] if 0 <= pos < len(ego_rows) - 1 else None
        source = "adjacent_rows"
        warning = "heading_used_adjacent_rows"

    if before is not None and after is not None:
        return bearing_deg(before.lat, before.lon, after.lat, after.lon), source, before, after, warning

    if after is not None:
        return bearing_deg(ref.lat, ref.lon, after.lat, after.lon), "ref_to_next_row", ref, after, "heading_used_one_neighbor"

    if before is not None:
        return bearing_deg(before.lat, before.lon, ref.lat, ref.lon), "prev_row_to_ref", before, ref, "heading_used_one_neighbor"

    if len(ego_rows) >= 2:
        return bearing_deg(ego_rows[0].lat, ego_rows[0].lon, ego_rows[-1].lat, ego_rows[-1].lon), "first_to_last_fallback", ego_rows[0], ego_rows[-1], "heading_used_first_last"

    return None, "unavailable", None, None, "heading_unavailable"


def footprint_m(agl_m, hfov_deg):
    if agl_m is None:
        return None, None
    vfov_deg = (512.0 / 640.0) * hfov_deg
    width_m = 2.0 * agl_m * math.tan(math.radians(hfov_deg) / 2.0)
    height_m = 2.0 * agl_m * math.tan(math.radians(vfov_deg) / 2.0)
    return width_m, height_m


def rectangle_corners(lat, lon, width_m, height_m, x_axis_heading_deg):
    if None in (lat, lon, width_m, height_m, x_axis_heading_deg):
        return [None] * 8

    lat_rad = math.radians(lat)
    cos_lat = math.cos(lat_rad)
    if abs(cos_lat) < 1.0e-12:
        return [None] * 8

    def axis_unit(heading_deg):
        heading = math.radians(heading_deg)
        return math.sin(heading), math.cos(heading)  # east, north

    xe, xn = axis_unit(x_axis_heading_deg)
    ye, yn = axis_unit((x_axis_heading_deg + 90.0) % 360.0)
    corners = []
    for sx, sy in ((1, 1), (1, -1), (-1, -1), (-1, 1)):
        east = sx * 0.5 * width_m * xe + sy * 0.5 * height_m * ye
        north = sx * 0.5 * width_m * xn + sy * 0.5 * height_m * yn
        clat = lat + math.degrees(north / EARTH_RADIUS_M)
        clon = lon + math.degrees(east / (EARTH_RADIUS_M * cos_lat))
        corners.extend([clat, clon])
    return corners


def parse_ground_planes(path):
    if not path.exists():
        return {}, "missing_ground_planes"
    rows = read_rows(path)
    if not rows:
        return {}, "empty_ground_planes"
    header = rows[0]
    by_scene = {}
    for row in rows[1:]:
        if not row:
            continue
        key = row[0].strip()
        if not key:
            continue
        values = {}
        for idx, name in enumerate(header):
            if idx >= len(row):
                continue
            values[name] = row[idx]
        by_scene[key] = values
    return by_scene, ""


def choose_version_dir(scene_dir, version):
    preferred = scene_dir / version
    if preferred.is_dir():
        return preferred, version, ""
    versions = sorted(p.name for p in scene_dir.iterdir() if p.is_dir() and p.name.startswith("v"))
    if not versions:
        return preferred, version, "missing_version_dir"
    return scene_dir / versions[-1], versions[-1], f"used_fallback_version:{versions[-1]}"


def build_groups(linked_root):
    entries = []
    for entry in os.scandir(linked_root):
        if entry.name.startswith("."):
            continue
        if not entry.is_dir(follow_symlinks=True):
            continue
        entries.append(entry.name)
    entries.sort()

    groups = []
    pending = []
    for name in entries:
        if name.endswith("-index"):
            groups.append((len(groups) + 1, name, pending))
            pending = []
        else:
            pending.append(name)
    if pending:
        groups.append((len(groups) + 1, "", pending))
    return groups


def make_row(linked_root, version, group_index, index_id, group_scenes, scene_id, args, ground_rows, group_warning):
    warnings = [w for w in [group_warning] if w]
    scene_dir = linked_root / scene_id
    scene_seconds = scene_id_to_seconds(scene_id)
    version_dir, version_used, version_warning = choose_version_dir(scene_dir, version)
    if version_warning:
        warnings.append(version_warning)

    hist_path = version_dir / f"{scene_id}-elevations_histogram.csv"
    ego_path = version_dir / f"{scene_id}-egomotion.csv"
    has_elevations_histogram = hist_path.exists()
    has_egomotion = ego_path.exists()

    hist, hist_warning = ({}, "missing_elevations_histogram")
    if has_elevations_histogram:
        hist, hist_warning = parse_elevations_histogram(hist_path, args.forest_threshold_m)
    if hist_warning:
        warnings.append(hist_warning)

    ego, ego_warning = ({}, "missing_egomotion")
    if has_egomotion:
        ego, ego_warning = parse_egomotion(ego_path, scene_seconds)
    if ego_warning:
        warnings.append(ego_warning)

    agl_m = hist.get("agl_m")
    width_m, height_m = footprint_m(agl_m, args.hfov_deg)

    ref = ego.get("ref")
    first = ego.get("first")
    last = ego.get("last")
    heading_from = ego.get("heading_from")
    heading_to = ego.get("heading_to")
    center_lat = ref.lat if ref else None
    center_lon = ref.lon if ref else None
    center_alt = ref.alt if ref else None
    x_axis_heading = ego.get("image_x_axis_deg")
    corners = rectangle_corners(center_lat, center_lon, width_m, height_m, x_axis_heading)

    gp = ground_rows.get(scene_id, {})
    has_ground_plane_row = bool(gp)
    if index_id and not has_ground_plane_row:
        warnings.append("missing_ground_plane_row")
    group_start = group_scenes[0] if group_scenes else ""
    group_end = group_scenes[-1] if group_scenes else ""

    return {
        "status": "missing" if warnings else "ok",
        "group_index": group_index,
        "index_id": index_id,
        "index_timestamp_s": scene_id_to_seconds(index_id) if index_id else None,
        "group_size": len(group_scenes),
        "group_start_id": group_start,
        "group_end_id": group_end,
        "scene_id": scene_id,
        "scene_label": scene_label(scene_id),
        "scene_timestamp_s": scene_seconds,
        "version": version_used,
        "has_version_dir": version_dir.is_dir(),
        "has_elevations_histogram": has_elevations_histogram,
        "has_egomotion": has_egomotion,
        "has_ground_plane_row": has_ground_plane_row,
        "agl_m": agl_m,
        "hfov_deg": args.hfov_deg,
        "vfov_deg": (512.0 / 640.0) * args.hfov_deg,
        "footprint_x_m": width_m,
        "footprint_y_m": height_m,
        "forest_threshold_m": args.forest_threshold_m,
        "fraction_lower_at_threshold": hist.get("fraction_lower_at_threshold"),
        "forestation_fraction": hist.get("forestation_fraction"),
        "forestation_percent": None if hist.get("forestation_fraction") is None else 100.0 * hist["forestation_fraction"],
        "hist_min_elev_m": hist.get("hist_min_elev_m"),
        "hist_max_elev_m": hist.get("hist_max_elev_m"),
        "ego_row_count": ego.get("ego_row_count"),
        "ref_source": ego.get("ref_source"),
        "ref_scene_index": ref.scene_index if ref else None,
        "ref_timestamp_s": ref.timestamp_s if ref else None,
        "center_lat": center_lat,
        "center_long": center_lon,
        "center_alt_m": center_alt,
        "first_scene_index": first.scene_index if first else None,
        "first_timestamp_s": first.timestamp_s if first else None,
        "first_lat": first.lat if first else None,
        "first_long": first.lon if first else None,
        "last_scene_index": last.scene_index if last else None,
        "last_timestamp_s": last.timestamp_s if last else None,
        "last_lat": last.lat if last else None,
        "last_long": last.lon if last else None,
        "segment_heading_deg": ego.get("segment_heading_deg"),
        "heading_source": ego.get("heading_source"),
        "heading_from_scene_index": heading_from.scene_index if heading_from else None,
        "heading_from_timestamp_s": heading_from.timestamp_s if heading_from else None,
        "heading_from_lat": heading_from.lat if heading_from else None,
        "heading_from_long": heading_from.lon if heading_from else None,
        "heading_to_scene_index": heading_to.scene_index if heading_to else None,
        "heading_to_timestamp_s": heading_to.timestamp_s if heading_to else None,
        "heading_to_lat": heading_to.lat if heading_to else None,
        "heading_to_long": heading_to.lon if heading_to else None,
        "flight_heading_deg": ego.get("flight_heading_deg"),
        "rect_x_axis_heading_deg": x_axis_heading,
        "corner1_lat": corners[0],
        "corner1_long": corners[1],
        "corner2_lat": corners[2],
        "corner2_long": corners[3],
        "corner3_lat": corners[4],
        "corner3_long": corners[5],
        "corner4_lat": corners[6],
        "corner4_long": corners[7],
        "gp_good": parse_float(gp.get("Good")),
        "gp_x_m": parse_float(gp.get("X")),
        "gp_y_m": parse_float(gp.get("Y")),
        "gp_z_m": parse_float(gp.get("Z")),
        "gp_lat": parse_float(gp.get("Latitude")),
        "gp_long": parse_float(gp.get("Longitude")),
        "gp_alt_m": parse_float(gp.get("Altitude")),
        "gp_disp_center": parse_float(gp.get("disp_center")),
        "linked_path": str(scene_dir),
        "warnings": ";".join(warnings),
    }


def write_summary(rows, out_path):
    headers = [
        "status",
        "group_index",
        "index_id",
        "index_timestamp_s",
        "group_size",
        "group_start_id",
        "group_end_id",
        "scene_id",
        "scene_label",
        "scene_timestamp_s",
        "version",
        "has_version_dir",
        "has_elevations_histogram",
        "has_egomotion",
        "has_ground_plane_row",
        "agl_m",
        "hfov_deg",
        "vfov_deg",
        "footprint_x_m",
        "footprint_y_m",
        "forest_threshold_m",
        "fraction_lower_at_threshold",
        "forestation_fraction",
        "forestation_percent",
        "hist_min_elev_m",
        "hist_max_elev_m",
        "ego_row_count",
        "ref_source",
        "ref_scene_index",
        "ref_timestamp_s",
        "center_lat",
        "center_long",
        "center_alt_m",
        "first_scene_index",
        "first_timestamp_s",
        "first_lat",
        "first_long",
        "last_scene_index",
        "last_timestamp_s",
        "last_lat",
        "last_long",
        "segment_heading_deg",
        "heading_source",
        "heading_from_scene_index",
        "heading_from_timestamp_s",
        "heading_from_lat",
        "heading_from_long",
        "heading_to_scene_index",
        "heading_to_timestamp_s",
        "heading_to_lat",
        "heading_to_long",
        "flight_heading_deg",
        "rect_x_axis_heading_deg",
        "corner1_lat",
        "corner1_long",
        "corner2_lat",
        "corner2_long",
        "corner3_lat",
        "corner3_long",
        "corner4_lat",
        "corner4_long",
        "gp_good",
        "gp_x_m",
        "gp_y_m",
        "gp_z_m",
        "gp_lat",
        "gp_long",
        "gp_alt_m",
        "gp_disp_center",
        "linked_path",
        "warnings",
    ]
    out_path.parent.mkdir(parents=True, exist_ok=True)
    with open(out_path, "w", newline="", encoding="utf-8") as f:
        writer = csv.writer(f, delimiter="\t", lineterminator="\n")
        writer.writerow(headers)
        for row in rows:
            writer.writerow([fmt(row.get(header)) for header in headers])


def main():
    parser = argparse.ArgumentParser(
        description="Summarize FOPEN NC linked result directories into a TSV for LibreOffice Calc."
    )
    parser.add_argument(
        "linked_root",
        type=Path,
        help="Linked view root containing timestamp and *-index directories.",
    )
    parser.add_argument("-o", "--out", type=Path, required=True, help="Output TSV path.")
    parser.add_argument("--version", default="v88", help="Preferred result version directory (default: v88).")
    parser.add_argument(
        "--forest-threshold-m",
        type=float,
        default=1.0,
        help="Elevation threshold above ground used for forestation fraction (default: 1.0).",
    )
    parser.add_argument("--hfov-deg", type=float, default=32.0, help="Camera horizontal FoV in degrees.")
    args = parser.parse_args()

    linked_root = args.linked_root.resolve()
    if not linked_root.is_dir():
        print(f"ERROR: linked root is not a directory: {linked_root}", file=sys.stderr)
        return 1

    rows = []
    groups = build_groups(linked_root)
    for group_index, index_id, group_scenes in groups:
        ground_rows = {}
        group_warning = ""
        if index_id:
            index_base = scene_id_base(index_id)
            index_dir = linked_root / index_id
            index_version_dir, _, index_version_warning = choose_version_dir(index_dir, args.version)
            ground_path = index_version_dir / f"{index_base}-ground_planes.csv"
            ground_rows, ground_warning = parse_ground_planes(ground_path)
            group_warning = ";".join(w for w in [index_version_warning, ground_warning] if w)
        else:
            group_warning = "missing_following_index"
        for scene_id in group_scenes:
            rows.append(
                make_row(
                    linked_root,
                    args.version,
                    group_index,
                    index_id,
                    group_scenes,
                    scene_id,
                    args,
                    ground_rows,
                    group_warning,
                )
            )

    write_summary(rows, args.out.resolve())
    warning_rows = sum(1 for row in rows if row.get("warnings"))
    print(f"wrote {len(rows)} rows to {args.out.resolve()}")
    print(f"groups: {len(groups)}; rows with warnings: {warning_rows}")
    return 0


if __name__ == "__main__":
    raise SystemExit(main())
