import cv2
import math
import csv
import os

def generate_image(folder, filename, display):
    #read servo value from CSV
    csv_path = os.path.join(folder, "data.csv")
    servo = None
    
    with open(csv_path, 'r') as f:
        reader = csv.reader(f)
        for row in reader:
            if row[0] == filename:
                servo = int(row[1])
                break
    
    if servo is None:
        raise ValueError(f"Filename '{filename}' not found in {csv_path}")
    
    #load and process image
    img_path = os.path.join(folder, filename)
    img = cv2.imread(img_path)

    if img is None:
        raise FileNotFoundError(f"Image not found: {img_path}")

    h, w = img.shape[:2]
    arrow_length = h/2

    angle = (servo - 1000) * math.pi / 1000.0 #map to degrees

    startpoint = (int(w/2), int(h))
    endpoint = (
        int(w/2 + arrow_length*math.cos(angle)),
        int(h - arrow_length*math.sin(angle))
    )
    cv2.arrowedLine(img,startpoint,endpoint,(0, 0, 255), 5)

    if(display):
        cv2.imshow(filename, img)
        cv2.waitKey(0)
        cv2.destroyAllWindows()
    
    os.makedirs("arrowed", exist_ok=True)
    cv2.imwrite(os.path.join("arrowed", filename), img)

generate_image("run_2026-03-19_17-18-55", "330.jpg", True)