import time
import logger
import pandas as pd
import matplotlib.pyplot as plt
import matplotlib.animation as animation

# ✅ Configure Logging for Graphing Module
LOGGING_ENABLED = True

if LOGGING_ENABLED:
    logging.basicConfig(
        filename="graphing.log",
        level=logging.DEBUG,
        format="%(asctime)s [%(levelname)s] %(message)s",
        datefmt="%d-%m-%Y %H:%M:%S"
    )

class RSIGraphing:
    """Handles real-time and CSV-based graphing for RSI analysis."""

    def __init__(self, csv_file=None):
        """Initialize graphing for real-time plotting or CSV replay."""
        self.csv_file = csv_file
        self.fig, self.ax = plt.subplots(1, 2, figsize=(12, 5))  # ✅ Two subplots: Time-Series & X-Y Trajectory

        # ✅ Titles and Labels
        self.ax[0].set_title("Real-Time Position Tracking")
        self.ax[0].set_xlabel("Time")
        self.ax[0].set_ylabel("Position (mm)")
        self.ax[1].set_title("2D Trajectory (X-Y)")
        self.ax[1].set_xlabel("X Position")
        self.ax[1].set_ylabel("Y Position")

        # ✅ Data storage for plotting
        self.time_data = []
        self.x_data, self.y_data, self.z_data = [], [], []
        self.actual_x, self.actual_y, self.actual_z = [], [], []
        self.trajectory_x, self.trajectory_y = [], []

        self.plot_mode = "TCP"  # ✅ Toggle between "TCP" and "Joints"

        if csv_file:
            self.load_csv_log(csv_file)
            self.plot_csv_data()
        else:
            self.ani = animation.FuncAnimation(self.fig, self.update_graph, interval=500, save_count=100)

            plt.show()

    def update_graph(self, frame):
        """Update the live graph with new position data."""
        timestamp = time.strftime("%H:%M:%S")
        self.time_data.append(timestamp)

        # ✅ Simulated Data for Testing (Replace with real-time data)
        self.x_data.append(len(self.time_data) * 10)
        self.y_data.append(len(self.time_data) * 5)
        self.z_data.append(len(self.time_data) * 2)

        self.actual_x.append(len(self.time_data) * 9.8)
        self.actual_y.append(len(self.time_data) * 4.9)
        self.actual_z.append(len(self.time_data) * 1.8)

        # ✅ 2D Trajectory (X-Y)
        self.trajectory_x.append(len(self.time_data) * 9.8)
        self.trajectory_y.append(len(self.time_data) * 4.9)

        # ✅ Clear and replot data
        self.ax[0].clear()
        self.ax[0].plot(self.time_data, self.x_data, label="Planned X", linestyle="dashed")
        self.ax[0].plot(self.time_data, self.y_data, label="Planned Y", linestyle="dashed")
        self.ax[0].plot(self.time_data, self.z_data, label="Planned Z", linestyle="dashed")

        self.ax[0].plot(self.time_data, self.actual_x, label="Actual X", marker="o")
        self.ax[0].plot(self.time_data, self.actual_y, label="Actual Y", marker="o")
        self.ax[0].plot(self.time_data, self.actual_z, label="Actual Z", marker="o")

        self.ax[0].legend()
        self.ax[0].set_title(f"Real-Time {self.plot_mode} Position")
        self.ax[0].set_xlabel("Time")
        self.ax[0].set_ylabel("Position (mm)")

        # ✅ 2D Trajectory (X-Y)
        self.ax[1].clear()
        self.ax[1].plot(self.trajectory_x, self.trajectory_y, label="Actual Path", marker="o")
        self.ax[1].set_title("2D Trajectory (X-Y)")
        self.ax[1].set_xlabel("X Position")
        self.ax[1].set_ylabel("Y Position")
        self.ax[1].legend()

    def load_csv_log(self, filename):
        """Load a CSV log and replay motion."""
        df = pd.read_csv(filename)
        self.time_data = df["Timestamp"].tolist()
        self.trajectory_x = df["Receive.RIst.X"].tolist()
        self.trajectory_y = df["Receive.RIst.Y"].tolist()

        logging.info(f"Loaded CSV log: {filename}")

    def plot_csv_data(self):
        """Plot data from a CSV file for post-analysis."""
        plt.figure(figsize=(10, 5))
        plt.plot(self.trajectory_x, self.trajectory_y, label="Planned Path", linestyle="dashed")
        plt.scatter(self.trajectory_x, self.trajectory_y, color='red', label="Actual Path", marker="o")
        plt.title("CSV Replay: 2D Trajectory (X-Y)")
        plt.xlabel("X Position")
        plt.ylabel("Y Position")
        plt.legend()
        plt.show()

if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser(description="RSI Graphing Utility")
    parser.add_argument("--csv", help="Path to CSV file for replay mode", type=str)
    args = parser.parse_args()

    if args.csv:
        RSIGraphing(csv_file=args.csv)
    else:
        RSIGraphing()  # Run in live mode if no CSV is provided