//#####################################################################
// Copyright 2003-2004, Ronald Fedkiw, Eran Guendelman, Rachel Weinstein.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class RIGID_BODY_COLLISIONS_3D
//#####################################################################
#ifndef __RIGID_BODY_COLLISIONS_3D__
#define __RIGID_BODY_COLLISIONS_3D__

#include <float.h>
#include <stdlib.h>
#include "RIGID_BODY_COLLISIONS.h"
#include "RIGID_BODY_SPATIAL_PARTITION_3D.h"
#include "RIGID_BODY_INTERSECTIONS_3D.h"
namespace PhysBAM
{

template<class T> class ARTICULATED_RIGID_BODY_3D;

template<class T>
class RIGID_BODY_COLLISIONS_3D: public RIGID_BODY_COLLISIONS<T>
{
public:
	using RIGID_BODY_COLLISIONS<T>::verbose;
	using RIGID_BODY_COLLISIONS<T>::skip_collision_check;
	using RIGID_BODY_COLLISIONS<T>::collision_manager;
protected:
	using RIGID_BODY_COLLISIONS<T>::collision_iterations;
	using RIGID_BODY_COLLISIONS<T>::contact_iterations;
	using RIGID_BODY_COLLISIONS<T>::contact_level_iterations;
	using RIGID_BODY_COLLISIONS<T>::contact_pair_iterations;
	using RIGID_BODY_COLLISIONS<T>::epsilon_scaling;
	using RIGID_BODY_COLLISIONS<T>::epsilon_scaling_for_level;
	using RIGID_BODY_COLLISIONS<T>::shock_propagation_iterations;
	using RIGID_BODY_COLLISIONS<T>::shock_propagation_level_iterations;
	using RIGID_BODY_COLLISIONS<T>::shock_propagation_pair_iterations;
	using RIGID_BODY_COLLISIONS<T>::push_out_iterations;
	using RIGID_BODY_COLLISIONS<T>::push_out_level_iterations;
	using RIGID_BODY_COLLISIONS<T>::push_out_pair_iterations;
	using RIGID_BODY_COLLISIONS<T>::use_freezing_with_push_out;
	using RIGID_BODY_COLLISIONS<T>::use_gradual_push_out;
	using RIGID_BODY_COLLISIONS<T>::desired_separation_distance;
	using RIGID_BODY_COLLISIONS<T>::rolling_friction;
	using RIGID_BODY_COLLISIONS<T>::precomputed_contact_pairs_for_level;

public:
	LIST_ARRAY<RIGID_BODY_3D<T>*>& rigid_bodies;
	RIGID_BODY_SPATIAL_PARTITION_3D<T> spatial_partition;
	RIGID_BODY_INTERSECTIONS_3D<T> intersections;
	T min_bounding_box_width; // not used internally - but useful externally
private:
	ARRAY<VECTOR_3D<T> > position_save;
	ARRAY<QUATERNION<T> > orientation_save;

public:
	RIGID_BODY_COLLISIONS_3D (LIST_ARRAY<RIGID_BODY_3D<T>*>& rigid_bodies_input, const T voxel_size)
		: rigid_bodies (rigid_bodies_input),
		  spatial_partition (voxel_size, rigid_bodies), intersections (rigid_bodies)
	{
		// initialize bounding box information
		min_bounding_box_width = FLT_MAX;

		for (int i = 1; i <= rigid_bodies.m; i++)
		{
			BOX_3D<T>& box = *rigid_bodies (i)->triangulated_surface->bounding_box;
			VECTOR_3D<T> size = box.Size();
			min_bounding_box_width = min (min_bounding_box_width, size.x, size.y, size.z);
		}
	}

	void Initialize_Data_Structures()
	{
		intersections.Initialize_Data_Structures();
		skip_collision_check.Initialize (rigid_bodies.m);
		position_save.Resize_Array (rigid_bodies.m);
		orientation_save.Resize_Array (rigid_bodies.m);
	}

