/* NAME = code.c */
/**********************************************/
/*    Extrait du code C d'évaluation de       */
/*        la loi de la puissance 1/3          */
/*        Fri 15-2014 (16.0) (Michel BRET)    */
/**********************************************/

#include "defa.h"
#include "defb.h"
#include "exta.h"
#include "extb.h"

void EXEC_LAW_SPEED_ROTA_FOLLOW(struct Objet *, float, float, float *);

long  EXEC_LAW_SPEED_ROTA(struct Objet *p_obj, long *p_law, float *pf_force_rota_add_obj, float *pf_force_rota_obj)
{
	long n, dim, i;
	long *p, *p_lead, *p_follow;
	float law0, law, coe;
	float *pf_speed_rota_obj, *pf, *pf_force_rota_add_lead, *pf_force_rota_lead;
	float *pf_speed_rota_obj0;
	struct Objet *p_obj0, *p_obj_lead, *p_obj_follow;

	pf = (float *) (p_law + DIM_TETE); law = *pf;
	coe = (p_law[2] > 1) ? pf[1] : 1.0;
	if ((p = GET_DATAS_2(p_obj->bloc, SPEED, ROTA)) == 0)
	{
		if ((p = ajou_bloc(p_obj, 9 + DIM_TETE)) == 0) goto erreur;
		SET_BLOC_2(p, SPEED, ROTA); p += p[2]; set_float_1(p, 0, 0.0);
	}
	pf_speed_rota_obj0 = (float *) (p + DIM_TETE);	/* -> speed rota obj */
	law0 = law;

	p_obj0 = p_obj;
	if (Sem_Commande[LEAD] > 0)	/* yes lead */
	{
	/* obj -> lead */
	/* ----------- */
		while (p_lead = GET_DATAS_1(p_obj->bloc, LEAD))
		{
			if ((p = GET_DATAS_2(p_obj->bloc, SPEED, ROTA)) == 0)
			{
				dim = DIM_TETE + 1;
				if ((p = ajou_bloc(p_obj, 8 + dim)) == 0) goto erreur;
				SET_BLOC_2(p, SPEED, ROTA); p += p[2]; set_float_1(p, 0, 0.0);
			}
			pf_speed_rota_obj = (float *) (p + DIM_TETE);	/* -> speed rota obj */

			if ((p_obj = get_objet_simple(Pile_Vol, p_lead, 0)) == 0)
			{
				OBJECT_NOT_TROUVE(1, VOL, p_lead, 0, 0, 0); return;
			}
			if ((p = GET_DATAS_3(p_obj->bloc, FORCE, ROTA, ADD)) == 0)
			{
				dim = DIM_TETE + 2;
				if ((p = ajou_bloc(p_obj, 10 + dim)) == 0) goto erreur;
				SET_BLOC_3(p, FORCE, ROTA, ADD); p += p[2];
				set_float_2(p, 0, 0.0, 0.0);
			}
			pf_force_rota_add_lead = (float *) (p + DIM_TETE);
			*pf_force_rota_add_lead += law * *pf_speed_rota_obj;
			law *= coe;
		}
	}
	if (Sem_Commande[FOLLOW] <= 0)	/* yes follow */
	{
	/* obj -> followers */
	/* ---------------- */
		p_obj = p_obj0; pf_speed_rota_obj = pf_speed_rota_obj0; law = law0;
		if (p_follow = GET_DATAS_1(p_obj->bloc, FOLLOW))
		{
			n = p_follow[2];
			for (i = 0; i < n; i++)
			{
				if (p_obj_follow =  get_objet_simple(Pile_Vol, p_follow, i))
					EXEC_LAW_SPEED_ROTA_FOLLOW(p_obj_follow, law, coe, pf_speed_rota_obj);
				law *= coe;
			}
		}
	}

	return(0);
erreur:
	if (Debug_SYSTEM) VOIR_CHAR("%s\n", "EXEC_LAW_SPEED_ROTA()"); return(ERR0);
}

void EXEC_LAW_SPEED_ROTA_FOLLOW(struct Objet *p_obj_follow, float law, float coe, float *pf_speed_rota_obj)
{
	long n, i;
	long *p, *p_follow;
	float *pf_force_rota_follow, *pf_force_rota_add_follow;

	if (p = GET_DATAS_3(p_obj_follow->bloc, FORCE, ROTA, ADD))
	{
		pf_force_rota_follow = (float *) (p + DIM_TETE);
		*pf_force_rota_follow += law * *pf_speed_rota_obj;
	}
	if (p = GET_DATAS_2(p_obj_follow->bloc, FORCE, ROTA))
	{
		pf_force_rota_follow = (float *) (p + DIM_TETE);
		*pf_force_rota_follow += law * *pf_speed_rota_obj;
	}
/* Followers */
	if(coe != 0.0)
	{
		if (p_follow = GET_DATAS_1(p_obj_follow->bloc, FOLLOW))
		{
			n = p_follow[2];
			for (i = 0; i < n; i++)
			{
				law *= coe;
				if (p_obj_follow =  get_objet_simple(Pile_Vol, p_follow, i))
					EXEC_LAW_SPEED_ROTA_FOLLOW(p_obj_follow, law, coe, pf_speed_rota_obj);
			}
		}
	}
}

/* END */