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

# Sciezka do folderu z plikiem csv
file_folder = "D:\\roboty przemyslowe 1\\trasa 2"
# Nazwa pliku
file_name1 = "dataLog_Omron-LD_241024_182221.csv"

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

# Odczytanie pliku
df1 = pd.read_csv(file_path1)
# Wyswietlenie wymiarow datasetu
print(df1.shape)
# Wyswietlenie 5 pierwszych linijek datasetu
print(df1.head())
# Wyswietlenie nazw kolumn
print(df1.columns)

# Wybranie wartosci z przejazdu Trasy 1
df1 = df1[df1["ModeName  (string)"] == "Patrolling route Trasa1"]
# Wyswietlenie wymiarow datasetu
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))

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

# Obliczyc dokladnosc

# Obliczyc powtarzalnosc

# Nalezy wyodrebnic wartosci, gdzie Robot po dojechaniu do celu wykonywal polecenie Wait - tam gdzie "ModeStatus  (string)" == 'Waiting'