import numpy as np
import math
import matplotlib.pyplot as plt

k = 0.1  # 前视距离系数
Lfc = 2.0  # 前视距离
Kp = 1.0  # 速度P控制器系数
dt = 0.1  # 时间间隔，单位：s
L = 2.9  # 车辆轴距，单位：m


def plot_arrow(x, y, yaw, length=5, width=1):
    dx = length * math.cos(yaw)
    dy = length * math.sin(yaw)
    plt.arrow(x, y, dx, dy, head_length=width, head_width=width)
    plt.plot([x, x + dx], [y, y + dy])


def plot_robot(x, y, yaw, robot_length, robot_width):
    corner_x = x - robot_length / 2
    corner_y = y - robot_width / 2
    rectangle = plt.Rectangle((corner_x, corner_y), robot_length, robot_width, angle=math.degrees(yaw), fill=False)
    plt.gca().add_patch(rectangle)


class VehicleState:

    def __init__(self, x=0.0, y=0