	static void Adjust_Bounding_Boxes (LIST_ARRAY<RIGID_BODY_3D<T>*>& rigid_bodies, const T false_thickness = 1, const T extra_padding_distance = 0)
	{
		for (int i = 1; i <= rigid_bodies.m; i++)
		{
			assert (rigid_bodies (i)->triangulated_surface->bounding_box); // bounding boxes should already be set up
			BOX_3D<T>& box = *rigid_bodies (i)->triangulated_surface->bounding_box;
			bool already_adjusted = false; // multiple rigid bodies can share a triangulated surface - don't adjust bounding box multiple times

			for (int j = 1; j <= i - 1; j++) if (rigid_bodies (i)->triangulated_surface->bounding_box == rigid_bodies (j)->triangulated_surface->bounding_box)
				{
					already_adjusted = true;
					break;
				}

			if (!already_adjusted)
			{
				if (box.xmin == box.xmax)
				{
					box.xmin -= false_thickness;
					box.xmax += false_thickness;
				}

				if (box.ymin == box.ymax)
				{
					box.ymin -= false_thickness;
					box.ymax += false_thickness;
				}

				if (box.zmin == box.zmax)
				{
					box.zmin -= false_thickness;
					box.zmax += false_thickness;
				}

				box.xmin -= extra_padding_distance;
				box.xmax += extra_padding_distance;
				box.ymin -= extra_padding_distance;
				box.ymax += extra_padding_distance;
				box.zmin -= extra_padding_distance;
				box.zmax += extra_padding_distance;
			}

			rigid_bodies (i)->Update_Bounding_Box();
		}
	}

	static BOX_3D<T> Scene_Bounding_Box (LIST_ARRAY<RIGID_BODY_3D<T>*>& rigid_bodies)
	{
		BOX_3D<T> scene_bounding_box = rigid_bodies (1)->Axis_Aligned_Bounding_Box();

		for (int i = 2; i <= rigid_bodies.m; i++) if (!rigid_bodies (i)->is_static && rigid_bodies (i)->add_to_spatial_partition)
				scene_bounding_box = BOX_3D<T>::Combine (scene_bounding_box, rigid_bodies (i)->Axis_Aligned_Bounding_Box());

		return scene_bounding_box;
	}

	static T Average_Bounding_Box_Size (LIST_ARRAY<RIGID_BODY_3D<T>*>& rigid_bodies)
	{
		T average_size = 0;
		int count = 0;

		for (int i = 1; i <= rigid_bodies.m; i++) if (!rigid_bodies (i)->is_static && rigid_bodies (i)->add_to_spatial_partition)
			{
				count++;
				VECTOR_3D<T> size = rigid_bodies (i)->triangulated_surface->bounding_box->Size();
				average_size += (size.x + size.y + size.z);
			}

		return (T) one_third * average_size / count;
	}

	static T Maximum_Bounding_Box_Size (LIST_ARRAY<RIGID_BODY_3D<T>*>& rigid_bodies)
	{
		T max_size = 0;

		for (int i = 1; i <= rigid_bodies.m; i++) if (!rigid_bodies (i)->is_static && rigid_bodies (i)->add_to_spatial_partition)
			{
				VECTOR_3D<T> size = rigid_bodies (i)->triangulated_surface->bounding_box->Size();
				max_size = max (max_size, size.x, size.y, size.z);
			}

		return max_size;
	}

	void Save_Positions()
	{
		for (int i = 1; i <= rigid_bodies.m; i++) Save_Position (i);
	}

	void Restore_Positions()
	{
		for (int i = 1; i <= rigid_bodies.m; i++) Restore_Position (i);
	}

	void Euler_Step_Position_With_New_Velocity (const int index, const T dt, const T time)
	{
		VECTOR_3D<T> velocity = rigid_bodies (index)->velocity, angular_momentum = rigid_bodies (index)->angular_momentum; // save velocity
		rigid_bodies (index)->Euler_Step_Velocity (dt, time); // temporarily update velocity
		Euler_Step_Position (index, dt, time);
		rigid_bodies (index)->velocity = velocity;
		rigid_bodies (index)->angular_momentum = angular_momentum; // restore velocity
		rigid_bodies (index)->Update_Angular_Velocity();
	} // re-sync this

private:
	void Save_Position (const int index)
	{
		position_save (index) = rigid_bodies (index)->position;
		orientation_save (index) = rigid_bodies (index)->orientation;
	}

	void Restore_Position (const int index)
	{
		rigid_bodies (index)->position = position_save (index);
		rigid_bodies (index)->orientation = orientation_save (index);
		rigid_bodies (index)->Update_Angular_Velocity();
		Update_Bounding_Box (index);
	}

	void Euler_Step_Position (const int index, const T dt, const T time)
	{
		rigid_bodies (index)->Euler_Step_Position (dt, time);
		Update_Bounding_Box (index);
		skip_collision_check.Set_Last_Moved (index);
	}

	void Update_Bounding_Box (const int index)
	{
		if (!rigid_bodies (index)->is_static) rigid_bodies (index)->Update_Bounding_Box();
	}

//#####################################################################
public:
	bool Check_For_Any_Interpenetration();
	void Print_Interpenetration_Statistics();
//#####################################################################
};
}
#endif