import pygame
import math
import csv
from datetime import datetime

class Object:
    def __init__(self):
        self.m2p = 3779.52
        self.width = 0.01 * self.m2p
        self.vl=0.01*self.m2p
        self.vr=0.01*self.m2p
        self.theta = 0
        self.startpos = (200, 200)
        self.x=self.startpos[0]
        self.y=self.startpos[1]
        self.path = []

    def move(self, deltaTime, event=None):
        if event is not None:
            if event.type == pygame.KEYDOWN:
                if event.key == pygame.K_LEFT:
                    self.vl -= 10
                elif event.key == pygame.K_RIGHT:
                    self.vl += 10
                elif event.key == pygame.K_UP:
                    self.vr += 10
                elif event.key == pygame.K_DOWN:
                    self.vr -= 10

        self.x+=((self.vl+self.vr)/2)*math.cos(self.theta)*deltaTime
        self.y-=((self.vl+self.vr)/2)*math.sin(self.theta)*deltaTime
        self.theta+=(self.vr-self.vl)/self.width*deltaTime

        self.path.append((self.x, self.y))
        if len(self.path) > 10000:
            self.path.pop(0)

    def displayValues(self, screen, font, color):
        theta_deg = math.degrees(self.theta)
        screen.blit(font.render(f"Theta: {theta_deg:.2f}°", True, color), (20, 60))
        screen.blit(font.render(f"vl: {self.vl:.2f}", True, color), (20, 100))
        screen.blit(font.render(f"vr: {self.vr:.2f}", True, color), (20, 140))
        screen.blit(font.render(f"x: {self.x:.2f}", True, color), (20, 180))
        screen.blit(font.render(f"y: {self.y:.2f}", True, color), (20, 220))

    def drawPath(self, screen, color):
        for pos in self.path:
            pygame.draw.circle(screen, color, (int(pos[0]), int(pos[1])),1)

def open_new_csv():
    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
    filename = f"robot_data_{timestamp}.csv"
    file = open(filename, mode='w', newline='')
    writer = csv.writer(file)
    writer.writerow(['time', 'theta_deg', 'vl', 'vr', 'x', 'y'])
    return file, writer

object = Object()

# ----------------------- Main Function ---------------------
BLACK = (0, 0, 0) # Background color
WHITE = (255, 255, 255)  # Text color
START = (200,200) #Start position

pygame.init()

clock = pygame.time.Clock()
screen = pygame.display.set_mode((1200, 800))
pygame.display.set_caption("Values")
font = pygame.font.SysFont(None, 36)

lasttime = pygame.time.get_ticks()
running = True

# Open CSV file for logging
csv_file, csv_writer = open_new_csv()

while running:
    current_time = pygame.time.get_ticks()
    deltaTime = (current_time - lasttime) / 1000.0
    lasttime = current_time
    
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False
        elif event.type == pygame.KEYDOWN:
            if event.key == pygame.K_r:  # Press 'R' to reset
                object.x = object.startpos[0]
                object.y = object.startpos[1]
                object.theta = 0
                object.vl = 0.01 * object.m2p
                object.vr = 0.01 * object.m2p
                object.path.clear()
                
                # Close old csv file and open new one
                csv_file.close()
                csv_file, csv_writer = open_new_csv()
            else:
                object.move(deltaTime, event)

    object.move(deltaTime)

    # Write current data to CSV
    time_now = pygame.time.get_ticks() / 1000  # seconds
    theta_deg = math.degrees(object.theta)
    csv_writer.writerow([f"{time_now:.3f}", f"{theta_deg:.2f}", f"{object.vl:.2f}", f"{object.vr:.2f}", f"{object.x:.2f}", f"{object.y:.2f}"])

    screen.fill(BLACK)

    delta_text = font.render(f"Delta Time: {deltaTime:.4f} s", True, WHITE)
    screen.blit(delta_text, (20, 20))

    object.displayValues(screen, font, WHITE)
    object.drawPath(screen, WHITE)

    pygame.display.flip()
    clock.tick(60)

csv_file.close()
pygame.quit()