/*
	Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2007
	Copyright Stichting C.A. Muller Radioastronomiestation, 2007

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <controller/controller_block.h>
#include <log/log.h>

/*
	Motor model for DC motors.
	Calculates Current, Torque and back EMF based on given Voltage and
	angular velocity.

       inputs      outputs
       nr name     nr name

     ----------------------
     |                    |
  ---| 0  V          0  I |----
     |	                  |
  ---| 1  w          1  T |----
     |	                  |
     |               2 Eb |----
     |	                  |
     ----------------------
     
 */

struct controller_block_private {
	float *V;
	float *w;
	float I;
	float T;
	float Eb;
	
	float kt;
	float ke;
	float iR;

	float R;
};

static void mmdcv_calculate(struct controller_block *mmdcv)
{
	struct controller_block_private *priv = mmdcv->private;
	float V = *priv->V;
	float w = *priv->w;
	float I, Eb, Er, T;
	float kt = priv->kt;
	float ke = priv->ke;
	float iR = priv->iR;

	Eb = w * ke;
	Er = V - Eb;
	I = (Er) * iR;
	T = I * kt;

	priv->I = I;
	priv->T = T;
	priv->Eb = Eb;
}

static int param_set_kt(struct controller_block *mmdcv, char *param, int argc,
    va_list val)
{
	mmdcv->private->kt = va_arg(val, double);
	return 0;
}

static int param_set_ke(struct controller_block *mmdcv, char *param, int argc,
    va_list val)
{
	mmdcv->private->ke = va_arg(val, double);
	return 0;
}

static int param_set_R(struct controller_block *mmdcv, char *param, int argc,
    va_list val)
{
	mmdcv->private->R = va_arg(val, double);
	mmdcv->private->iR = 1.0 / mmdcv->private->R;
	return 0;
}

static struct controller_block_param_list params[] = {
	{ "kt", false, param_set_kt, .args = { "double", NULL } },
	{ "ke", false, param_set_ke, .args = { "double", NULL } },
	{ "R",  false, param_set_R,  .args = { "double", NULL } },
	{ NULL },
};

static struct controller_block_interm_list interms[] = {
	{ "V", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, V) },
	{ "w", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, w) },
	{ NULL }
};

static struct controller_block_outterm_list outterms[] = {
	{ "I",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, I)  },
	{ "T",  CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, T)  },
	{ "Eb", CONTROLLER_BLOCK_TERM_FLOAT, offsetof(struct controller_block_private, Eb) },
	{ NULL }
};

static struct controller_block * block_motor_model_dc_v_create(char *name, int argc, va_list val)
{
	struct controller_block *mmdcv;
	
	if (!(mmdcv = controller_block_alloc("motor_model_dc_v", name, sizeof(struct controller_block_private))))
		return NULL;

	mmdcv->private->I = 0.0;
	mmdcv->private->T = 0.0;
	mmdcv->private->Eb = 0.0;
	mmdcv->private->kt = 0.0;
	mmdcv->private->ke = 0.0;
	mmdcv->private->R = 0.0;
	mmdcv->private->iR = 0.0;

	if (controller_block_interm_list_init(mmdcv, interms))
		goto err_block;

	if (controller_block_outterm_list_init(mmdcv, outterms))
		goto err_block;

	mmdcv->calculate = mmdcv_calculate;

	if (controller_block_param_list_add(mmdcv, params))
		goto err_block;

	if (controller_block_add(mmdcv))
		goto err_block;
	
	return mmdcv;

err_block:
	controller_block_free(mmdcv);
	return NULL;
}

BLOCK_CREATE(motor_model_dc_v) = {
	.create = block_motor_model_dc_v_create,
	.args = { NULL },
};