import os
import pandas as pd
import numpy as np
from matplotlib import pyplot as plt

# Path to the folder containing the CSV file
file_folder = "D:\\industrial robots 1\\route 2"
# File name
file_name1 = "dataLog_Omron-LD_241024_182221.csv"

file_path1 = os.path.join(file_folder, file_name1)

# Read the file
df1 = pd.read_csv(file_path1)
# Display dataset dimensions
print(df1.shape)
# Display the first 5 rows of the dataset
print(df1.head())
# Display column names
print(df1.columns)

# Select values from route with name "Trasa1"
df1 = df1[df1["ModeName  (string)"] == "Patrolling route Trasa1"]
# Display dataset dimensions
print(df1.shape)

x_robot = np.array(df1['RobotX  (mm)']).reshape((-1, 1))
y_robot = np.array(df1['RobotY  (mm)']).reshape((-1, 1))
th_robot = np.array(df1['RobotTh  (degrees)']).reshape((-1, 1))

# Display data
plt.title('')
plt.xlabel('x [mm]')
plt.ylabel('y [mm]')
plt.plot(x_robot, y_robot, c="g")
plt.show()

# Calculate accuracy

# Calculate repeatability

# Extract values where the robot executed the "Wait" command upon reaching its target - where "ModeStatus (string)" == 'Waiting'