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


#include "Robot.h"

#include "util.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 Vector3d ConnectPosL1 = Joint->FirstLink->Position + Joint->GetFirstWorldDirection();
		Joint->SecondLink->Position = ConnectPosL1 - Joint->GetSecondWorldDirection();
		Joint->SecondLink->Orientation = Quaterniond::Identity();
	}
}


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){
			Vector3d TransForce = Vector3d::Zero();
			if (HasGravity)
				TransForce += Link->GetGravityForce();
			if (Link->IsEffector)
				TransForce += Link->Mass * ToEigen(EffectorMove);
			const Vector3d RotForce = Vector3d::Zero();
			Link->Update(h, TransForce, RotForce);
		}
		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();
}

ULink* ARobot::GetEffector() const{
	for (auto* Link : Links)
		if (Link->IsEffector)
			return Link;
	return nullptr;
}