import math
import drawsvg as draw
import os
from pathlib import Path

images_path = Path(os.path.realpath(__file__)).parent.joinpath('src').joinpath('images')

# Define arrowhead marker
arrow = draw.Marker(0,0,10,10, refX="10", refY="5", markerWidth="6", markerHeight="6", orient='auto-start-reverse')
arrow.append(draw.Path(d='M 0 0 L 10 5 L 0 10 z', fill='black'))

class Point:
    def __init__(self, x, y):
        self.x = x
        self.y = y

def force_diagram():
    d = draw.Drawing(600, 600, origin='center')

    # Outline drawing area
    d.append(draw.Rectangle(-300,-300,600,600, fill='white'))

    # Coordinate system
    c = translate(coordinate_system(), -230, -230)
    d.append(c)

    # Define group
    g = draw.Group(fill='black', stroke='black', stroke_width=1.5)

    A = Point(0, -200)
    B = Point(0, 200)
    steering_angle = 20

    w = 300
    g.append(draw.Rectangle(-w/2,-280, w, 560, fill='none', stroke_width=1))
    wheel = draw.Rectangle(-15,-40, 30, 80, fill='none', stroke_width=1)
    g.append(translate(rotate(wheel, steering_angle), -w/2, A.y))
    g.append(translate(rotate(wheel, steering_angle), w/2, A.y))
    g.append(translate(wheel, -w/2, B.y))
    g.append(translate(wheel, w/2, B.y))

    # Center of Mass
    g.append(draw.Circle(0, 0, 3))
    g.append(text("CM", 0, 0, angle=180))

    # Velocity vector
    g.append(draw.Line(0, 0, -50, -160, marker_end=arrow))
    g.append(text("v", -50, -160, angle=-110))

    # Point A
    g.append(text("A", A.x, A.y, angle=180))
    g.append(draw.Circle(A.x, A.y, 3))
    g.append(draw.Line(A.x, A.y, 80, -170, marker_end=arrow))
    g.append(text("F_A", 80, -170))

    # Point B
    g.append(text("B", B.x, B.y, angle=180))
    g.append(draw.Circle(B.x, B.y, 3))
    g.append(draw.Line(B.x, B.y, 70, 200, marker_end=arrow))
    g.append(text("F_B", 70, 200))

    d.append(translate(g, 100, 0))
    d.save_svg(f'{images_path}/force_diagram.svg')

def coordinate_system():
    g = draw.Group(stroke='black', stroke_width=1.5)

    g.append(draw.Line(0, 0, 50, 0, marker_end=arrow))
    g.append(text("x", 50, 0))

    g.append(draw.Line(0, 0, 0, 50, marker_end=arrow))
    g.append(text("y", 0, 50, angle=90))

    # Arc
    cx = 0
    cy = 0
    r = 50
    start = 120
    end = -30
    start_angle = math.radians(start)
    end_angle = math.radians(end)

    x0 = cx + r * math.cos(start_angle)
    y0 = cy + r * math.sin(start_angle)
    x1 = cx + r * math.cos(end_angle)
    y1 = cy + r * math.sin(end_angle)

    # SVG arc path from (x0, y0) to (x1, y1) around center
    path_d = f'M {x0} {y0} A {r} {r} 0 1 1 {x1} {y1}'
    arc = draw.Path(d=path_d, fill='none', marker_end=arrow)
    g.append(arc)

    return g

# Wrap in translated group
def translate(t, x, y):
    g = draw.Group(transform=f"translate({x},{y})")
    g.append(t)
    return g

# Wrap in rotated group
def rotate(t, a):
    g = draw.Group(transform=f"rotate({a})")
    g.append(t)
    return g

def text(text, x, y, offset=20, angle=0):
    angle = math.radians(angle)
    x = x + offset * math.cos(angle)
    y = y + offset * math.sin(angle)

    return draw.Text(text, 18, x, y, stroke_width=1, center=True)

force_diagram()