import csv
import os
import re
import matplotlib.pyplot as plt
import numpy as np
import glob

# files = [
#     "stanley_*_*_*_*.csv",
# ]
files = [
    "lqr_*_*_*_*_*.csv",
]

data = {}
for pattern in files:

    matched_files = glob.glob(pattern)

    if not matched_files:
        raise FileNotFoundError(
            f"找不到文件: {pattern}"
        )
    matched_files.sort(
        key=lambda x: (
            float(x.split("_")[1]),   # 速度
            float(x.split("_")[2]),   # 参数1
            float(x.split("_")[3]),   # 参数2
            float(x.split("_")[4]),   # 参数2
            float(x.split("_")[5].replace(".csv", ""))  # 半径
        )
    )

    print(f"匹配到 {len(matched_files)} 个文件:")
    for file in matched_files:
        if not os.path.exists(file):
            raise FileNotFoundError(f"找不到文件: {file}")

        distance = []
        lateral_error = []
        heading_error = []
        yaw_command = []

        with open(file, "r", encoding="utf-8-sig", newline="") as f:
            reader = csv.DictReader(f)

            required = {"distance_m", "lateral_error_m", "heading_error_rad","yaw_command"}
            if not required.issubset(reader.fieldnames or []):
                raise ValueError(
                    f"{file} 缺少必要列，需要: {required}，实际列: {reader.fieldnames}"
                )

            for row in reader:
                try:
                    distance.append(float(row["distance_m"]))
                    lateral_error.append(abs(float(row["lateral_error_m"])))
                    heading_error.append(abs(float(row["heading_error_rad"])))
                    yaw_command.append(float(row["yaw_command"]))
                except (ValueError, TypeError):
                    continue

        name = os.path.splitext(os.path.basename(file))[0]
        data[name] = {
            "distance": distance,
            "lateral_error": lateral_error,
            "heading_error": heading_error,
            "yaw_command": yaw_command,
        }

# 颜色由 matplotlib 自动分配，不手动指定
names = list(data.keys())

# =========================
# 图1：Lateral Error
# =========================
plt.figure(figsize=(12, 6))

for name in names:
    plt.plot(
        data[name]["distance"][::15],
        data[name]["lateral_error"][::15],
        label=name,
        linewidth=1.5,
    )

plt.xlabel("Distance (m)")
plt.ylabel("Lateral Error (m)")
plt.title("Lateral Error Comparison")
plt.grid(True, alpha=0.3)
plt.legend()
plt.tight_layout()

lateral_output = "X_Lateral_error_comparison.png"
plt.savefig(lateral_output, dpi=200)
plt.show()

# =========================
# 图2：Heading Error
# =========================
plt.figure(figsize=(12, 6))

for name in names:
    plt.plot(
        data[name]["distance"],
        data[name]["heading_error"],
        label=name,
        linewidth=1.5,
    )

plt.axhline(0, linewidth=2.0)
plt.xlabel("Distance (m)")
plt.ylabel("Heading Error (rad)")
plt.title("Heading Error Comparison")
plt.grid(True, alpha=0.3)
plt.legend()
plt.tight_layout()

heading_output = "X_Heading_error_comparison.png"
plt.savefig(heading_output, dpi=200)
plt.show()


# =========================
# 图3：Yaw Command
# =========================
plt.figure(figsize=(12, 6))

for name in names:
    plt.plot(
        data[name]["distance"],
        data[name]["yaw_command"],
        label=name,
        linewidth=1.5,
    )

plt.axhline(0, linewidth=2.0)
plt.xlabel("Distance (m)")
plt.ylabel("Yaw Command (rad)")
plt.title("Yaw Command Comparison")
plt.grid(True, alpha=0.3)
plt.legend()
plt.tight_layout()

yaw_output = "X_Yaw_command_comparison.png"
plt.savefig(yaw_output, dpi=200)
plt.show()
print("读取完成：")
for name in names:
    print(f"{name}: {len(data[name]['distance'])} points")

print("\n已保存：")
print(lateral_output)
print(heading_output)
print(yaw_output)


print("\n" + "=" * 90)
print(
    f"{'Controller':<20}"
    f"{'Lat Max(m)':>12}"
    f"{'Lat Mean(m)':>13}"
    f"{'Lat RMSE(m)':>14}"
    f"{'Lat P95(m)':>13}"
    f"{'Head Max(deg)':>15}"
    f"{'Head RMSE(deg)':>16}"
)
print("=" * 100)

for name in names:
    lat = np.array(data[name]["lateral_error"])
    head = np.array(data[name]["heading_error"])

    print(
        f"{name:<20}"
        f"{np.max(lat):>12.4f}"
        f"{np.mean(lat):>13.4f}"
        f"{np.sqrt(np.mean(lat**2)):>14.4f}"
        f"{np.percentile(lat, 95):>13.4f}"
        f"{np.degrees(np.max(head)):>15.4f}"
        f"{np.degrees(np.sqrt(np.mean(head**2))):>16.4f}"
    )

print("=" * 100)