extends Node2D

onready var rope_start
onready var rope_end
onready var rope_start_joint
onready var rope_end_joint

var RopePiece = preload("res://src/Contraptions/Rope/RopePiece.tscn")
var RopeAnchor = preload("res://src/Contraptions/Rope/RopeAnchor.tscn")
var piece_length := 2.0
var rope_close_tolerance := 2.0
var rope_pieces := []
var rope_piece_positions : PoolVector2Array= []
var anchor_distance := 1.0
# Is given in blocks
var movement_radius := 1.0

export var rope_tightness := 0.5
export var block_size := 16

func _process(_delta: float) -> void:
	# TODO Resize or reinitialize?
	rope_piece_positions.resize(0)
	rope_piece_positions = get_piece_positions()
	anchor_distance = rope_piece_positions[0].distance_to(rope_piece_positions[-1])
	if !rope_piece_positions.empty():
		update()

func spawn_rope(start_pos: Vector2, end_pos: Vector2, m_radius: float = 100, new_anchors: bool = true):
	# The rope should be the defined length even if the anchors are close together/far apart
	movement_radius = m_radius * block_size
	var mock_end_pos = start_pos + (end_pos - start_pos).normalized() * movement_radius * rope_tightness
	if(new_anchors):
		rope_start = RopeAnchor.instance()
		rope_end = RopeAnchor.instance()
		rope_start_joint = rope_start.get_node("cshape/pjoint")
		rope_end_joint = rope_end.get_node("cshape/pjoint")
		add_child(rope_start, true)
		add_child(rope_end, true)
	rope_start.global_position = start_pos
	rope_end.global_position = mock_end_pos
	anchor_distance = start_pos.distance_to(mock_end_pos)
	var pieces_amount = round(anchor_distance / piece_length)
	var spawn_angle = (mock_end_pos-start_pos).angle() - PI/2
	print(pieces_amount)
	print(mock_end_pos)
	print(spawn_angle)
	create_rope(pieces_amount, rope_start, mock_end_pos, spawn_angle)
	rope_end.global_position = end_pos


func create_rope(pieces_amount: int, last_piece: Object, end_pos: Vector2, spawn_angle: float) -> void:
	for i in pieces_amount:
		last_piece = add_piece(last_piece, i, spawn_angle)
		last_piece.set_name("rope_piece_"+str(i))
		rope_pieces.append(last_piece)

		var joint_pos = last_piece.get_node("cshape/pjoint").global_position
		if joint_pos.distance_to(end_pos) < rope_close_tolerance:
			break
	
	rope_end_joint.node_a = rope_end.get_path()
	rope_end_joint.node_b = rope_pieces[-1].get_path()


func add_piece(prev_piece: Object, id: int, spawn_angle:float) -> Object:
	var prev_joint: PinJoint2D = prev_piece.get_node("cshape/pjoint") as PinJoint2D
	var new_piece: Object = RopePiece.instance() as Object
	new_piece.global_position = prev_joint.global_position
	new_piece.rotation = spawn_angle
	new_piece.prev_piece = prev_piece
	new_piece.id = id
	add_child(new_piece, true)
	prev_joint.node_a = prev_piece.get_path()
	prev_joint.node_b = new_piece.get_path()

	return new_piece

func get_piece_positions() -> Array:
	var rope_points := []
	rope_points.append(rope_start_joint.global_position)
	for p in rope_pieces:
		rope_points.append(p.global_position)
	rope_points.append(rope_end_joint.global_position)
	return rope_points

func _draw() -> void:
	draw_polyline(
		rope_piece_positions, 
		Color(Color.green.linear_interpolate(Color.red, anchor_distance/max(1.0, movement_radius))),
		1.2
	)
