# Make a 3D path in STL
# Markus Opitz 2026


import numpy as np
import pandas as pd
from stl import mesh

# ==========
# parameter
# ==========
WIDTH = 56      # width (mm)
THICKNESS = 30  #thickness (mm)
SCALE = 0.0001  #scale (meter --> mm)

# ==========
# load CSV
# ==========
df = pd.read_csv("coords.csv")

lon = df["lon"].values
lat = df["lat"].values
alt = df["alt"].values

x = df["lon"].to_numpy(dtype=float).copy()
y = df["lat"].to_numpy(dtype=float).copy()
z = df["alt"].to_numpy(dtype=float).copy()

# scale
x = x * SCALE
y = y * SCALE
z = z * SCALE

# ==========
# projection
# ==========
x = lon * 111320
y = lat * 110540
z = alt

points = np.column_stack((x, y, z))

# ===============
# help function: calculate norm
# ===============

def get_normal(p1, p2):
    direction = np.array(p2 - p1, dtype=float).copy()
    direction[2] = 0

    norm = np.array([-direction[1], direction[0], 0], dtype=float)
    norm_len = np.linalg.norm(norm)

    if norm_len == 0:
        return np.array([0.0, 0.0, 0.0])

    return norm / norm_len

# ===============
# create anchor points
# ===============
left_points = []
right_points = []

for i in range(len(points)):
    if i == 0:
        normal = get_normal(points[i], points[i+1])
    elif i == len(points) -1:
        normal = get_normal(points[i-1], points[i])
    else:
        n1 = get_normal(points[i-1], points[i])
        n2 = get_normal(points[i], points[i+1])
        normal = (n1 + n2) / 2

    norm_len = np.linalg.norm(normal)
    if norm_len != 0:
        normal = normal / norm_len

    left = points[i] + normal * (WIDTH /2)
    right = points[i] - normal * (WIDTH /2)

    left_points.append(left)
    right_points.append(right)

left_points = np.array(left_points)
right_points = np.array(right_points)

# ===============
# make mesh
# ===============
faces = []

def quad_to_tri(p1, p2, p3, p4):
    return [
        [p1, p2, p3],
        [p1, p3, p4]
    ]

# top side
for i in range(len(points) -1):
    l1, l2 = left_points[i], left_points[i+1]
    r1, r2 = right_points[i], right_points[i+1]

    faces += quad_to_tri(l1, l2, r2, r1)

# bottom face (extruded downwards)
left_bottom = left_points.copy()
right_bottom = right_points.copy()

left_bottom[:,2] -=THICKNESS
right_bottom[:,2] -=THICKNESS

for i in range(len(points) - 1):
    l1, l2 = left_bottom[i], left_bottom[i+1]
    r1, r2 = right_bottom[i], right_bottom[i+1]

    faces += quad_to_tri(r1, r2, l2, l1)   # flipped!

# side walls
for i in range(len(points) -1):
    # left
    faces += quad_to_tri(left_points[i], left_points[i+1],
                         left_bottom[i+1], left_bottom[i])
    # right
    faces += quad_to_tri(right_points[i], right_points[i+1],
                         right_bottom[i+1], right_bottom[i])

# close back and rear side
faces += quad_to_tri(left_points[0], right_points[0],
                     right_bottom[0], left_bottom[0])

faces += quad_to_tri(right_points[-1], left_points[-1],
                     left_bottom[-1], right_bottom[-1])

# ===============
# save STL
# ===============
terrain_mesh = mesh.Mesh(np.zeros(len(faces), dtype=mesh.Mesh.dtype))

for i, f in enumerate(faces):
    terrain_mesh.vectors[i] = np.array(f)

terrain_mesh.save("path_band.stl")

print("STL done: path_band.stl")