import csv
import math
from pathlib import Path

# -----------------------
# CONFIG
# -----------------------
CSV_PATH = "conveyors.csv"
OUT_TSCN = "generated_conveyors.tscn"

SCALE = 0.0254
FIXED_Y = 2.5

STRAIGHT_BELT_ID = "3_38ygf"
CURVED_BELT_ID   = "1_ef28r"

SPUR_FORWARD_OFFSET = 0.75  # meters

# -----------------------
# HELPERS
# -----------------------
def transform_from_points(x1, y1, x2, y2):
    dx = x2 - x1
    dy = y2 - y1

    # length in meters
    length = math.hypot(dx, dy) * SCALE

    # POSITION
    mid_x = (x1 + x2) / 2 * SCALE
    mid_z = -(y1 + y2) / 2 * SCALE   # ✅ single Y→-Z mapping

    # ROTATION (IMPORTANT)
    # dy must be negated because Z is flipped
    rot_y = math.atan2(-dy, dx)

    return {
        "length": length,
        "pos": (mid_x, FIXED_Y, mid_z),
        "rot_y": rot_y,
    }


def transform3d(rot_y, x, y, z):
    c = math.cos(rot_y)
    s = math.sin(rot_y)
    return (
        f"Transform3D({c}, 0, {-s}, "
        f"0, 1, 0, "
        f"{s}, 0, {c}, "
        f"{x}, {y}, {z})"
    )

def parse_key(key):
    p = key.split("_")
    return p[0], int(p[1])

def end_point(conv):
    dx = math.cos(conv["rot_y"])
    dz = math.sin(conv["rot_y"])
    half = conv["length"] / 2
    return (
        conv["pos"][0] + dx * half,
        conv["pos"][2] + dz * half
    )

# -----------------------
# READ CSV
# -----------------------
straight = {}
spurs = []

with open(CSV_PATH, newline="") as f:
    reader = csv.DictReader(f)
    for row in reader:
        key = row["conveyor_key"].strip()
        included = row["included"].strip()

        prefix, sec = parse_key(key)

        if included == "1":
            if not all(row[c].strip() for c in ("start_x", "start_y", "end_x", "end_y")):
                continue

            conv = transform_from_points(
                float(row["start_x"]),
                float(row["start_y"]),
                float(row["end_x"]),
                float(row["end_y"])
            )

            conv["name"] = key
            conv["prefix"] = prefix
            conv["sec"] = sec
            straight[key] = conv

        else:
            spurs.append({
                "name": key,
                "prefix": prefix,
                "sec": sec
            })

# -----------------------
# WRITE TSCN
# -----------------------
lines = []
lines.append('[gd_scene load_steps=3 format=3]')
lines.append('')
lines.append('[ext_resource type="PackedScene" path="res://parts/assemblies/BeltConveyorAssembly.tscn" id="3_38ygf"]')
lines.append('[ext_resource type="PackedScene" path="res://parts/assemblies/CurvedBeltConveyorAssembly.tscn" id="1_ef28r"]')
lines.append('')
lines.append('[node name="GeneratedConveyors" type="Node3D"]')
lines.append('')


# -----------------------
# Straight conveyors
# -----------------------
for c in straight.values():
    x, y, z = c["pos"]
    t = transform3d(c["rot_y"], x, y, z)

    lines.append(f'[node name="{c["name"]}" parent="." instance=ExtResource("{STRAIGHT_BELT_ID}")]')
    lines.append(f"transform = {t}")
    lines.append("right_side_guards_enabled = false")
    lines.append("left_side_guards_enabled = false")
    lines.append("head_end_leg_enabled = false")
    lines.append("tail_end_leg_enabled = false")
    lines.append("enable_comms = true")
    lines.append(f'speed_tag_name = "{c["name"]}_OIP"')
    lines.append(f"size = Vector3({c['length']:.6f}, 0.5, 1.524)")
    lines.append("")

# -----------------------
# Spur conveyors
# -----------------------
for spur in spurs:
    prefix = spur["prefix"]
    sec = spur["sec"]

    prev_key = f"{prefix}_{sec-1}"
    next_key = f"{prefix}_{sec+1}"

    if prev_key not in straight or next_key not in straight:
        continue

    prev = straight[prev_key]
    nxt  = straight[next_key]

    px, pz = end_point(prev)

    fwd_x = math.cos(prev["rot_y"])
    fwd_z = math.sin(prev["rot_y"])

    mx = px + fwd_x * SPUR_FORWARD_OFFSET
    mz = pz + fwd_z * SPUR_FORWARD_OFFSET

    delta = nxt["rot_y"] - prev["rot_y"]
    spur_angle_deg = abs(delta) * 180.0 / math.pi
    if spur_angle_deg < 1.0:
        spur_angle_deg = 30.0

    t = transform3d(prev["rot_y"], mx, FIXED_Y, mz)

    lines.append(f'[node name="{spur["name"]}" parent="." instance=ExtResource("{CURVED_BELT_ID}")]')
    lines.append(f"transform = {t}")
    lines.append(f"conveyor_angle = {spur_angle_deg:.3f}")
    lines.append("enable_comms = true")
    lines.append(f'speed_tag_name = "{spur["name"]}_OIP"')
    lines.append("")

Path(OUT_TSCN).write_text("\n".join(lines), encoding="utf-8")
print(f"Generated {len(straight)} straight conveyors and {len(spurs)} spur candidates.")