//#####################################################################
// Copyright 2004-2005, Igor Neverov, Eftychios Sifakis.
// This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt.
//#####################################################################
// Class ACTIVATION_CONTROL_SET
//#####################################################################
#ifndef __ACTIVATION_CONTROL_SET__
#define __ACTIVATION_CONTROL_SET__

#include "FACE_CONTROL_SET.h"
#include "../../Public_Library/Forces_And_Torques/DIAGONALIZED_FINITE_VOLUME_3D.h"
#include "../../Public_Library/Interpolation/LINEAR_INTERPOLATION.h"

namespace PhysBAM
{

template <class T>
class ACTIVATION_CONTROL_SET: public FACE_CONTROL_SET<T>
{
public:
	LIST_ARRAY<T> activations, activations_save;
	LIST_ARRAY<bool> activation_active;
	mutable int single_activation_used_for_force_derivative;
	DIAGONALIZED_FINITE_VOLUME_3D<T>* muscle_force;
	T activation_cutoff, max_optimization_step_length, activation_penalty_coefficient;

	ACTIVATION_CONTROL_SET()
		: single_activation_used_for_force_derivative (0), muscle_force (0), activation_cutoff ( (T) 1), activation_penalty_coefficient ( (T) 10), max_optimization_step_length ( (T).4)
	{
		Save_Controls();
	}

	int Size() const
	{
		return activations.m;
	}

	int Add_Activation (const bool active = true)
	{
		assert (activations.m == activation_active.m);
		activations.Append_Element (0), activation_active.Append_Element (active);
		Save_Controls();
		return activations.m;
	}

	T operator() (const int control_id) const
	{
		return activations (control_id);
	}

	T& operator() (const int control_id)
	{
		return activations (control_id);
	}

	bool Control_Active (const int control_id) const
	{
		return activation_active (control_id);
	}

	bool& Control_Active (const int control_id)
	{
		return activation_active (control_id);
	}

	bool Positions_Determined_Kinematically (const int control_id) const
	{
		return false;
	}

	void Force_Derivative (ARRAY<VECTOR_3D<T> >& dFdl, const int control_id) const
	{
		assert (muscle_force);
		assert (Control_Active (control_id));
		single_activation_used_for_force_derivative = control_id;
		ARRAY<VECTOR_3D<T> >::copy (VECTOR_3D<T>(), dFdl);
		muscle_force->Add_Velocity_Independent_Forces (dFdl);
		single_activation_used_for_force_derivative = 0;
	}

	static T Piecewise_Quadratic_Penalty (const T a, const T a_min, const T a_max)
	{
		if (a < a_min) return sqr (a - a_min);

		if (a <= a_max) return 0;

		return sqr (a - a_max);
	}
	static T Piecewise_Quadratic_Penalty_Prime (const T a, const T a_min, const T a_max)
	{
		if (a < a_min) return 2 * (a - a_min);

		if (a <= a_max) return 0;

		return 2 * (a - a_max);
	}
	static T Piecewise_Quadratic_Penalty_Double_Prime (const T a, const T a_min, const T a_max)
	{
		if (a < a_min) return 2;

		if (a <= a_max) return 0;

		return 2;
	}

	T Penalty() const
	{
		assert (activations.m == activations_save.m);
		T penalty = 0;

		for (int i = 1; i <= activations.m; i++) penalty += Piecewise_Quadratic_Penalty (activations (i), clamp<T> (activations_save (i) - max_optimization_step_length, 0, activation_cutoff),
					clamp<T> (activations_save (i) + max_optimization_step_length, 0, activation_cutoff));

		return activation_penalty_coefficient * penalty;
	}

	T Penalty_Gradient (const int control_id) const
	{
		assert (1 <= control_id && control_id <= Size());
		return activation_penalty_coefficient * Piecewise_Quadratic_Penalty_Prime (activations (control_id), clamp<T> (activations_save (control_id) - max_optimization_step_length, 0, activation_cutoff),
				clamp<T> (activations_save (control_id) + max_optimization_step_length, 0, activation_cutoff));
	}

	T Penalty_Hessian (const int control_id1, const int control_id2) const
	{
		assert (1 <= control_id1 && control_id1 <= Size() && 1 <= control_id2 && control_id2 <= Size());

		if (control_id1 != control_id2) return 0;

		return activation_penalty_coefficient * Piecewise_Quadratic_Penalty_Double_Prime (activations (control_id1), clamp<T> (activations_save (control_id1) - max_optimization_step_length, 0, activation_cutoff),
				clamp<T> (activations_save (control_id1) + max_optimization_step_length, 0, activation_cutoff));
	}

	void Save_Controls()
	{
		activations_save = activations;
	}

	void Project_Parameters_To_Allowable_Range (const bool active_controls_only)
	{
		for (int i = 1; i <= activations.m; i++) if (!active_controls_only || activation_active (i))
				activations (i) = clamp<T> (activations (i), max (activations_save (i) - max_optimization_step_length, (T) 0), min (activations_save (i) + max_optimization_step_length, activation_cutoff));
	}

	void Interpolate (const T interpolation_fraction)
	{
		for (int i = 1; i <= activations.m; i++) activations (i) = LINEAR_INTERPOLATION<T, T>::Linear (activations_save (i), activations (i), interpolation_fraction);
	}

	void Print_Diagnostics (std::ostream& output = std::cout) const
	{
		for (int i = 1; i <= activations.m; i++)
		{
			output << "Activation #" << i << " [";

			if (activation_active (i)) output << "active] : ";
			else output << "inactive] : ";

			output << activations (i) << std::endl;
		}

		output << "Penalty at current activation levels : " << Penalty() << std::endl;
	}

//#####################################################################
	void Set_Attachment_Positions (ARRAY<VECTOR_3D<T> >&X) const {}
	void Kinematically_Update_Positions (ARRAY<VECTOR_3D<T> >&X) const {}
	void Kinematically_Update_Jacobian (ARRAY<VECTOR_3D<T> >&dX) const {}
//#####################################################################
};
}
#endif