#include "pios.h"
#include "pios_sim_priv.h"
#include "sim_model.h"

struct pios_sim_state pios_sim_state = {
	.accels = {0, 0, 0},
	.gyros = {0, 0, 0},
	.mag = {0, 0, 0},
	.baro = {0},
	.q = {1, 0, 0, 0},
	.velocity = {0, 0, 0},
	.position = {0, 0, 0},	
	.actuator = {0, 0, 0, 0, 0, 0, 0, 0}
};

/**
 * Initialize the model in the external library
 * @returns 0 for success, -1 if fails to initialize external library
 */
int PIOS_SIM_Init() 
{
	if (sim_model_init() != 0)
		return -1;
	return 0;
}

/**
 * Step the model simulation in the external library
 * @returns 0 for success, -1 for failure to step external library
 */
int PIOS_SIM_Step(float dT) 
{
	if (sim_model_step(dT, &pios_sim_state) != 0)
		return -1;

	return 0;
}

/**
 * Set the actuator inputs to the model
 * @param[in] actuator pointer to an array of actuators to set
 * @param[in] nchannels number of channels that are valid coming in
 */
void PIOS_SIM_SetActuator(float * actuator, int nchannels)
{
	for (int i = 0; i < NELEMENTS(pios_sim_state.actuator) && i < nchannels; i++)
		pios_sim_state.actuator[i] = actuator[i];
}

/**
 * Get the accelerometer data from the simulation model
 * @param[out] pointer to store the accelerometer data in
 */
void PIOS_SIM_GetAccels(float * accels)
{
	for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++)
		accels[i] = pios_sim_state.accels[i];
}

/**
 * Get the gyro data from the simulation model
 * @param[out] pointer to store the gyro data in
 */
void PIOS_SIM_GetGyros(float * gyros)
{
	for (int i = 0; i < NELEMENTS(pios_sim_state.accels); i++)
		gyros[i] = pios_sim_state.gyros[i];
}

/**
 * Get the current attitude from the simulation model
 * @param[out] quat pointer to store the quaternion attitude in
 */
void PIOS_SIM_GetAttitude(float * q)
{
	for (int i = 0; i < NELEMENTS(pios_sim_state.q); i++)
		q[i] = pios_sim_state.q[i];
}

/**
 * Get the current positiom from the simulation model
 * @param[out] position pointer to store the current position in (cm in NED
 * frame)
 */
void PIOS_SIM_GetVelocity(float * velocity)
{
	for (int i = 0; i < NELEMENTS(pios_sim_state.velocity); i++)
		velocity[i] = pios_sim_state.velocity[i];
}

/**
 * Get the current positiom from the simulation model
 * @param[out] position pointer to store the current position in (cm in NED
 * frame)
 */
void PIOS_SIM_GetPosition(float * position)
{
	for (int i = 0; i < NELEMENTS(pios_sim_state.position); i++)
		position[i] = pios_sim_state.position[i];
}