#include <OpenSim/OpenSim.h>

#include "LocomotorPrimitivesController.h"
#include "MuscleEMGProfile.h"

#define VERBOSE 1

static const double TIME_DAMP = 0.1;

#if 0
int LocomotorPrimitivesController::loadCsvData(const std::string &muscleName, const std::string &file)
{
	std::cout << ">> loading CSV data from " << file << std::endl;
	std::ifstream data(file);
	std::string line;

	std::cout << data.tellg() << std::endl;

	if (!data.is_open()) {
		std::cerr << "Error loading CSV data from " << file << std::endl;
		return -1;
	}

	MuscleEMGProfile p(muscleName, 0);

	while (std::getline(data, line)) {
		std::stringstream s(line);
		std::string cell;

		int i = 0;
		double x;
		while (std::getline(s, cell, ',')) {
			if (!isdigit(cell[0]))
				continue;

			std::cout << "read cell(" << (i == 0 ? "x" : "y") << ") -> " << cell << std::endl;
			double val = (double) atof(cell.c_str());

			if (i == 1)
				p.addData(x, val);
			else
				x = val;

			i = !i;
		}
	}

	data.close();

	//_act.push_back(p);
	std::cout << ">>> loaded EMG profile for " << p.getName() << ", " << p.getIdx() << "/" << p.getCapacity() << std::endl;

	return 0;
}
#endif

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

	/* Extract muscle activation for each of the muscles */
	int act_set_siz = getActuatorSet().getSize();
	for (int i = 0; i < act_set_siz; i++) {
		const OpenSim::Muscle *m = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get(i));
		const OpenSim::Array<int> indices = _act.getColumnIndicesForIdentifier(m->getName());

		if (indices.getSize() != 0) {
			double muscle_act = 0.0;

			_act.getDataAtTime(t, indices.get(0), &muscle_act);
			std::cout << indices.get(0) << "    actuation data for '" << m->getName() << "' found: " << muscle_act << std::endl;
		}

		//std::cout << "    " << p.getName() << "(" << p.getIdx() << "/" << p.getCapacity() << ")" << std::endl;
	}

#if 0
	//const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("bifemlh_r"));
	const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("rect_fem_r"));
	const OpenSim::Muscle *ercspn_r = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("ercspn_r"));
	const OpenSim::Muscle *ercspn_l = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("ercspn_l"));
	const OpenSim::Muscle *extobl_r = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("extobl_r"));
	const OpenSim::Muscle *extobl_l = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("extobl_l"));

	double v = rectfem->getLengtheningSpeed(s);
	double act = v * _alpha;

	if (act < 0.0)
		act = 0.0;
	if (act > 1.0)
		act = 1.0;

	if (t - last_twitch < TIME_DAMP)
		act = 0.0;

	if (act > 0.0)
		last_twitch = t;
	
	//if (VERBOSE && act > 0.0)
	//	std::cout << "(" << std::fixed << t << ") " << "v=" << std::fixed << v << ", act=" << std::fixed << act << std::endl;

	SimTK::Vector ctrl_rectfem(1, act);
	SimTK::Vector ctrl_extobl(1, 0.79);
	SimTK::Vector ctrl_ercsp(1, 0.375);

	rectfem->addInControls(ctrl_rectfem, controls);

	ercspn_r->addInControls(ctrl_ercsp, controls);
	ercspn_l->addInControls(ctrl_ercsp, controls);
	extobl_r->addInControls(ctrl_extobl, controls);
	extobl_l->addInControls(ctrl_extobl, controls);
#endif
}