// Fill out your copyright notice in the Description page of Project Settings.


#include "Robot.h"

ARobot::ARobot(){
	PrimaryActorTick.bCanEverTick = true;
}

void ARobot::BeginPlay(){
	Super::BeginPlay();

	for (auto* Link : Links)
		Link->Setup();
	ResetPositionAndOrientation();
}

void ARobot::ResetPositionAndOrientation(){
	for (const auto* Joint : Joints){
		const ULink* L1 = Joint->FirstLink;
		ULink* L2 = Joint->SecondLink;

		Vector3d ConnectPosL1 = L1->Position + Joint->GetFirstWorldDirection();
		const Vector3d Correction = ConnectPosL1 - Joint->GetSecondWorldDirection();
		L2->Position = Correction;
	}
}


void ARobot::Connect(ULink* L1, ULink* L2){
	Links.AddUnique(L1);
	Links.Add(L2);
	UJoint* Joint = NewObject<UJoint>(this);
	Joint->FirstLink = L1;
	Joint->SecondLink = L2;
	L1->NextJoint = Joint;
	L2->PrevJoint = Joint;
	Joints.Add(Joint);
}


void ARobot::Tick(const float DeltaTime){
	Super::Tick(DeltaTime);
	
	constexpr int SubSteps = 50;
	const double h = DeltaTime / SubSteps;
	for (int i = 0; i < SubSteps; i++){
		for (auto* Link : Links)
			Link->Update(h);
		for (const auto* Joint : Joints)
			Joint->SolvePosition(h);
		for (auto* Link : Links)
			Link->Integrate(h);
		for (const auto* Joint : Joints)
			Joint->SolveVelocity(h);
	}
	for (auto* Link : Links)
		Link->UpdateInternalTransform();
}