/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void m_frequ(scicos_block *block, int flag)
{
double *mat = NULL;
double *Dt = NULL;
double *off = NULL;
SCSINT32_COP *icount = NULL;
double t = 0.0;
time_counter_t** work = (time_counter_t**) block->work;
time_counter_t *counter = NULL;
int m = 0;
mat = GetRealOparPtrs(block, 1);
Dt = GetRealOparPtrs(block, 2);
off = GetRealOparPtrs(block, 3);
icount = Getint32OparPtrs(block, 4);
m = GetOparSize(block, 1, 1);
switch (flag)
{
case 4 : /* the workspace is used to store discrete counter value */
{
if ((*work = (time_counter_t*) scicos_malloc(sizeof(time_counter_t) * 2)) == NULL)
{
set_block_error(-16);
return;
}
counter = *work;
*counter = *icount;
(*(counter + 1)) = 0;
break;
}
/* event date computation */
case 3 :
{
counter = *work;
t = get_scicos_time();
*counter += (int)mat[*(counter + 1)]; /*increase counter*/
block->evout[(int)mat[*(counter + 1) + m] - 1] = *off + ((double) * counter / (*Dt)) - t;
(*(counter + 1))++;
*(counter + 1) = *(counter + 1) % m;
break;
}
/* finish */
case 5 :
{
scicos_free(*work); /*free the workspace*/
break;
}
default :
break;
}
}
/* Copyright INRIA
* Scicos block simulator
* event delay with discrete counter
*/
SCICOS_BLOCKS_IMPEXP void evtdly4(scicos_block *block, int flag)
{
double t = 0.;
time_counter_t** work = (time_counter_t**) block->work;
time_counter_t* i = NULL;
switch (flag)
{
/* init */
case 4 : /* the workspace is used to store discrete counter value */
{
if ((*work = (time_counter_t*) scicos_malloc(sizeof(time_counter_t))) == NULL)
{
set_block_error(-16);
return;
}
i = *work;
(*i) = 0;
break;
}
/* event date computation */
case 3 :
{
double dt;
i = *work;
t = get_scicos_time();
(*i)++; /*increase counter*/
dt = block->rpar[1] + (*i) * block->rpar[0] - t;
/* on event enabled, use the default delay if not scheduled */
if (block->rpar[1] >= 0 && dt < 0)
{
dt = block->rpar[0];
}
block->evout[0] = dt;
break;
}
/* finish */
case 5 :
{
scicos_free(*work); /*free the workspace*/
break;
}
default :
break;
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void backlash(scicos_block *block,int flag)
{
double* rw = NULL,t = 0.;
if (flag == 4){/* the workspace is used to store previous values */
if ((*block->work= scicos_malloc(sizeof(double)* 4))== NULL ) {
set_block_error(-16);
return;
}
rw=*block->work;
t=get_scicos_time();
rw[0]=t;
rw[1]=t;
rw[2]=block->rpar[0];
rw[3]=block->rpar[0];
}else if (flag == 5){
scicos_free(*block->work);
}else if (flag == 1) {
rw=*block->work;
t=get_scicos_time();
if(t>rw[1]) {
rw[0]=rw[1];
rw[2]=rw[3];
}
rw[1]=t;
if(block->inptr[0][0]>rw[2]+block->rpar[1]/2){
rw[3]=block->inptr[0][0]-block->rpar[1]/2;
} else if (block->inptr[0][0]<rw[2]-block->rpar[1]/2){
rw[3]=block->inptr[0][0]+block->rpar[1]/2;
} else {
rw[3]=rw[2];
}
block->outptr[0][0]=rw[3];
} else if (flag == 9) {
rw=*block->work;
t=get_scicos_time();
if(t>rw[1]){
block->g[0] = block->inptr[0][0]-block->rpar[1]/2-rw[3];
block->g[1] = block->inptr[0][0]+block->rpar[1]/2-rw[3];
}else{
block->g[0] = block->inptr[0][0]-block->rpar[1]/2-rw[2];
block->g[1] = block->inptr[0][0]+block->rpar[1]/2-rw[2];
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_lu(scicos_block *block, int flag)
{
double *u = NULL;
double *y1 = NULL;
double *y2 = NULL;
int mu = 0;
int nu = 0;
int info = 0;
int i = 0, j = 0, l = 0, ij = 0, ik = 0;
mat_lu_struct *ptr = NULL;
mu = GetInPortRows(block, 1);
nu = GetInPortCols(block, 1);
u = GetRealInPortPtrs(block, 1);
y1 = GetRealOutPortPtrs(block, 1);
y2 = GetRealOutPortPtrs(block, 2);
l = Min(mu, nu);
/*init : initialization*/
if (flag == 4)
{
if ((*(block->work) = (mat_lu_struct*) scicos_malloc(sizeof(mat_lu_struct))) == NULL)
{
set_block_error(-16);
return;
}
ptr = *(block->work);
if ((ptr->ipiv = (int*) scicos_malloc(sizeof(int) * nu)) == NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if ((ptr->dwork = (double*) scicos_malloc(sizeof(double) * (mu * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if ((ptr->IL = (double*) scicos_malloc(sizeof(double) * (mu * l))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if ((ptr->IU = (double*) scicos_malloc(sizeof(double) * (l * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->IL);
scicos_free(ptr->dwork);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag == 5)
{
ptr = *(block->work);
if ((ptr->IU) != NULL)
{
scicos_free(ptr->ipiv);
scicos_free(ptr->dwork);
scicos_free(ptr->IL);
scicos_free(ptr->IU);
scicos_free(ptr);
return;
}
}
else
{
ptr = *(block->work);
for (i = 0; i < (mu * nu); i++)
{
ptr->dwork[i] = u[i];
}
C2F(dgetrf)(&mu, &nu, ptr->dwork, &mu, ptr->ipiv, &info);
if (info != 0)
{
if (flag != 6)
{
set_block_error(-7);
return;
}
}
for (j = 0; j < l; j++)
{
for (i = 0; i < mu; i++)
{
ij = i + j * mu;
if (i == j)
{
*(y2 + ij) = 1;
}
else if (i > j)
{
*(y2 + ij) = *(ptr->dwork + ij);
}
else
{
*(y2 + ij) = 0;
}
}
}
for (j = 0; j < nu; j++)
{
for (i = 0; i < l; i++)
{
ij = i + j * l;
ik = i + j * mu;
if (i <= j)
{
*(y1 + ij) = *(ptr->dwork + ik);
}
else
{
*(y1 + ij) = 0;
}
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matz_inv(scicos_block *block,int flag)
{
double *ur = NULL;
double *yr = NULL;
double *ui = NULL;
double *yi = NULL;
int nu = 0;
int info = 0;
int i = 0;
mat_inv_struct *ptr = NULL;
nu =GetInPortRows(block,1);
ur=GetRealInPortPtrs(block,1);
ui=GetImagInPortPtrs(block,1);
yr=GetRealOutPortPtrs(block,1);
yi=GetImagOutPortPtrs(block,1);
/*init : initialization*/
if (flag==4)
{
if((*(block->work)=(mat_inv_struct*) scicos_malloc(sizeof(mat_inv_struct)))==NULL)
{
set_block_error(-16);
return;
}
ptr=*(block->work);
if((ptr->ipiv=(int*) scicos_malloc(sizeof(int)*nu))==NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if((ptr->wrk=(double*) scicos_malloc(sizeof(double)*(2*nu*nu)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->LX=(double*) scicos_malloc(sizeof(double)*(2*nu*nu)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->LX);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag==5)
{
ptr=*(block->work);
if((ptr->LX)!=NULL)
{
scicos_free(ptr->ipiv);
scicos_free(ptr->LX);
scicos_free(ptr->wrk);
scicos_free(ptr);
return;
}
}
else
{
ptr=*(block->work);
for (i=0;i<(nu*nu);i++)
{
ptr->LX[2*i]=ur[i];
ptr->LX[2*i+1]=ui[i];
}
C2F(zgetrf)(&nu,&nu,ptr->LX,&nu,ptr->ipiv,&info);
if (info !=0)
{
if (flag!=6)
{set_block_error(-7);
return;
}
}
C2F(zgetri)(&nu,ptr->LX,&nu,ptr->ipiv,ptr->wrk,&nu,&info);
for (i=0;i<(nu*nu);i++)
{
yr[i]=ptr->LX[2*i];
yi[i]=ptr->LX[2*i+1];
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_vpv(scicos_block *block,int flag)
{
double *u = NULL;
double *y1 = NULL;
double *y2 = NULL;
double *y3 = NULL;
double *y4 = NULL;
int nu = 0;
int info = 0;
int i = 0,j = 0,lwork = 0,lwork1 = 0,ii = 0,ij = 0,ij1 = 0,ji = 0;
int symmetric = 0;
mat_vps_struct *ptr = NULL;
nu =GetInPortRows(block,1);
u=GetRealInPortPtrs(block,1);
y1=GetRealOutPortPtrs(block,1);
y2=GetImagOutPortPtrs(block,1);
y3=GetRealOutPortPtrs(block,2);
y4=GetImagOutPortPtrs(block,2);
lwork1=4*nu;
lwork=3*nu-1;
/*init : initialization*/
if (flag==4)
{if((*(block->work)=(mat_vps_struct*) scicos_malloc(sizeof(mat_vps_struct)))==NULL)
{set_block_error(-16);
return;}
ptr=*(block->work);
if((ptr->LA=(double*) scicos_malloc(sizeof(double)*(nu*nu)))==NULL)
{set_block_error(-16);
scicos_free(ptr);
return;}
if((ptr->L0=(double*) scicos_malloc(sizeof(double)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->LVR=(double*) scicos_malloc(sizeof(double)*(nu*nu)))==NULL)
{set_block_error(-16);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->LW=(double*) scicos_malloc(sizeof(double)*(nu*nu)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->LWR=(double*) scicos_malloc(sizeof(double)*(nu*1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LW);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->LWI=(double*) scicos_malloc(sizeof(double)*(nu*1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LWR);
scicos_free(ptr->LW);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->dwork=(double*) scicos_malloc(sizeof(double)*lwork))==NULL)
{set_block_error(-16);
scicos_free(ptr->LWI);
scicos_free(ptr->LWR);
scicos_free(ptr->LW);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->dwork1=(double*) scicos_malloc(sizeof(double)*lwork1))==NULL)
{set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->LWI);
scicos_free(ptr->LWR);
scicos_free(ptr->LW);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
}
/* Terminaison */
else if (flag==5)
{ptr=*(block->work);
if(ptr->dwork1!=NULL){
scicos_free(ptr->LA);
scicos_free(ptr->L0);
scicos_free(ptr->LVR);
scicos_free(ptr->LW);
scicos_free(ptr->LWI);
scicos_free(ptr->LWR);
scicos_free(ptr->dwork);
scicos_free(ptr->dwork1);
scicos_free(ptr);
return;}
}
else
{
ptr=*(block->work);
C2F(dlacpy)("F",&nu,&nu,u,&nu,ptr->LA,&nu);
symmetric=1;
for (j=0;j<nu;j++)
{for (i=j;i<nu;i++)
{ij=i+j*nu;
ji=j+i*nu;
if (i!=j)
{if (*(ptr->LA+ij)==*(ptr->LA+ji)) symmetric*= 1;
else { symmetric*=0;break;}}}}
if (symmetric==1)
{C2F(dsyev)("V","U",&nu,ptr->LA,&nu,ptr->LW,ptr->dwork,&lwork,&info);
if (info!=0)
{if (flag!=6)
{set_block_error(-7);
return;
}}
for (i=0;i<nu;i++)
{ii=i+i*nu;
*(y1+ii)=*(ptr->LW+i);
}
C2F(dlacpy)("F",&nu,&nu,ptr->LA,&nu,y3,&nu);
}
else
{C2F(dgeev)("N","V",&nu,ptr->LA,&nu,ptr->LWR,ptr->LWI,ptr->dwork1,&nu,ptr->LVR,&nu,ptr->dwork1,&lwork1,&info);
if (info!=0)
{if (flag!=6)
{set_block_error(-7);
return;
}}
*(ptr->L0)=0;
C2F(dlaset)("F",&nu,&nu,ptr->L0,ptr->L0,y1,&nu);
C2F(dlaset)("F",&nu,&nu,ptr->L0,ptr->L0,y2,&nu);
for (i=0;i<nu;i++)
{ii=i+i*nu;
*(y1+ii)=*(ptr->LWR+i);
*(y2+ii)=*(ptr->LWI+i);
}
for (j=0;j<nu;j++)
{if (*(ptr->LWI+j)==0)
for (i=0;i<nu;i++)
{ij=i+(j)*nu;
*(y3+ij)=*(ptr->LVR +ij);
*(y4+ij)=0;}
else{
for (i=0;i<nu;i++)
{ij=i+(j)*nu;
ij1=i+(j+1)*nu;
*(y3+ij)=*(ptr->LVR +ij);
*(y4+ij)=*(ptr->LVR +ij1);
*(y3+ij1)=*(ptr->LVR +ij);
*(y4+ij1)=-(*(ptr->LVR +ij1));}
j++;}
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matz_bksl(scicos_block *block,int flag)
{
double *u1r = NULL,*u1i = NULL;
double *u2r = NULL,*u2i = NULL;
double *yr = NULL,*yi = NULL;
int mu = 0,vu = 0,wu = 0;
int nu1 = 0;
int nu2 = 0;
int info = 0;
int i = 0,j = 0,l = 0,lw = 0,lu = 0,rw = 0,ij = 0,k = 0;
mat_bksl_struct *ptr = NULL;
double rcond = 0., ANORM = 0., EPS = 0.;
vu =GetOutPortRows(block,1);
wu =GetOutPortCols(block,1);
mu =GetInPortRows(block,1);
nu1 =GetInPortCols(block,1);
nu2 =GetInPortCols(block,2);
u1r=GetRealInPortPtrs(block,1);
u1i=GetImagInPortPtrs(block,1);
u2r=GetRealInPortPtrs(block,2);
u2i=GetImagInPortPtrs(block,2);
yr=GetRealOutPortPtrs(block,1);
yi=GetImagOutPortPtrs(block,1);
l=Max(mu,nu1);
lw=Max(2*Min(mu,nu1),nu1+1);
lu=Max(lw,Min(mu,nu1)+nu2);
lw=Max(2*nu1,Min(mu,nu1)+lu);
rw=2*nu1;
/*init : initialization*/
if (flag==4)
{
if((*(block->work)=(mat_bksl_struct*) scicos_malloc(sizeof(mat_bksl_struct)))==NULL)
{
set_block_error(-16);
return;
}
ptr=*(block->work);
if((ptr->ipiv=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if((ptr->rank=(int*) scicos_malloc(sizeof(int)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->jpvt=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{
set_block_error(-16);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->iwork=(double*) scicos_malloc(sizeof(double)*2*nu1))==NULL)
{set_block_error(-16);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->dwork=(double*) scicos_malloc(sizeof(double)*2*lw))==NULL)
{
set_block_error(-16);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->IN1F=(double*) scicos_malloc(sizeof(double)*(2*mu*nu1)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->IN1=(double*) scicos_malloc(sizeof(double)*(2*mu*nu1)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->IN1F);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->IN2X=(double*) scicos_malloc(sizeof(double)*(2*l*nu2)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->IN1);
scicos_free(ptr->IN1F);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->IN2=(double*) scicos_malloc(sizeof(double)*(2*mu*nu2)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->IN2);
scicos_free(ptr->IN1);
scicos_free(ptr->IN1F);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag==5)
{
ptr=*(block->work);
if((ptr->IN2)!=NULL)
{
scicos_free(ptr->ipiv);
scicos_free(ptr->rank);
scicos_free(ptr->jpvt);
scicos_free(ptr->iwork);
scicos_free(ptr->IN1F);
scicos_free(ptr->IN1);
scicos_free(ptr->IN2X);
scicos_free(ptr->IN2);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;
}
}
else
{
ptr=*(block->work);
for (i=0;i<(mu*nu1);i++)
{
ptr->IN1[2*i]=u1r[i];
ptr->IN1[2*i+1]=u1i[i];
}
for (i=0;i<(mu*nu2);i++)
{
ptr->IN2[2*i]=u2r[i];
ptr->IN2[2*i+1]=u2i[i];
}
EPS=C2F(dlamch)("e",1L);
ANORM=C2F(zlange)("1",&mu,&nu1,ptr->IN1,&mu,ptr->dwork);
if (mu==nu1)
{
C2F(zlacpy)("F",&mu,&nu1,ptr->IN1,&mu,ptr->IN1F,&mu);
C2F(zgetrf)(&nu1,&nu1,ptr->IN1F,&nu1,ptr->ipiv,&info);
rcond=0;
if (info==0)
{
C2F(zgecon)("1",&nu1,ptr->IN1F,&nu1,&ANORM,&rcond,ptr->dwork,ptr->iwork,&info);
if (rcond>pow(EPS,0.5))
{
C2F(zgetrs)("N",&nu1,&nu2,ptr->IN1F,&nu1,ptr->ipiv,ptr->IN2,&nu1,&info);
for (i=0;i<(mu*nu2);i++)
{*(yr+i)=*(ptr->IN2+2*i);
*(yi+i)=*(ptr->IN2+(2*i)+1);}
return;
}
}
}
rcond=pow(EPS,0.5);
for (i=0;i<nu1;i++) *(ptr->jpvt+i)=0;
C2F(zlacpy)("F",&mu,&nu2,ptr->IN2,&mu,ptr->IN2X,&l);
C2F(zgelsy1)(&mu,&nu1,&nu2,ptr->IN1,&mu,ptr->IN2X,&l,ptr->jpvt,&rcond,ptr->rank,ptr->dwork,&lw,ptr->iwork,&info);
if (info!=0)
{
if (flag!=6)
{
set_block_error(-7);
return;
}
}
k=0;
for(j=0;j<wu;j++)
{
for(i=0;i<vu;i++)
{
ij=i+j*l;
*(yr+k)=*(ptr->IN2X+2*ij);
*(yi+k)=*(ptr->IN2X+(2*ij)+1);
k++;
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void time_delay(scicos_block *block, int flag)
{
/* rpar[0]=delay, rpar[1]=init value, ipar[0]=buffer length */
double** work = (double**) block->work;
double *pw = NULL, del = 0., t = 0., td = 0., eps = 0.;
int* iw = NULL;
int i = 0, j = 0, k = 0;
if (flag == 4)
{
/* the workspace is used to store previous values */
if ((*work = (double*) scicos_malloc(sizeof(int) + sizeof(double) *
block->ipar[0] * (1 + block->insz[0]))) == NULL )
{
set_block_error(-16);
return;
}
eps = 1.0e-9; /* shift times to left to avoid replacing 0 */
pw = *work;
pw[0] = -block->rpar[0] * (block->ipar[0] - 1) - eps;
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[block->ipar[0]*j] = block->rpar[1];
}
for (i = 1; i < block->ipar[0]; i++)
{
pw[i] = pw[i - 1] + block->rpar[0] - eps;
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[i + block->ipar[0]*j] = block->rpar[1];
}
}
iw = (int *)(pw + block->ipar[0] * (1 + block->insz[0]));
*iw = 0;
for (k = 0; k < block->insz[0]; k++)
{
block->outptr[0][k] = block->rpar[1];
}
}
else if (flag == 5)
{
scicos_free(*work);
}
else if (flag == 0 || flag == 2)
{
if (flag == 2)
{
do_cold_restart();
}
pw = *work;
iw = (int *)(pw + block->ipar[0] * (1 + block->insz[0]));
t = get_scicos_time();
td = t - block->rpar[0];
if (td < pw[*iw])
{
scicos_print(_("delayed time=%f but last stored time=%f \n"), td, pw[*iw]);
scicos_print(_("Consider increasing the length of buffer in delay block \n"));
}
if (t > pw[(block->ipar[0] + *iw - 1) % block->ipar[0]])
{
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[*iw + block->ipar[0]*j] = block->inptr[0][j - 1];
}
pw[*iw] = t;
/*scicos_print(_("**time is %f. I put %f, in %d \n"), t,block->inptr[0][0],*iw);*/
*iw = (*iw + 1) % block->ipar[0];
}
else
{
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[(block->ipar[0] + *iw - 1) % block->ipar[0] + block->ipar[0]*j] = block->inptr[0][j - 1];
}
pw[(block->ipar[0] + *iw - 1) % block->ipar[0]] = t;
/*scicos_print("**time is %f. I put %f, in %d \n", t,block->inptr[0][0],*iw);*/
}
}
else if (flag == 1)
{
pw = *work;
iw = (int *) (pw + block->ipar[0] * (1 + block->insz[0]));
t = get_scicos_time();
td = t - block->rpar[0];
i = 0;
j = block->ipar[0] - 1;
while (j - i > 1)
{
k = (i + j) / 2;
if (td < pw[(k + *iw) % block->ipar[0]])
{
j = k;
}
else if (td > pw[(k + *iw) % block->ipar[0]])
{
i = k;
}
else
{
i = k;
j = k;
break;
}
}
i = (i + *iw) % block->ipar[0];
j = (j + *iw) % block->ipar[0];
del = pw[j] - pw[i];
/* scicos_print(_("time is %f. interpolating %d and %d, i.e. %f, %f\n"), t,i,j,pw[i],pw[j]);
scicos_print(_("values are %f %f.\n"),pw[i+block->ipar[0]],pw[j+block->ipar[0]]);*/
if (del != 0.0)
{
for (k = 1; k < block->insz[0] + 1; k++)
{
block->outptr[0][k - 1] = ((pw[j] - td) * pw[i + block->ipar[0] * k] +
(td - pw[i]) * pw[j + block->ipar[0] * k]) / del;
}
}
else
{
for (k = 1; k < block->insz[0] + 1; k++)
{
block->outptr[0][k - 1] = pw[i + block->ipar[0] * k];
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_div(scicos_block *block,int flag)
{
double *u1 = NULL;
double *u2 = NULL;
double *y = NULL;
int mu1 = 0,mu2 = 0;
int nu = 0,nu2 = 0;
int info = 0;
int i = 0,j = 0,l = 0,lw = 0,lu = 0,ij = 0,ji = 0;
mat_div_struct *ptr = NULL;
double rcond = 0., ANORM = 0., EPS = 0.;
mu2 =GetInPortRows(block,1);
nu =GetInPortCols(block,1);
mu1 =GetInPortRows(block,2);
nu2 =GetInPortCols(block,2);
u2=GetRealInPortPtrs(block,1);
u1=GetRealInPortPtrs(block,2);
y=GetRealOutPortPtrs(block,1);
l=Max(mu1,nu);
lu=Max(4*nu,Min(mu1,nu)+3*mu1+1);
lw=Max(lu,2*Min(mu1,nu)+mu2);
/*init : initialization*/
if (flag==4)
{if((*(block->work)=(mat_div_struct*) scicos_malloc(sizeof(mat_div_struct)))==NULL)
{set_block_error(-16);
return;}
ptr=*(block->work);
if((ptr->ipiv=(int*) scicos_malloc(sizeof(int)*nu))==NULL)
{set_block_error(-16);
scicos_free(ptr);
return;}
if((ptr->rank=(int*) scicos_malloc(sizeof(int)))==NULL)
{set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->jpvt=(int*) scicos_malloc(sizeof(int)*mu1))==NULL)
{set_block_error(-16);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->iwork=(int*) scicos_malloc(sizeof(int)*nu))==NULL)
{set_block_error(-16);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->dwork=(double*) scicos_malloc(sizeof(double)*lw))==NULL)
{set_block_error(-16);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->LAF=(double*) scicos_malloc(sizeof(double)*(nu*mu1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->LBT=(double*) scicos_malloc(sizeof(double)*(l*mu2)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LAF);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->LAT=(double*) scicos_malloc(sizeof(double)*(nu*mu1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LBT);
scicos_free(ptr->LAF);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
}
/* Terminaison */
else if (flag==5)
{ptr=*(block->work);
if((ptr->LAT)!=NULL) {
scicos_free(ptr->ipiv);
scicos_free(ptr->rank);
scicos_free(ptr->jpvt);
scicos_free(ptr->iwork);
scicos_free(ptr->LAF);
scicos_free(ptr->LBT);
scicos_free(ptr->LAT);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;}
}
else
{
ptr=*(block->work);
EPS=C2F(dlamch)("e",1L);
ANORM=C2F(dlange)("l",&mu1,&nu,u1,&mu1,ptr->dwork);
for (j=0;j<mu1;j++)
{for (i=0;i<nu;i++)
{ij=i+j*nu;
ji=j+i*mu1;
*(ptr->LAT+ij)=*(u1+ji);}
}
for (j=0;j<mu2;j++)
{for (i=0;i<nu;i++)
{ij=i+j*l;
ji=j+i*mu2;
*(ptr->LBT+ij)=*(u2+ji);}
}
if (mu1==nu)
{C2F(dlacpy)("F",&nu,&nu,ptr->LAT,&nu,ptr->LAF,&nu);
C2F(dgetrf)(&nu,&nu,ptr->LAF,&nu,ptr->ipiv,&info);
rcond=0;
if (info==0)
{C2F(dgecon)("1",&nu,ptr->LAF,&nu,&ANORM,&rcond,ptr->dwork,ptr->iwork,&info);
if (rcond>pow(EPS,0.5))
{C2F(dgetrs)("N",&nu,&mu2,ptr->LAF,&nu,ptr->ipiv,ptr->LBT,&nu,&info);
for (j=0;j<nu;j++)
{for (i=0;i<mu2;i++)
{ij=i+j*mu2;
ji=j+i*nu;
*(y+ij)=*(ptr->LBT+ji);}
}
return;
}
}
}
rcond=pow(EPS,0.5);
for (i=0;i<mu1;i++) *(ptr->jpvt+i)=0;
C2F(dgelsy1)(&nu,&mu1,&mu2,ptr->LAT,&nu,ptr->LBT,&l,ptr->jpvt,&rcond,ptr->rank,ptr->dwork,&lw,&info);
if (info!=0)
{if (flag!=6)
{set_block_error(-7);
return;
}
}
for (j=0;j<mu1;j++)
{for (i=0;i<mu2;i++)
{ij=i+j*mu2;
ji=j+i*l;
*(y+ij)=*(ptr->LBT+ji);}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_inv(scicos_block * block, int flag)
{
double *u = NULL;
double *y = NULL;
int nu = 0;
int info = 0;
int i = 0;
mat_inv_struct** work = (mat_inv_struct**) block->work;
mat_inv_struct *ptr = NULL;
nu = GetInPortRows(block, 1);
u = GetRealInPortPtrs(block, 1);
y = GetRealOutPortPtrs(block, 1);
/*init : initialization */
if (flag == 4)
{
if ((*work = (mat_inv_struct *) scicos_malloc(sizeof(mat_inv_struct))) == NULL)
{
set_block_error(-16);
return;
}
ptr = *work;
if ((ptr->ipiv = (int *)scicos_malloc(sizeof(int) * nu)) == NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if ((ptr->dwork = (double *)scicos_malloc(sizeof(double) * nu)) == NULL)
{
set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag == 5)
{
ptr = *work;
if ((ptr->dwork) != NULL)
{
scicos_free(ptr->ipiv);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;
}
}
else
{
ptr = *work;
for (i = 0; i < (nu * nu); i++)
{
y[i] = u[i];
}
C2F(dgetrf) (&nu, &nu, &y[0], &nu, ptr->ipiv, &info);
if (info != 0)
{
if (flag != 6)
{
Coserror(_("The LU factorization has been completed, but the factor U is exactly singular : U(%d,%d) is exactly zero."), info, info);
return;
}
}
C2F(dgetri) (&nu, y, &nu, ptr->ipiv, ptr->dwork, &nu, &info);
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void ricc_m(scicos_block *block, int flag)
{
double *u1 = NULL;
double *u2 = NULL;
double *u3 = NULL;
double *y = NULL;
int *ipar = NULL;
int nu = 0;
int info = 0, i = 0;
int lw = 0;
ricc_struct *ptr = NULL;
nu = GetInPortCols(block, 1);
u1 = GetRealInPortPtrs(block, 1);
u2 = GetRealInPortPtrs(block, 2);
u3 = GetRealInPortPtrs(block, 3);
y = GetRealOutPortPtrs(block, 1);
ipar = GetIparPtrs(block);
if (ipar[0] == 1)
{
if (ipar[1] == 1)
{
lw = 9 * nu * nu + 4 * nu + Max(1, 6 * nu);
}
else
{
lw = 9 * nu * nu + 7 * nu + 1;
}
}
else
{
if (ipar[1] == 1)
{
lw = 12 * nu * nu + 22 * nu + Max(21, 4 * nu);
}
else
{
lw = 28 * nu * nu + 2 * nu + Max(1, 2 * nu);
}
}
/*init : initialization*/
if (flag == 4)
{
if ((*(block->work) = (ricc_struct*) scicos_malloc(sizeof(ricc_struct))) == NULL)
{
set_block_error(-16);
return;
}
ptr = *(block->work);
if ((ptr->bwork = (int*) scicos_malloc(sizeof(int) * 2 * nu)) == NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if ((ptr->iwork = (int*) scicos_malloc(sizeof(int) * Max(2 * nu, nu * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->bwork);
scicos_free(ptr);
return;
}
if ((ptr->dwork = (double*) scicos_malloc(sizeof(double) * lw)) == NULL)
{
set_block_error(-16);
scicos_free(ptr->iwork);
scicos_free(ptr->bwork);
scicos_free(ptr);
return;
}
if ((ptr->LWR = (double*) scicos_malloc(sizeof(double) * nu)) == NULL)
{
set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->bwork);
scicos_free(ptr);
return;
}
if ((ptr->LWI = (double*) scicos_malloc(sizeof(double) * nu)) == NULL)
{
set_block_error(-16);
scicos_free(ptr->LWR);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->bwork);
scicos_free(ptr);
return;
}
if ((ptr->Rcond = (double*) scicos_malloc(sizeof(double))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->LWI);
scicos_free(ptr->LWR);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->bwork);
scicos_free(ptr);
return;
}
if ((ptr->Ferr = (double*) scicos_malloc(sizeof(double))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->Rcond);
scicos_free(ptr->LWI);
scicos_free(ptr->LWR);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->bwork);
scicos_free(ptr);
return;
}
if ((ptr->LX = (double*) scicos_malloc(sizeof(double) * nu * nu)) == NULL)
{
set_block_error(-16);
scicos_free(ptr->Ferr);
scicos_free(ptr->Rcond);
scicos_free(ptr->LWI);
scicos_free(ptr->LWR);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->bwork);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag == 5)
{
ptr = *(block->work);
if ((ptr->LX) != NULL)
{
scicos_free(ptr->bwork);
scicos_free(ptr->Ferr);
scicos_free(ptr->Rcond);
scicos_free(ptr->iwork);
scicos_free(ptr->LWR);
scicos_free(ptr->LWI);
scicos_free(ptr->LX);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;
}
}
else
{
ptr = *(block->work);
if (ipar[0] == 1)
{
if (ipar[1] == 1)
{
C2F(riccsl)("N", &nu, u1, &nu, "U", u3, &nu, u2, &nu, ptr->LX, &nu, ptr->LWR, ptr->LWI, ptr->Rcond, ptr->Ferr, ptr->dwork, &lw, ptr->iwork, ptr->bwork, &info);
}
else
{
C2F(riccms)("N", &nu, u1, &nu, "U", u3, &nu, u2, &nu, ptr->LX, &nu, ptr->LWR, ptr->LWI, ptr->Rcond, ptr->Ferr, ptr->dwork, &lw, ptr->iwork, &info);
}
}
else
{
if (ipar[1] == 1)
{
C2F(ricdsl)("N", &nu, u1, &nu, "U", u3, &nu, u2, &nu, ptr->LX, &nu, ptr->LWR, ptr->LWI, ptr->Rcond, ptr->Ferr, ptr->dwork, &lw, ptr->iwork, ptr->bwork, &info);
}
else
{
C2F(ricdmf)("N", &nu, u1, &nu, "U", u3, &nu, u2, &nu, ptr->LX, &nu, ptr->LWR, ptr->LWI, ptr->Rcond, ptr->Ferr, ptr->dwork, &lw, ptr->iwork, &info);
}
}
if (info != 0)
{
if (flag != 6)
{
set_block_error(-7);
return;
}
}
for (i = 0; i < nu * nu; i++)
{
*(y + i) = *(ptr->LX + i);
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matz_vpv(scicos_block *block,int flag)
{
double *ur = NULL,*ui = NULL;
double *y1r = NULL,*y1i = NULL,*y2r = NULL,*y2i = NULL;
int nu = 0;
int info = 0;
int i = 0,lwork = 0,lwork1 = 0,j = 0,ii = 0,ij = 0,ji = 0,rw = 0;
int hermitien = 0;
double l0 = 0.;
mat_vpv_struct *ptr = NULL;
nu =GetInPortRows(block,1);
ur=GetRealInPortPtrs(block,1);
ui=GetImagInPortPtrs(block,1);
y1r=GetRealOutPortPtrs(block,1);
y1i=GetImagOutPortPtrs(block,1);
y2r=GetRealOutPortPtrs(block,2);
y2i=GetImagOutPortPtrs(block,2);
lwork1=2*nu;
lwork=2*nu-1;
rw=3*nu-2;
/*init : initialization*/
if (flag==4)
{
if((*(block->work)=(mat_vpv_struct*) scicos_malloc(sizeof(mat_vpv_struct)))==NULL)
{
set_block_error(-16);
return;
}
ptr=*(block->work);
if((ptr->LA=(double*) scicos_malloc(sizeof(double)*(2*nu*nu)))==NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if((ptr->LX=(double*) scicos_malloc(sizeof(double)*(2*nu)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->LA);
scicos_free(ptr);
return;
}
if((ptr->LVR=(double*) scicos_malloc(sizeof(double)*(2*nu*nu)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->LX);
scicos_free(ptr->LA);
scicos_free(ptr);
return;
}
if((ptr->dwork=(double*) scicos_malloc(sizeof(double)*2*lwork))==NULL)
{
set_block_error(-16);
scicos_free(ptr->LVR);
scicos_free(ptr->LX);
scicos_free(ptr->LA);
scicos_free(ptr);
return;
}
if((ptr->rwork=(double*) scicos_malloc(sizeof(double)*2*rw))==NULL)
{
set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->LVR);
scicos_free(ptr->LX);
scicos_free(ptr->LA);
scicos_free(ptr);
return;
}
if((ptr->dwork1=(double*) scicos_malloc(sizeof(double)*2*lwork1))==NULL)
{
set_block_error(-16);
scicos_free(ptr->rwork);
scicos_free(ptr->dwork);
scicos_free(ptr->LVR);
scicos_free(ptr->LX);
scicos_free(ptr->LA);
scicos_free(ptr);
return;
}
if((ptr->rwork1=(double*) scicos_malloc(sizeof(double)*2*lwork1))==NULL)
{
set_block_error(-16);
scicos_free(ptr->dwork1);
scicos_free(ptr->rwork);
scicos_free(ptr->dwork);
scicos_free(ptr->LVR);
scicos_free(ptr->LX);
scicos_free(ptr->LA);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag==5)
{
ptr=*(block->work);
if((ptr->rwork1)!=NULL)
{
scicos_free(ptr->LA);
scicos_free(ptr->LX);
scicos_free(ptr->LVR);
scicos_free(ptr->rwork);
scicos_free(ptr->rwork1);
scicos_free(ptr->dwork);
scicos_free(ptr->dwork1);
scicos_free(ptr);
return;
}
}
else
{
ptr=*(block->work);
for (i=0;i<(nu*nu);i++)
{
ptr->LA[2*i]=ur[i];
ptr->LA[2*i+1]=ui[i];
}
hermitien=1;
for (j=0;j<nu;j++)
{
for (i=j;i<nu;i++)
{
ij=i+j*nu;
ji=j+i*nu;
if (i!=j)
{
if ((*(ptr->LA+2*ij)==*(ptr->LA+2*ji))&&(*(ptr->LA+2*ij+1)==-(*(ptr->LA+2*ji+1)))) hermitien*= 1;
else { hermitien*=0;break;
}
}
}
}
if (hermitien==1)
{
C2F(zheev)("V","U",&nu,ptr->LA,&nu,ptr->LX,ptr->dwork,&lwork,ptr->rwork,&info);
if (info!=0)
{
if (flag!=6)
{
set_block_error(-7);
return;
}
}
for (i=0;i<nu;i++)
{
ii=i+i*nu;
*(y1r+ii)=*(ptr->LX+i);
}
for(i=0;i<nu*nu;i++)
{
*(y2r+i)=*(ptr->LA+2*i);
*(y2i+i)=*(ptr->LA+2*i+1);
}
}
else
{
C2F(zgeev)("N","V",&nu,ptr->LA,&nu,ptr->LX,ptr->dwork1,&nu,ptr->LVR,&nu,ptr->dwork1,&lwork1,ptr->rwork1,&info);
if (info!=0)
{
if (flag!=6)
{
set_block_error(-7);
return;
}
}
l0=0;
C2F(dlaset)("F",&nu,&nu,&l0,&l0,y1r,&nu);
C2F(dlaset)("F",&nu,&nu,&l0,&l0,y1i,&nu);
C2F(dlaset)("F",&nu,&nu,&l0,&l0,y2r,&nu);
C2F(dlaset)("F",&nu,&nu,&l0,&l0,y2i,&nu);
for (i=0;i<nu;i++)
{
ii=i+i*nu;
*(y1r+ii)=*(ptr->LX+2*i);
*(y1i+ii)=*(ptr->LX+2*i+1);
}
for(i=0;i<nu*nu;i++)
{
*(y2r+i)=*(ptr->LVR+2*i);
*(y2i+i)=*(ptr->LVR+2*i+1);
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_expm(scicos_block *block, int flag)
{
double *u = NULL;
double *y = NULL;
int nu = 0;
int ierr = 0;
mat_exp_struct** work = (mat_exp_struct**) block->work;
mat_exp_struct *ptr = NULL;
nu = GetInPortCols(block, 1);
u = GetRealInPortPtrs(block, 1);
y = GetRealOutPortPtrs(block, 1);
/*init : initialization*/
if (flag == 4)
{
if ((*work = (mat_exp_struct*) scicos_malloc(sizeof(mat_exp_struct))) == NULL)
{
set_block_error(-16);
return;
}
ptr = *work;
if ((ptr->iwork = (int*) scicos_malloc(sizeof(int) * (2 * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if ((ptr->dwork = (double*) scicos_malloc(sizeof(double) * (nu * (2 * nu + 2 * nu + 5)))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->iwork);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag == 5)
{
ptr = *work;
if (ptr->dwork != NULL)
{
scicos_free(ptr->iwork);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;
}
}
else
{
ptr = *work;
C2F(dexpm1)(&nu, &nu, u, y, &nu, ptr->dwork, ptr->iwork, &ierr);
if (ierr != 0)
{
if (flag != 6)
{
set_block_error(-7);
return;
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void variable_delay(scicos_block *block, int flag)
{
/* rpar[0]=max delay, rpar[1]=init value, ipar[0]=buffer length */
double** work = (double**) block->work;
double* pw = NULL, del = 0., t = 0., td = 0.;
int* iw = NULL;
int i = 0, j = 0, k = 0;
if (flag == 4) /* the workspace is used to store previous values */
{
if ((*work = (double*)
scicos_malloc(sizeof(int) + sizeof(double) *
block->ipar[0] * (1 + block->insz[0]))) == NULL )
{
set_block_error(-16);
return;
}
pw = *work;
pw[0] = -block->rpar[0] * block->ipar[0];
for (i = 1; i < block->ipar[0]; i++)
{
pw[i] = pw[i - 1] + block->rpar[0];
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[i + block->ipar[0]*j] = block->rpar[1];
}
}
iw = (int *) (pw + block->ipar[0] * (1 + block->insz[0]));
*iw = 0;
}
else if (flag == 5)
{
scicos_free(*work);
}
else if (flag == 1)
{
if (get_phase_simulation() == 1)
{
do_cold_restart();
}
pw = *work;
iw = (int *) (pw + block->ipar[0] * (1 + block->insz[0]));
t = get_scicos_time();
del = Min(Max(0, block->inptr[1][0]), block->rpar[0]);
td = t - del;
if (td < pw[*iw])
{
scicos_print(_("delayed time=%f but last stored time=%f\n"), td, pw[*iw]);
scicos_print(_("Consider increasing the length of buffer in variable delay block\n"));
}
if (t > pw[(block->ipar[0] + *iw - 1) % block->ipar[0]])
{
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[*iw + block->ipar[0]*j] = block->inptr[0][j - 1];
}
pw[*iw] = t;
*iw = (*iw + 1) % block->ipar[0];
}
else
{
for (j = 1; j < block->insz[0] + 1; j++)
{
pw[(block->ipar[0] + *iw - 1) % block->ipar[0] + block->ipar[0]*j] = block->inptr[0][j - 1];
}
pw[(block->ipar[0] + *iw - 1) % block->ipar[0]] = t;
}
i = 0;
j = block->ipar[0] - 1;
while (j - i > 1)
{
k = (i + j) / 2;
if (td < pw[(k + *iw) % block->ipar[0]])
{
j = k;
}
else if (td > pw[(k + *iw) % block->ipar[0]])
{
i = k;
}
else
{
i = k;
j = k;
break;
}
}
i = (i + *iw) % block->ipar[0];
j = (j + *iw) % block->ipar[0];
del = pw[j] - pw[i];
if (del != 0.0)
{
for (k = 1; k < block->insz[0] + 1; k++)
{
block->outptr[0][k - 1] = ((pw[j] - td) * pw[i + block->ipar[0] * k] +
(td - pw[i]) * pw[j + block->ipar[0] * k]) / del;
}
}
else
{
for (k = 1; k < block->insz[0] + 1; k++)
{
block->outptr[0][k - 1] = pw[i + block->ipar[0] * k];
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_svd(scicos_block *block, int flag)
{
double *u = NULL;
double *y1 = NULL, *y2 = NULL, *y3 = NULL;
int nu = 0, mu = 0;
int info = 0;
int i = 0, j = 0, ij = 0, ji = 0, ii = 0, lwork = 0;
mat_sdv_struct** work = (mat_sdv_struct**) block->work;
mat_sdv_struct *ptr = NULL;
mu = GetInPortRows(block, 1);
nu = GetInPortCols(block, 1);
u = GetRealInPortPtrs(block, 1);
y1 = GetRealOutPortPtrs(block, 1);
y2 = GetRealOutPortPtrs(block, 2);
y3 = GetRealOutPortPtrs(block, 3);
/* for lapack 3.1 (2006)*/
lwork = Max(3 * Min(mu, nu) + Max(mu, nu), 5 * Min(mu, nu));
lwork = Max(1, lwork);
/*init : initialization*/
if (flag == 4)
{
if ((*work = (mat_sdv_struct*) scicos_malloc(sizeof(mat_sdv_struct))) == NULL)
{
set_block_error(-16);
return;
}
ptr = *work;
if ((ptr->l0 = (double*) scicos_malloc(sizeof(double))) == NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if ((ptr->LA = (double*) scicos_malloc(sizeof(double) * (mu * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->l0);
scicos_free(ptr);
return;
}
if ((ptr->LSV = (double*) scicos_malloc(sizeof(double) * (Min(mu, nu)))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->LA);
scicos_free(ptr->l0);
scicos_free(ptr);
return;
}
if ((ptr->LVT = (double*) scicos_malloc(sizeof(double) * (nu * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->LSV);
scicos_free(ptr->LA);
scicos_free(ptr->l0);
scicos_free(ptr);
return;
}
if ((ptr->dwork = (double*) scicos_malloc(sizeof(double) * lwork)) == NULL)
{
set_block_error(-16);
scicos_free(ptr->LVT);
scicos_free(ptr->LSV);
scicos_free(ptr->LA);
scicos_free(ptr->l0);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag == 5)
{
ptr = *work;
if ((ptr->dwork) != NULL)
{
scicos_free(ptr->l0);
scicos_free(ptr->LA);
scicos_free(ptr->LSV);
scicos_free(ptr->LVT);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;
}
}
else
{
ptr = *work;
C2F(dlacpy)("F", &mu, &nu, u, &mu, ptr->LA, &mu);
C2F(dgesvd)("A", "A", &mu, &nu, ptr->LA, &mu, ptr->LSV, y1, &mu, ptr->LVT, &nu, ptr->dwork, &lwork, &info);
if (info != 0)
{
if (flag != 6)
{
set_block_error(-7);
return;
}
}
*(ptr->l0) = 0;
C2F(dlaset)("F", &mu, &nu, ptr->l0, ptr->l0, y2, &mu);
for (i = 0; i < Min(mu, nu); i++)
{
ii = i + i * mu;
*(y2 + ii) = *(ptr->LSV + i);
}
for (j = 0; j < nu; j++)
{
for (i = j; i < nu; i++)
{
ij = i + j * nu;
ji = j + i * nu;
*(y3 + ij) = *(ptr->LVT + ji);
*(y3 + ji) = *(ptr->LVT + ij);
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void lookup_c(scicos_block *block, int flag)
{
double a = 0., b = 0., c = 0., y1 = 0., y2 = 0., t1 = 0., t2 = 0.;
int *ind = NULL, inow = 0, i = 0, ip1 = 0;
double *y = NULL, *u = NULL, u0 = 0.;
double d1 = 0., d2 = 0., h = 0., dh = 0., ddh = 0., dddh = 0.;
switch (flag)
{
/* init */
case 4 : /* the workspace is used to store discrete counter value */
{
if ((*block->work = scicos_malloc(1 * sizeof(int))) == NULL)
{
set_block_error(-16);
return;
}
ind = *block->work;
ind[0] = 0;
return;
}
/* event date computation */
case 1 :
{
y = GetRealOutPortPtrs(block, 1);
u = GetRealInPortPtrs(block, 1);
u0 = u[0];
ind = *block->work;
i = ind[0];
ip1 = i + 1;
if ((Extrapo == 0) || ((Extrapo == 1) && ((Order == 0) || (Order == 8) || (Order == 9))))
{
if (u0 < RPAR[0])
{
y[0] = RPAR[nPoints];
break;
}
if (u0 >= RPAR[nPoints - 1])
{
y[0] = RPAR[nPoints * 2 - 1];
break;
}
}
if (u0 < RPAR[i])
{
i = FindIndex(Order, u0, 0, i, RPAR, nPoints);
}
else if (u0 >= RPAR[ip1])
{
i = FindIndex(Order, u0, ip1, nPoints - 1, RPAR, nPoints);
}
ind[0] = i;
if (Order == 0) /* (METHOD=='zero order-below')*/
{
y[0] = RPAR[nPoints + i];
break;
}
if (Order == 8) /* (METHOD=='zero order-above')*/
{
y[0] = RPAR[nPoints + i + 1];
break;
}
if (Order == 9) /* (METHOD=='zero order-nearest')*/
{
if (u0 < (RPAR[i] + RPAR[i + 1]) / 2 )
{
y[0] = RPAR[nPoints + i];
}
else
{
y[0] = RPAR[nPoints + i + 1];
}
break;
}
if (Order == 1)
{
t1 = RPAR[i];
t2 = RPAR[i + 1];
y1 = RPAR[nPoints + i];
y2 = RPAR[nPoints + i + 1];
y[0] = (y2 - y1) * (u0 - t1) / (t2 - t1) + y1;
break;
}
if ((Order == 2) && (nPoints > 2))
{
t1 = RPAR[i];
a = RPAR[2 * nPoints + i];
b = RPAR[2 * nPoints + i + nPoints - 1];
c = RPAR[2 * nPoints + i + 2 * nPoints - 2];
y[0] = a * (u0 - t1) * (u0 - t1) + b * (u0 - t1) + c;
break;
}
if ((Order >= 3) && (Order <= 7))
{
t1 = RPAR[i];
t2 = RPAR[i + 1];
y1 = RPAR[nPoints + i];
y2 = RPAR[nPoints + i + 1];
d1 = RPAR[2 * nPoints + i];
d2 = RPAR[2 * nPoints + i + 1];
/*-- this function is defined in curve_c.c ---*/
scicos_evalhermite(&u0, &t1, &t2, &y1, &y2, &d1, &d2, &h, &dh, &ddh, &dddh, &inow);
y[0] = h;
break;
}
}
/* event date computation */
case 3 :
{
/* ind=*block->work;
i=ind[0];
if ((Order==1)||(Order==0)){
i=ind[2];
if (i==nPoints-1){
if (Periodic==1) {
i=0;
ind[0]=-1;
ind[1]=0;
}
}
if (i<nPoints-1) {
block->evout[0]=RPAR[i+1]-RPAR[i];
ind[2]=i+1;
}
if (ind[2]==1) ind[3]++;
}
if (Order>=2){
if ( Periodic) {
block->evout[0]=T;
}else{
if (ind[3]==0) {
block->evout[0]=T;
}
}
ind[3]++;
ind[0]=-1;
ind[1]=0;
}*/
break;
}
/* finish */
case 5 :
{
scicos_free(*block->work); /*free the workspace*/
break;
}
default :
break;
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_bksl(scicos_block *block,int flag)
{
double *u1 = NULL;
double *u2 = NULL;
double *y = NULL;
int mu = 0;
int nu1 = 0;
int nu2 = 0;
int info = 0;
int i = 0,l = 0,lw = 0,lu = 0;
mat_bksl_struct *ptr = NULL;
double rcond = 0., ANORM = 0., EPS = 0.;
mu =GetInPortRows(block,1);
nu1 =GetInPortCols(block,1);
nu2 =GetInPortCols(block,2);
u1=GetRealInPortPtrs(block,1);
u2=GetRealInPortPtrs(block,2);
y=GetRealOutPortPtrs(block,1);
l=Max(mu,nu1);
lu=Max(4*nu1,Min(mu,nu1)+3*nu1+1);
lw=Max(lu,2*Min(mu,nu1)+nu2);
/*init : initialization*/
if (flag==4)
{if((*(block->work)=(mat_bksl_struct*) scicos_malloc(sizeof(mat_bksl_struct)))==NULL)
{set_block_error(-16);
return;}
ptr=*(block->work);
if((ptr->ipiv=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{set_block_error(-16);
scicos_free(ptr);
return;}
if((ptr->rank=(int*) scicos_malloc(sizeof(int)))==NULL)
{set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->jpvt=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{set_block_error(-16);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->iwork=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{set_block_error(-16);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->dwork=(double*) scicos_malloc(sizeof(double)*lw))==NULL)
{set_block_error(-16);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->LAF=(double*) scicos_malloc(sizeof(double)*(mu*nu1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->LA=(double*) scicos_malloc(sizeof(double)*(mu*nu1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LAF);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->LXB=(double*) scicos_malloc(sizeof(double)*(l*nu2)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LA);
scicos_free(ptr->LAF);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
}
/* Terminaison */
else if (flag==5)
{ptr=*(block->work);
if(ptr->LXB!=NULL){
scicos_free(ptr->ipiv);
scicos_free(ptr->rank);
scicos_free(ptr->jpvt);
scicos_free(ptr->iwork);
scicos_free(ptr->LAF);
scicos_free(ptr->LA);
scicos_free(ptr->LXB);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;}
}
else
{
ptr=*(block->work);
EPS=C2F(dlamch)("e",1L);
ANORM=C2F(dlange)("1",&mu,&nu1,u1,&mu,ptr->dwork);
C2F(dlacpy)("F",&mu,&nu1,u1,&mu,ptr->LA,&mu);
if (mu==nu1)
{C2F(dlacpy)("F",&mu,&nu1,ptr->LA,&mu,ptr->LAF,&mu);
C2F(dgetrf)(&nu1,&nu1,ptr->LAF,&nu1,ptr->ipiv,&info);
rcond=0;
if (info==0)
{C2F(dgecon)("1",&nu1,ptr->LAF,&nu1,&ANORM,&rcond,ptr->dwork,ptr->iwork,&info);
if (rcond>pow(EPS,0.5))
{C2F(dlacpy)("F",&nu1,&nu2,u2,&nu1,ptr->LXB,&nu1);
C2F(dgetrs)("N",&nu1,&nu2,ptr->LAF,&nu1,ptr->ipiv,ptr->LXB,&nu1,&info);
C2F(dlacpy)("F",&nu1,&nu2,ptr->LXB,&nu1,y,&nu1);
return;
}
}
}
rcond=pow(EPS,0.5);
C2F(dlacpy)("F",&mu,&nu2,u2,&mu,ptr->LXB,&l);
for (i=0;i<nu1;i++) *(ptr->jpvt+i)=0;
C2F(dgelsy1)(&mu,&nu1,&nu2,ptr->LA,&mu,ptr->LXB,&l,ptr->jpvt,&rcond,ptr->rank,ptr->dwork,&lw,&info);
if (info!=0)
{if (flag!=6)
{set_block_error(-7);
return;
}
}
C2F(dlacpy)("F",&nu1,&nu2,ptr->LXB,&l,y,&nu1);
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void ratelimiter(scicos_block *block,int flag)
{
/* rpar[0]=rising rate limit, rpar[1]=falling rate limit */
double* pw = NULL;
double rate = 0. , t = 0.;
if (flag == 4)
{
/* the workspace is used to store previous values */
if ((*block->work = scicos_malloc(sizeof(double)*4))== NULL )
{
set_block_error(-16);
return;
}
pw=*block->work;
pw[0]=0.0;
pw[1]=0.0;
pw[2]=0.0;
pw[3]=0.0;
}
else if (flag == 5)
{
scicos_free(*block->work);
}
else if (flag==1)
{
if (get_phase_simulation()==1) do_cold_restart();
pw=*block->work;
t=get_scicos_time();
if(t>pw[2])
{
pw[0]=pw[2];
pw[1]=pw[3];
rate=(block->inptr[0][0]-pw[1])/(t-pw[0]);
}
else if(t<=pw[2])
{
if(t>pw[0])
{
rate=(block->inptr[0][0]-pw[1])/(t-pw[0]);
}
else
{
rate=0.0;
}
}
if(rate>block->rpar[0])
{
block->outptr[0][0]=(t-pw[0])*block->rpar[0]+pw[1];
}
else if(rate<block->rpar[1])
{
block->outptr[0][0]=(t-pw[0])*block->rpar[1]+pw[1];
}
else
{
block->outptr[0][0]=block->inptr[0][0];
}
pw[2]=t;
pw[3]=block->outptr[0][0];
}
}
