/*2011-01-03T15:22:58-01:00*/

/********************************************************************
 * plateau_tournant_cov.c
 * 
 * generated by: plcgen 2.1.201
 ********************************************************************/

#include "cb_comp.h"
#include "cb_covs.h"
#include "iec_1131.h"

#include "plateau_tournant.h"


#ifndef PI
#define PI 3.1415926535898
#endif

/**************************** Variables *****************************/

CB_Index plateau_tournant__num = 0;
CB_Mem_Float *plateau_tournant_vitesseRotation;
CB_Mem_Float *plateau_tournant_incDeplacementX;
CB_Mem_Float *plateau_tournant_incDeplacementY;
CB_Mem_Float *plateau_tournant_incDeplacementZ;
CB_Mem_Float *plateau_tournant_angleRotationRad;
CB_Mem_Float *plateau_tournant_incRotation;
CB_Mem_Float *plateau_tournant_centreX;
CB_Mem_Float *plateau_tournant_centreY;
CB_Mem_Float *plateau_tournant_centreZ;
CB_Mem_Float *plateau_tournant_angleRotationDeg;
CB_Mem_Int *plateau_tournant_status_act_prod;
CB_Mem_Float *plateau_tournant_m11;
CB_Mem_Float *plateau_tournant_m12;
CB_Mem_Float *plateau_tournant_m13;
CB_Mem_Float *plateau_tournant_m14;
CB_Mem_Float *plateau_tournant_m21;
CB_Mem_Float *plateau_tournant_m22;
CB_Mem_Float *plateau_tournant_m23;
CB_Mem_Float *plateau_tournant_m24;
CB_Mem_Float *plateau_tournant_m31;
CB_Mem_Float *plateau_tournant_m32;
CB_Mem_Float *plateau_tournant_m33;
CB_Mem_Float *plateau_tournant_m34;
CB_Mem_Float *plateau_tournant_m41;
CB_Mem_Float *plateau_tournant_m42;
CB_Mem_Float *plateau_tournant_m43;
CB_Mem_Float *plateau_tournant_m44;
CB_Mem_Float *plateau_tournant_incrementRotation;
CB_Mem_Float *plateau_tournant_positionsRecalage;
CB_Mem_Float *plateau_tournant_deltaRecalage;
CB_Mem_Bool *plateau_tournant_d_blocage;
CB_Mem_Float *plateau_tournant_v_d_blocage;
CB_Mem_Bool *plateau_tournant_d_recalage;
CB_Mem_Float *plateau_tournant_v_d_recalage;

/**************************** Variables *****************************/

#define vitesseRotation (plateau_tournant_vitesseRotation->CB_current_value)
#define incDeplacementX (plateau_tournant_incDeplacementX->CB_current_value)
#define incDeplacementY (plateau_tournant_incDeplacementY->CB_current_value)
#define incDeplacementZ (plateau_tournant_incDeplacementZ->CB_current_value)
#define angleRotationRad (plateau_tournant_angleRotationRad->CB_current_value)
#define incRotation (plateau_tournant_incRotation->CB_current_value)
#define centreX (plateau_tournant_centreX->CB_current_value)
#define centreY (plateau_tournant_centreY->CB_current_value)
#define centreZ (plateau_tournant_centreZ->CB_current_value)
#define angleRotationDeg (plateau_tournant_angleRotationDeg->CB_current_value)
#define status_act_prod (plateau_tournant_status_act_prod->CB_current_value)
#define m11 (plateau_tournant_m11->CB_current_value)
#define m12 (plateau_tournant_m12->CB_current_value)
#define m13 (plateau_tournant_m13->CB_current_value)
#define m14 (plateau_tournant_m14->CB_current_value)
#define m21 (plateau_tournant_m21->CB_current_value)
#define m22 (plateau_tournant_m22->CB_current_value)
#define m23 (plateau_tournant_m23->CB_current_value)
#define m24 (plateau_tournant_m24->CB_current_value)
#define m31 (plateau_tournant_m31->CB_current_value)
#define m32 (plateau_tournant_m32->CB_current_value)
#define m33 (plateau_tournant_m33->CB_current_value)
#define m34 (plateau_tournant_m34->CB_current_value)
#define m41 (plateau_tournant_m41->CB_current_value)
#define m42 (plateau_tournant_m42->CB_current_value)
#define m43 (plateau_tournant_m43->CB_current_value)
#define m44 (plateau_tournant_m44->CB_current_value)
#define incrementRotation (plateau_tournant_incrementRotation->CB_current_value)
#define positionsRecalage(i) (plateau_tournant_positionsRecalage[i+1].CB_current_value)
#define deltaRecalage (plateau_tournant_deltaRecalage->CB_current_value)
#define d_blocage (plateau_tournant_d_blocage->CB_current_value)
#define v_d_blocage (plateau_tournant_v_d_blocage->CB_current_value)
#define d_recalage (plateau_tournant_d_recalage->CB_current_value)
#define v_d_recalage (plateau_tournant_v_d_recalage->CB_current_value)


/************************ Components offsets ************************/


/********************* Initialization function **********************/

