// 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();
	
	GetComponents<ULink>(Parts);
	for (auto* Link : Parts){
		Link->Setup();
		if (Link->PrevJoint)
			Joints.Add(Link->PrevJoint);
	}

	// Example: 3 Parts are connected with 2 Joints
	assert(Parts.Num() == Joints.Num() + 1);
}

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