#include "XNGroundHandling.h"
#include "XNGroundHandling_p.h"
#include <XNCore/XNModelManager.h>
#include <XNCore/XNDDSManager.h>

XN_MODEL_INITIALIZE(XNGroundHandling)

XNGroundHandling::XNGroundHandling() : XNModelObject(new XNGroundHandlingPrivate())
{
}

XNGroundHandling::~XNGroundHandling()
{
}

XNGroundHandling::XNGroundHandling(PrivateType *p) : XNModelObject(p)
{
}

void XNGroundHandling::Initialize()
{
	T_D();
	XNModelObject::Initialize();
	if (d->_dynamicLib) {
		d->_fun = (FunctionType)dlsym(d->_dynamicLib, d->_entryPointName.c_str());
		if (!d->_fun) {
			LOG_WARNING("Failed to resolve SACSCGroundHandlingEntry");
		}
	}
	//add other initial code here
}

void XNGroundHandling::PrepareForExecute()
{
	T_D();
	XNModelObject::PrepareForExecute();
	//add your initial data code here
	d->_data.input_ground								= new input_ground_S();
	d->_data.input_ground->l_04_i_gdcomac_brake_torq_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.input_ground->l_04_i_gdcomac_brake_torq_f8[i] = new double[2];
	}
	d->_data.input_ground->l_04_i_gdcomac_gear_f8	   = new double[3];
	d->_data.input_ground->l_04_i_gdcomac_gsteer_f8	   = new double[3];
	d->_data.input_ground->l_04_i_gdcomac_tire_pres_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.input_ground->l_04_i_gdcomac_tire_pres_f8[i] = new double[2];
	}
	d->_data.input_ground->l_04_i_gdcomac_contdep_f8	= new double[7];
	d->_data.input_ground->l_04_i_gdcomac_brake_temp_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.input_ground->l_04_i_gdcomac_brake_temp_f8[i] = new double[2];
	}
	d->_data.input_ground->l_04_i_gdcomac_tire_tburst_l1 = new unsigned char *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.input_ground->l_04_i_gdcomac_tire_tburst_l1[i] = new unsigned char[2];
	}
	d->_data.input_ground->l_04_i_gdcomac_tire_tflat_l1 = new unsigned char *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.input_ground->l_04_i_gdcomac_tire_tflat_l1[i] = new unsigned char[2];
	}
	d->_data.input_ground->l_04_i_gdcomac_rcon_ci_f8  = new double[14];
	d->_data.output_ground							  = new output_ground_S();
	d->_data.output_ground->l_04_o_gdcomac_fygs_f8	  = new double[3];
	d->_data.output_ground->l_04_o_gdcomac_mzgs_f8	  = new double[3];
	d->_data.output_ground->l_04_o_gdcomac_mu_f8	  = new double[3];
	d->_data.output_ground->l_04_o_gdcomac_dstroke_f8 = new double[3];
	d->_data.output_ground->l_04_o_gdcomac_sr_f8	  = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_sr_f8[i] = new double[2];
	}
	d->_data.output_ground->l_04_o_gdcomac_sy_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_sy_f8[i] = new double[2];
	}
	d->_data.output_ground->l_04_o_gdcomac_sx_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_sx_f8[i] = new double[2];
	}
	d->_data.output_ground->l_04_o_gdcomac_xft_f8	   = new double[3];
	d->_data.output_ground->l_04_o_gdcomac_yft_f8	   = new double[3];
	d->_data.output_ground->l_04_o_gdcomac_zft_f8	   = new double[3];
	d->_data.output_ground->l_04_o_gdcomac_tire_vel_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_tire_vel_f8[i] = new double[2];
	}
	d->_data.output_ground->l_04_o_gdcomac_tire_temp_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_tire_temp_f8[i] = new double[2];
	}
	d->_data.output_ground->l_04_o_gdcomac_tire_burst_l1 = new unsigned char *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_tire_burst_l1[i] = new unsigned char[2];
	}
	d->_data.output_ground->l_04_o_gdcomac_utirew_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_utirew_f8[i] = new double[2];
	}
	d->_data.output_ground->l_04_o_gdcomac_vtirew_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_vtirew_f8[i] = new double[2];
	}
	d->_data.output_ground->l_04_o_gdcomac_whl_omega_f8 = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_whl_omega_f8[i] = new double[2];
	}
	d->_data.output_ground->l_04_o_gdcomac_dstruc_f8  = new double[6];
	d->_data.output_ground->l_04_o_gdcomac_nd_f8	  = new double[3];
	d->_data.output_ground->l_04_o_gdcomac_wor_par_f8 = new double[3];
	d->_data.output_ground->l_04_o_gdcomac_vczt_f8	  = new double *[3];
	for (int i = 0; i < 3; i++) {
		d->_data.output_ground->l_04_o_gdcomac_vczt_f8[i] = new double[2];
	}
	d->_inputInterface.Initialize(GetFramework(), GetUniqueId(), 1);
	d->_outputInterface.Initialize(GetFramework(), GetUniqueId(), 2);
	d->_heartbeatInterface.Initialize(GetFramework(), GetUniqueId(), 2);
}

void XNGroundHandling::StepUpdate()
{
	T_D();
	XNModelObject::StepUpdate();
	if (d->_fun) {
		//add your input code here
		d->_inputInterface.getData(d->_data.input_ground);
		d->_fun(&d->_data);
		d->_outputInterface.setData(d->_data.output_ground);
		d->_heartbeatInterface.setData(&d->_data);
		//add your output code here
	}
}