#include <OpenSim/OpenSim.h>

#include "LocomotorPrimitivesController.h"

static const double TIME_DAMP = 0.1;
static const unsigned int N_PRINT = 1000;

static Logger &logger = Logger::getInstance();

int LocomotorPrimitivesController::checkControls()
{
	const int act_set_siz = getActuatorSet().getSize();
	int ret = 0;

	/* Iterate over all controls in the model and check whether they're
	 * available in the loaded storage */
	for (int i = 0; i < act_set_siz; i++) {
		const OpenSim::Muscle *muscle = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get(i));
		const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(muscle->getName());

		if (indices.getSize() == 0) {
			logger.err("no actuation data for muscle '%s' found\n", muscle->getName().c_str());
			ret--;
		}
	}

	return ret;
}

void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK::Vector &controls) const
{
	double t = s.getTime();
	static double last_twitch = t;
	static unsigned int n = 0;

	/* Extract muscle activation for each of the muscles */
	const int act_set_siz = getActuatorSet().getSize();
	const int nc = _act.getSmallestNumberOfStates();

	/* Get the first nc states at a specified time (_NOT_ the state at nc as
	 * with getData()). */
	_act.getDataAtTime(t, nc, _muscle_act);

	for (int i = 0; i < act_set_siz; i++) {
		const OpenSim::Muscle *muscle = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get(i));
		const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(muscle->getName());
		double mact;

		if (indices.getSize() != 0) {
			int idx = indices.get(0) - 1;
			if (n % N_PRINT == 0)
				logger.log("%f (%d) actuation data for muscle '%s' found: %f\n",
						t, idx, muscle->getName().c_str(), _muscle_act[idx]);

			if (_muscle_act[idx] > 1.0) {
				logger.err("muscle actuation larger than 1.0 (%f), truncating to 1.0\n", _muscle_act[idx]);
				_muscle_act[idx] = 1.0;
			}

			mact = _muscle_act[idx];
		} else {
			// no actuation data for muscle at that time, so set it to 0.0
			mact = 0.0;
		}

		SimTK::Vector ctrl(1, mact);
		muscle->addInControls(ctrl, controls);
	}

	n++;
}