int _bibliotheque_epo__plateau_tournant_init(void)
{
	(self.Float+17+3)->CB_current_value = 90.0; /*positionsRecalage[2]*/
	(self.Float+17+4)->CB_current_value = 180.0; /*positionsRecalage[3]*/
	(self.Float+17+5)->CB_current_value = 270.0; /*positionsRecalage[4]*/
	(self.Float+17+6)->CB_current_value = 360.0; /*positionsRecalage[5]*/
	(self.Float+24)->CB_current_value = 10.0; /*deltaRecalage*/
	self.Bool+=2;
	self.Int+=1;
	self.Float+=27;


	return 0;
}


/************************ Behavior function *************************/

int bibliotheque_epo__plateau_tournant(void)
{
	static char *__comp_name = "bibliotheque_epo.plateau_tournant";
	if (exec_term_node(self_num)) {

		/*********************** Internals variables ************************/

		plateau_tournant_status_act_prod = self.Int+0;
		plateau_tournant_m11 = self.Float+0;
		plateau_tournant_m12 = self.Float+1;
		plateau_tournant_m13 = self.Float+2;
		plateau_tournant_m14 = self.Float+3;
		plateau_tournant_m21 = self.Float+4;
		plateau_tournant_m22 = self.Float+5;
		plateau_tournant_m23 = self.Float+6;
		plateau_tournant_m24 = self.Float+7;
		plateau_tournant_m31 = self.Float+8;
		plateau_tournant_m32 = self.Float+9;
		plateau_tournant_m33 = self.Float+10;
		plateau_tournant_m34 = self.Float+11;
		plateau_tournant_m41 = self.Float+12;
		plateau_tournant_m42 = self.Float+13;
		plateau_tournant_m43 = self.Float+14;
		plateau_tournant_m44 = self.Float+15;
		plateau_tournant_incrementRotation = self.Float+16;
		plateau_tournant_positionsRecalage = self.Float+17;
		plateau_tournant_deltaRecalage = self.Float+24;
		plateau_tournant_d_blocage = self.Bool+0;
		plateau_tournant_v_d_blocage = self.Float+25;
		plateau_tournant_d_recalage = self.Bool+1;
		plateau_tournant_v_d_recalage = self.Float+26;

		/********************* Pre-conditions execution *********************/

		/*no Pre-conditions*/

		/************************ Actor Myself init *************************/

		setMyselfFor(self_num);

		/************************ Behavior execution ************************/

		{
			/* -------------------------------------------------------------------------------------------------	*/
			/* Modèle de Comportement Opératif											*/
			/* Catégorie Acteurs													*/
			/* Techniques Nouvelles d'Informatique										*/
			/* Date de révision : Octobre 2002											*/
			/* -------------------------------------------------------------------------------------------------	*/
			/* Ce module représente l'évolution d'un Plateau Tournant							*/
			/* Il reçoit une vitesse de rotation et permet d'entrainer les acteurs/produits se trouvant		*/
			/*	en interactions												*/
			/* ------------------------------------------------------------------------------------------------	*/
			
			/* Traitement de la rotation(hauteur de la came) */
			if (d_blocage) 
				incrementRotation = 0;
			else {
				/* calcul de l'incrément de rotation en fonction de  l'horloge de simulation */
				incrementRotation = vitesseRotation * (CLOCK_PERIOD/1000000.0) ;	
			}
			
			/* Demande manuelle de recalage de la table à une position */
			if (d_recalage) {
				incrementRotation = v_d_recalage - getAngleZ(myself);
				d_recalage = 0;
			}
			
			/* recalage automatique de la table sur des positions fixes */
			if (vitesseRotation == 0.0) {
				CB_Int i;
				CB_Float angleCourant;
				angleCourant = getAngleZ(myself);
				for (i=1; i<=5; i++) {
					if ((angleCourant - deltaRecalage < positionsRecalage(i)) && (angleCourant + deltaRecalage > positionsRecalage(i))) {
						incrementRotation = positionsRecalage(i) - angleCourant;
						break;
					 }
				}
			}
			
			/*	rotation de la table			    */
			if (incrementRotation != 0.0) rotateCenterBy(myself, 0.0, 0.0, incrementRotation);
			
			/* Calcul de l'angle de rotation en Degrés et en radians */
			angleRotationDeg = getAngleZ(myself);
			angleRotationRad = angleRotationDeg * PI/180.0;
			
			
			/*	Calcule du deplacement de la table	    */
			if (incDeplacementX != 0.0) moveXBy(myself, incDeplacementX);
			if (incDeplacementY != 0.0) moveYBy(myself, incDeplacementY);
			if (incDeplacementZ != 0.0) moveZBy(myself, incDeplacementZ);
			
			/*	Calcule du centre de rotation de la table	   */
			centreX = getCenterX(myself);
			centreY = getCenterY(myself);
			centreZ = getCenterZ(myself);
			
			/*	Retour de l'angle de rotation pour les éléments entrainés en rotation	*/
			incRotation = incrementRotation;
			
			
					}

		CB_post_float(plateau_tournant_angleRotationRad);
		CB_post_float(plateau_tournant_incRotation);
		CB_post_float(plateau_tournant_centreX);
		CB_post_float(plateau_tournant_centreY);
		CB_post_float(plateau_tournant_centreZ);
		CB_post_float(plateau_tournant_angleRotationDeg);

		/******************** Post-conditions execution *********************/

		/*no Post-conditions*/
	}

	/*************************** self update ****************************/

	self.Bool += 2;
	self.Int += 1;
	self.Float += 27;

	return 0;
}

