import pygame
import math


class Envir:
    def __init__(self, dimensions):
        #  colours
        self.black = (0, 0, 0)
        self.white = (255, 255, 255)
        self.green = (0, 255, 0)
        self.blue = (0, 0, 255)
        self.red = (255, 0, 0)
        self.yellow = (255, 255, 0)

        #  Map dimensions
        self.height = dimensions[0]
        self.width = dimensions[1]

        #  Window Settings
        pygame.display.set_caption("Differential Drive Car")
        self.map = pygame.display.set_mode((self.width,
                                            self.height))
        
        #  Text variables
        self.txt = "default text"
        self.font = pygame.font.Font('freesansbold.ttf',50)
        self.txt = self.font.render('default', True, self.white, self.black)
        self.txtRect = self.txt.get_rect()
        self.txtRect.center = (dimensions [1] - 600,
                               dimensions [0] - 100)
        
    def write_info(self, Vl, Vr, theta):
        txt = f"Vl = {Vl} Vr = {Vr}  theta = {int(math.degrees(theta))}"
        self.txt = self.font.render (txt, True, self.white, self.black)
        self.map.blit(self.txt, self.txtRect)



class Robot:
    def __init__(self, startpos, car, width):
        self.m2p = 3779.52  # meters to pixels

        #  robot dims
        self.w = width
        self.x = startpos[0]
        self.y = startpos[1]
        self.theta = 0
        self.vl = 0.01 * self.m2p  # speed in meters per second initially before multiplication
        self.vr = 0.01 * self.m2p
        self.maxspeed = 0.02 * self.m2p
        self.minspeed = 0.02 * self.m2p

        #  graphics
        self.img = pygame.image.load(car)
        self.rotated = self.img
        self.rect = self.rotated.get_rect(center=(self.x,
                                                  self.y))

    def draw(self, map):
        map.blit(self.rotated, self.rect)


    def move(self, event=None):

        #  input keys for the robot
        if event is not None:
            if event.type == pygame.KEYDOWN:
                if event.key == pygame.K_1:
                    self.vl += 0.001 * self.m2p
                elif event.key == pygame.K_2:
                    self.vl -= 0.001 * self.m2p
                elif event.key == pygame.K_9:
                    self.vr += 0.001 * self.m2p
                elif event.key == pygame.K_8:
                    self.vr -= 0.001 * self.m2p
        
        #  moving the robot
        self.x += ((self.vl+self.vr)/2) * math.cos(self.theta) * dt
        self.y -= ((self.vl+self.vr)/2) * math.cos(self.theta) * dt
        self.theta += (self.vr - self.vl)/self.w * dt

        #  rotating the robot
        self.rotated = pygame.transform.rotozoom(self.img,
                                                 math.degrees(self.theta), 1)
        self.rect = self.rotated.get_rect(center=(self.x, self.y))




#  Initialisation
pygame.init()

start = (200, 200)  # Start position
dims = (800, 1200)  # dimensions
running = True  # Running or not
environment = Envir(dims)  # the envir
dt = 0
lasttime = pygame.time.get_ticks()

#  Robot initialization
robot = Robot(start, r"C:\Users\mrash\OneDrive\Diff drive robot\car.png",
              0.01 * 3779.52)
#  Simulation loop
while running:
    for event in pygame.event.get():  # Activating the quit button
        if event.type == pygame.QUIT:
            running = False
        robot.move(event)
        
    dt = (pygame.time.get_ticks() - lasttime)/1000
    lasttime = pygame.time.get_ticks()
    pygame.display.update()
    environment.map.fill(environment.black)
    robot.draw(environment.map)
    environment.write_info(int(robot.vl),
                           int(robot.vr),
                           robot.theta)