#ifndef LOCOMOTORPRIMITIVES_CONTROLLER_H_
#define LOCOMOTORPRIMITIVES_CONTROLLER_H_

#include <OpenSim/OpenSim.h>

#include "Logger.h"

class LocomotorPrimitivesController : public OpenSim::Controller {
OpenSim_DECLARE_CONCRETE_OBJECT(LocomotorPrimitivesController, OpenSim::Controller);

public:
	LocomotorPrimitivesController(OpenSim::Storage act, double alpha)
		: OpenSim::Controller(), _act(act), _alpha(alpha)
	{
		_muscle_act = new double[act.getSmallestNumberOfStates()];
	}

	~LocomotorPrimitivesController()
	{
		delete[] _muscle_act;
	}

	/**
	 * Check availability of control data for all controls in the model.
	 *
	 * @return    0 if control data is available for all controls, negative
	 *            number of missing control data vectors on error
	 */
	int checkControls();
	void computeControls(const SimTK::State &s, SimTK::Vector &controls) const;
private:
	double _alpha;
	/* used to store actuation data in compute controls */
	double *_muscle_act;

	OpenSim::Storage _act;

	double getMuscleActivation(const std::string &muscle_name) const;
};

#endif /* LOCOMOTORPRIMITIVES_CONTROLLER_H_ */