/*------------------------------------------------------------------------------
* tle.c: NORAD TLE (two line element) functions
*
*          Copyright (C) 2012-2020 by T.TAKASU, All rights reserved.
*
* references:
*     [1] F.R.Hoots and R.L.Roehrich, Spacetrack report No.3, Models for
*         propagation of NORAD element sets, December 1980
*     [2] D.A.Vallado, P.Crawford, R.Hujsak and T.S.Kelso, Revisiting
*         Spacetrack Report #3, AIAA 2006-6753, 2006
*     [3] CelesTrak (http://www.celestrak.com)
*
* version : $Revision:$ $Date:$
* history : 2012/11/01 1.0  new
*           2013/01/25 1.1  fix bug on binary search
*           2014/08/26 1.2  fix bug on tle_pos() to get tle by satid or desig
*           2020/11/30 1.3  fix problem on duplicated names in a satellite
*-----------------------------------------------------------------------------*/
#include "rtklib.h"

/* SGP4 model propagator by STR#3 (ref [1] sec.6,11) -------------------------*/

#define DE2RA       0.174532925E-1
#define E6A         1.E-6
#define PIO2        1.57079633
#define QO          120.0
#define SO          78.0
#define TOTHRD      0.66666667
#define TWOPI       6.2831853
#define X3PIO2      4.71238898
#define XJ2         1.082616E-3
#define XJ3         -0.253881E-5
#define XJ4         -1.65597E-6
#define XKE         0.743669161E-1
#define XKMPER      6378.135
#define XMNPDA      1440.0
#define AE          1.0
#define CK2         5.413080E-4         /* = 0.5*XJ2*AE*AE */
#define CK4         0.62098875E-6       /* = -0.375*XJ4*AE*AE*AE*AE */
#define QOMS2T      1.88027916E-9       /* = pow((QO-SO)*AE/XKMPER,4.0) */
#define S           1.01222928          /* = AE*(1.0+SO/XKMPER) */

static void SGP4_STR3(double tsince, const tled_t *data, double *rs)
{
    double xnodeo,omegao,xmo,eo,xincl,xno,xndt2o,xndd6o,bstar;
    double a1,cosio,theta2,x3thm1,eosq,betao2,betao,del1,ao,delo,xnodp,aodp,s4;
    double qoms24,perige,pinvsq,tsi,eta,etasq,eeta,psisq,coef,coef1,c1,c2,c3,c4;
    double c5,sinio,a3ovk2,x1mth2,theta4,xmdot,x1m5th,omgdot,xhdot1,xnodot;
    double omgcof,xmcof,xnodcf,t2cof,xlcof,aycof,delmo,sinmo,x7thm1,c1sq,d2,d3;
    double d4,t3cof,t4cof,t5cof,xmdf,omgadf,xnoddf,omega,xmp,tsq,xnode,delomg;
    double delm,tcube,tfour,a,e,xl,beta,xn,axn,xll,aynl,xlt,ayn,capu,sinepw;
    double cosepw,epw,ecose,esine,elsq,pl,r,rdot,rfdot,betal,cosu,sinu,u,sin2u;
    double cos2u,rk,uk,xnodek,xinck,rdotk,rfdotk,sinuk,cosuk,sinik,cosik,sinnok;
    double cosnok,xmx,xmy,ux,uy,uz,vx,vy,vz,x,y,z,xdot,ydot,zdot;
    double temp,temp1,temp2,temp3,temp4,temp5,temp6,tempa,tempe,templ;
    int i,isimp;
    
    xnodeo=data->OMG*DE2RA;
    omegao=data->omg*DE2RA;
    xmo=data->M*DE2RA;
    xincl=data->inc*DE2RA;
    temp=TWOPI/XMNPDA/XMNPDA;
    xno=data->n*temp*XMNPDA;
    xndt2o=data->ndot*temp;
    xndd6o=data->nddot*temp/XMNPDA;
    bstar=data->bstar/AE;
    eo=data->ecc;
    /*
    * recover original mean motion (xnodp) and semimajor axis (aodp)
    * from input elements
    */
    a1=pow(XKE/xno,TOTHRD);
    cosio=cos(xincl);
    theta2=cosio*cosio;
    x3thm1=3.0*theta2-1.0;
    eosq=eo*eo;
    betao2=1.0-eosq;
    betao=sqrt(betao2);
    del1=1.5*CK2*x3thm1/(a1*a1*betao*betao2);
    ao=a1*(1.0-del1*(0.5*TOTHRD+del1*(1.0+134.0/81.0*del1)));
    delo=1.5*CK2*x3thm1/(ao*ao*betao*betao2);
    xnodp=xno/(1.0+delo);
    aodp=ao/(1.0-delo);
    /*
    * initialization
    * for perigee less than 220 kilometers, the isimp flag is set and
    * the equations are truncated to linear variation in sqrt a and
    * quadratic variation in mean anomaly. also, the c3 term, the
    * delta omega term, and the delta m term are dropped.
    */
    isimp=0;
    if ((aodp*(1.0-eo)/AE)<(220.0/XKMPER+AE)) isimp=1;
    
    /* for perigee below 156 km, the values of s and qoms2t are altered */
    s4=S;
    qoms24=QOMS2T;
    perige=(aodp*(1.0-eo)-AE)*XKMPER;
    if (perige<156.0) {
        s4=perige-78.0;
        if (perige<=98.0) s4=20.0;
        qoms24=pow((120.0-s4)*AE/XKMPER,4.0);
        s4=s4/XKMPER+AE;
    }
    pinvsq=1.0/(aodp*aodp*betao2*betao2);
    tsi=1.0/(aodp-s4);
    eta=aodp*eo*tsi;
    etasq=eta*eta;
    eeta=eo*eta;
    psisq=fabs(1.0-etasq);
    coef=qoms24*pow(tsi,4.0);
    coef1=coef/pow(psisq,3.5);
    c2=coef1*xnodp*(aodp*(1.0+1.5*etasq+eeta*(4.0+etasq))+0.75*
            CK2*tsi/psisq*x3thm1*(8.0+3.0*etasq*(8.0+etasq)));
    c1=bstar*c2;
    sinio=sin(xincl);
    a3ovk2=-XJ3/CK2*pow(AE,3.0);
    c3=coef*tsi*a3ovk2*xnodp*AE*sinio/eo;
    x1mth2=1.0-theta2;
    c4=2.0*xnodp*coef1*aodp*betao2*(eta*
            (2.0+0.5*etasq)+eo*(0.5+2.0*etasq)-2.0*CK2*tsi/
            (aodp*psisq)*(-3.0*x3thm1*(1.0-2.0*eeta+etasq*
            (1.5-0.5*eeta))+0.75*x1mth2*(2.0*etasq-eeta*
            (1.0+etasq))*cos(2.0*omegao)));
    c5=2.0*coef1*aodp*betao2*(1.0+2.75*(etasq+eeta)+eeta*etasq);
    theta4=theta2*theta2;
    temp1=3.0*CK2*pinvsq*xnodp;
    temp2=temp1*CK2*pinvsq;
    temp3=1.25*CK4*pinvsq*pinvsq*xnodp;
    xmdot=xnodp+0.5*temp1*betao*x3thm1+0.0625*temp2*betao*
            (13.0-78.0*theta2+137.0*theta4);
    x1m5th=1.0-5.0*theta2;
    omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7.0-114.0*theta2+
            395.0*theta4)+temp3*(3.0-36.0*theta2+49.0*theta4);
    xhdot1=-temp1*cosio;
    xnodot=xhdot1+(0.5*temp2*(4.0-19.0*theta2)+2.0*temp3*(3.0-
            7.0*theta2))*cosio;
    omgcof=bstar*c3*cos(omegao);
    xmcof=-TOTHRD*coef*bstar*AE/eeta;
    xnodcf=3.5*betao2*xhdot1*c1;
    t2cof=1.5*c1;
    xlcof=0.125*a3ovk2*sinio*(3.0+5.0*cosio)/(1.0+cosio);
    aycof=0.25*a3ovk2*sinio;
    delmo=pow(1.0+eta*cos(xmo),3.0);
    sinmo=sin(xmo);
    x7thm1=7.0*theta2-1.0;
    
    if (isimp!=1) {
        c1sq=c1*c1;
        d2=4.0*aodp*tsi*c1sq;
        temp=d2*tsi*c1/3.0;
        d3=(17.0*aodp+s4)*temp;
        d4=0.5*temp*aodp*tsi*(221.0*aodp+31.0*s4)*c1;
        t3cof=d2+2.0*c1sq;
        t4cof=0.25*(3.0*d3+c1*(12.0*d2+10.0*c1sq));
        t5cof=0.2*(3.0*d4+12.0*c1*d3+6.0*d2*d2+15.0*c1sq*(2.0*d2+c1sq));
    }
    else {
        d2=d3=d4=t3cof=t4cof=t5cof=0.0;
    }
    /* update for secular gravity and atmospheric drag */
    xmdf=xmo+xmdot*tsince;
    omgadf=omegao+omgdot*tsince;
    xnoddf=xnodeo+xnodot*tsince;
    omega=omgadf;
    xmp=xmdf;
    tsq=tsince*tsince;
    xnode=xnoddf+xnodcf*tsq;
    tempa=1.0-c1*tsince;
    tempe=bstar*c4*tsince;
    templ=t2cof*tsq;
    if (isimp==1) {
        delomg=omgcof*tsince;
        delm=xmcof*(pow(1.0+eta*cos(xmdf),3.0)-delmo);
        temp=delomg+delm;
        xmp=xmdf+temp;
        omega=omgadf-temp;
        tcube=tsq*tsince;
        tfour=tsince*tcube;
        tempa=tempa-d2*tsq-d3*tcube-d4*tfour;
        tempe=tempe+bstar*c5*(sin(xmp)-sinmo);
        templ=templ+t3cof*tcube+tfour*(t4cof+tsince*t5cof);
    }
    a=aodp*pow(tempa,2.0);
    e=eo-tempe;
    xl=xmp+omega+xnode+xnodp*templ;
    beta=sqrt(1.0-e*e);
    xn=XKE/pow(a,1.5);
    
    /* long period periodics */
    axn=e*cos(omega);
    temp=1.0/(a*beta*beta);
    xll=temp*xlcof*axn;
    aynl=temp*aycof;
    xlt=xl+xll;
    ayn=e*sin(omega)+aynl;
    
    /* solve keplers equation */
    capu=fmod(xlt-xnode,TWOPI);
    temp2=capu;
    for (i=0;i<10;i++) {
        sinepw=sin(temp2);
        cosepw=cos(temp2);
        temp3=axn*sinepw;
        temp4=ayn*cosepw;
        temp5=axn*cosepw;
        temp6=ayn*sinepw;
        epw=(capu-temp4+temp3-temp2)/(1.0-temp5-temp6)+temp2;
        if (fabs(epw-temp2)<=E6A) break;
        temp2=epw;
    }
    /* short period preliminary quantities */
    ecose=temp5+temp6;
    esine=temp3-temp4;
    elsq=axn*axn+ayn*ayn;
    temp=1.0-elsq;
    pl=a*temp;
    r=a*(1.0-ecose);
    temp1=1.0/r;
    rdot=XKE*sqrt(a)*esine*temp1;
    rfdot=XKE*sqrt(pl)*temp1;
    temp2=a*temp1;
    betal=sqrt(temp);
    temp3=1.0/(1.0+betal);
    cosu=temp2*(cosepw-axn+ayn*esine*temp3);
    sinu=temp2*(sinepw-ayn-axn*esine*temp3);
    u=atan2(sinu,cosu);
    sin2u=2.0*sinu*cosu;
    cos2u=2.0*cosu*cosu-1.0;
    temp=1.0/pl;
    temp1=CK2*temp;
    temp2=temp1*temp;
    
    /* update for short periodics */
    rk=r*(1.0-1.5*temp2*betal*x3thm1)+0.5*temp1*x1mth2*cos2u;
    uk=u-0.25*temp2*x7thm1*sin2u;
    xnodek=xnode+1.5*temp2*cosio*sin2u;
    xinck=xincl+1.5*temp2*cosio*sinio*cos2u;
    rdotk=rdot-xn*temp1*x1mth2*sin2u;
    rfdotk=rfdot+xn*temp1*(x1mth2*cos2u+1.5*x3thm1);
    
    /* orientation vectors */
    sinuk=sin(uk);
    cosuk=cos(uk);
    sinik=sin(xinck);
    cosik=cos(xinck);
    sinnok=sin(xnodek);
    cosnok=cos(xnodek);
    xmx=-sinnok*cosik;
    xmy=cosnok*cosik;
    ux=xmx*sinuk+cosnok*cosuk;
    uy=xmy*sinuk+sinnok*cosuk;
    uz=sinik*sinuk;
    vx=xmx*cosuk-cosnok*sinuk;
    vy=xmy*cosuk-sinnok*sinuk;
    vz=sinik*cosuk;
    
    /* position and velocity */
    x=rk*ux;
    y=rk*uy;
    z=rk*uz;
    xdot=rdotk*ux+rfdotk*vx;
    ydot=rdotk*uy+rfdotk*vy;
    zdot=rdotk*uz+rfdotk*vz;
    
    rs[0]=x*XKMPER/AE*1E3; /* (m) */
    rs[1]=y*XKMPER/AE*1E3;
    rs[2]=z*XKMPER/AE*1E3;
    rs[3]=xdot*XKMPER/AE*XMNPDA/86400.0*1E3; /* (m/s) */
    rs[4]=ydot*XKMPER/AE*XMNPDA/86400.0*1E3;
    rs[5]=zdot*XKMPER/AE*XMNPDA/86400.0*1E3;
}
/* drop spaces at string tail ------------------------------------------------*/
static void chop(char *buff)
{
    int i;
    for (i=strlen(buff)-1;i>=0;i--) {
        if (buff[i]==' '||buff[i]=='\r'||buff[i]=='\n') buff[i]='\0';
        else break;
    }
}
/* test TLE line checksum ----------------------------------------------------*/
static int checksum(const char *buff)
{
    int i,cs=0;
    
    if (strlen(buff)<69) return 0;
    
    for (i=0;i<68;i++) {
        if ('0'<=buff[i]&&buff[i]<='9') cs+=(int)(buff[i]-'0');
        else if (buff[i]=='-') cs+=1;
    }
    return (int)(buff[68]-'0')==cs%10;
}
/* decode TLE line 1 ---------------------------------------------------------*/
static int decode_line1(const char *buff, tled_t *data)
{
    double year,doy,nddot,exp1,bstar,exp2,ep[6]={2000,1,1};
    
    strncpy(data->satno,buff+2,5);       /* satellite number */
    data->satno[5]='\0';
    chop(data->satno);
    
    data->satclass=buff[7];              /* satellite classification */
    strncpy(data->desig,buff+9,8);       /* international designator */
    data->desig[8]='\0';
    chop(data->desig);
    
    year      =str2num(buff,18, 2);      /* epoch year */
    doy       =str2num(buff,20,12);      /* epoch day of year */
    data->ndot=str2num(buff,33,10);      /* 1st time derivative of n */
    nddot     =str2num(buff,44, 6);      /* 2nd time derivative of n */
    exp1      =str2num(buff,50, 2);
    bstar     =str2num(buff,53, 6);      /* Bstar drag term */
    exp2      =str2num(buff,59, 2);
    data->etype=(int)str2num(buff,62,1); /* ephemeris type */
    data->eleno=(int)str2num(buff,64,4); /* ephemeris number */
    data->nddot=nddot*1E-5*pow(10.0,exp1);
    data->bstar=bstar*1E-5*pow(10.0,exp2);
    
    ep[0]=year+(year<57.0?2000.0:1900.0);
    data->epoch=timeadd(epoch2time(ep),(doy-1.0)*86400.0);
    
    data->inc=data->OMG=data->ecc=data->omg=data->M=data->n=0.0;
    data->rev=0;
    return 1;
}
/* decode TLE line 2 ---------------------------------------------------------*/
static int decode_line2(const char *buff, tled_t *data)
{
    char satno[16];
    
    strncpy(satno,buff+2,5);             /* satellite number */
    satno[5]='\0';
    chop(satno);
    
    data->inc=str2num(buff, 8, 8);       /* inclination (deg) */
    data->OMG=str2num(buff,17, 8);       /* RAAN (deg) */
    data->ecc=str2num(buff,26, 7)*1E-7;  /* eccentricity */
    data->omg=str2num(buff,34, 8);       /* argument of perigee (deg) */
    data->M  =str2num(buff,43, 8);       /* mean anomaly (deg) */
    data->n  =str2num(buff,52,11);       /* mean motion (rev/day) */
    data->rev=(int)str2num(buff,63,5);   /* revolution number */
    
    if (strcmp(satno,data->satno)) {
        trace(2,"tle satno mismatch: %s %s\n",data->satno,satno);
        return 0;
    }
    if (data->n<=0.0||data->ecc<0.0) {
        trace(2,"tle data error: %s\n",satno);
        return 0;
    }
    return 1;
}
/* add TLE data --------------------------------------------------------------*/
static int add_data(tle_t *tle, const tled_t *data)
{
    tled_t *tle_data;
    
    if (tle->n>=tle->nmax) {
        tle->nmax=tle->nmax<=0?1024:tle->nmax*2;
        
        if (!(tle_data=(tled_t *)realloc(tle->data,sizeof(tled_t)*tle->nmax))) {
            trace(1,"tle malloc error\n");
            free(tle->data); tle->data=NULL; tle->n=tle->nmax=0;
            return 0;
        }
        tle->data=tle_data;
    }
    tle->data[tle->n++]=*data;
    return 1;
}
/* compare TLE data by satellite name ----------------------------------------*/
static int cmp_tle_data(const void *p1, const void *p2)
{
    const tled_t *q1=(const tled_t *)p1,*q2=(const tled_t *)p2;
    return strcmp(q1->name,q2->name);
}
/* read TLE file ---------------------------------------------------------------
* read NORAD TLE (two line element) data file (ref [2],[3])
* args   : char   *file     I   NORAD TLE data file
*          tle_t  *tle      O   TLE data
* return : status (1:ok,0:error)
* notes  : before calling the function, the TLE data should be initialized.
*          the file should be in a two line (only TLE) or three line (satellite
*          name + TLE) format.
*          the characters after # in a line are treated as comments.
*-----------------------------------------------------------------------------*/
extern int tle_read(const char *file, tle_t *tle)
{
    FILE *fp;
    tled_t data={{0}};
    char *p,buff[256];
    int line=0;
    
    if (!(fp=fopen(file,"r"))) {
        trace(2,"tle file open error: %s\n",file);
        return 0;
    }
    while (fgets(buff,sizeof(buff),fp)) {
        
        /* delete comments */
        if ((p=strchr(buff,'#'))) *p='\0';
        chop(buff);
        
        if (buff[0]=='1'&&checksum(buff)) {
            
            /* decode TLE line 1 */
            if (decode_line1(buff,&data)) line=1;
        }
        else if (line==1&&buff[0]=='2'&&checksum(buff)) {
            
            /* decode TLE line 2 */
            if (!decode_line2(buff,&data)) continue;
            
            /* add TLE data */
            if (!add_data(tle,&data)) {
                fclose(fp);
                return 0;
            }
            data.name[0]='\0';
            data.alias[0]='\0';
        }
        else if (buff[0]) {
            
            /* satellite name in three line format */
            strcpy(data.alias,buff);
            
            /* omit words in parentheses */
            if ((p=strchr(data.alias,'('))) *p='\0';
            chop(data.alias);
            line=0;
        }
    }
    fclose(fp);
    
    /* sort tle data by satellite name */
    if (tle->n>0) qsort(tle->data,tle->n,sizeof(tled_t),cmp_tle_data);
    return 1;
}
/* read TLE satellite name file ------------------------------------------------
* read TLE satellite name file
* args   : char   *file     I   TLE satellite name file
*          tle_t  *tle      IO  TLE data
* return : status (1:ok,0:error)
* notes  : before calling the function, call tle_read() to read tle table
*          the TLE satellite name file contains the following record as a text
*          line. strings after # are treated as comments.
*
*          name satno [desig [# comment]]
*
*            name : satellite name
*            satno: satellite catalog number
*            desig: international designator (optional)
*-----------------------------------------------------------------------------*/
extern int tle_name_read(const char *file, tle_t *tle)
{
    FILE *fp;
    tled_t data;
    char *p,buff[256],name[256],satno[256],desig[256];
    int i;
    
    if (!(fp=fopen(file,"r"))) {
        trace(2,"tle satellite name file open error: %s\n",file);
        return 0;
    }
    while (fgets(buff,sizeof(buff),fp)) {
        
        if ((p=strchr(buff,'#'))) *p='\0';
        
        desig[0]='\0';
        
        if (sscanf(buff,"%s %s %s",name,satno,desig)<2) continue;
        satno[5]='\0';
        
        for (i=0;i<tle->n;i++) {
            if (!strcmp(tle->data[i].satno,satno)||
                !strcmp(tle->data[i].desig,desig)) break;
        }
        if (i>=tle->n) {
            trace(4,"no tle data: satno=%s desig=%s\n",satno,desig);
            continue;
        }
        if (!*tle->data[i].name) {
            strncpy(tle->data[i].name,name,31);
            tle->data[i].name[31]='\0';
        }
        else {
            data=tle->data[i];
            strncpy(data.name,name,31);
            data.name[31]='\0';
            if (!add_data(tle,&data)) {
                break;
            }
        }
    }
    fclose(fp);
    
    /* sort tle data by satellite name */
    if (tle->n>0) qsort(tle->data,tle->n,sizeof(tled_t),cmp_tle_data);
    return 1;
}
/* satellite position and velocity with TLE data -------------------------------
* compute satellite position and velocity in ECEF with TLE data
* args   : gtime_t time     I   time (GPST)
*          char   *name     I   satellite name           ("": not specified)
*          char   *satno    I   satellite catalog number ("": not specified)
*          char   *desig    I   international designaor  ("": not specified)
*          tle_t  *tle      I   TLE data
*          erp_t  *erp      I   EOP data (NULL: not used)
*          double *rs       O   sat position/velocity {x,y,z,vx,vy,vz} (m,m/s)
* return : status (1:ok,0:error)
* notes  : the coordinates of the position and velocity are ECEF (ITRF)
*          if erp == NULL, polar motion and ut1-utc are neglected
*-----------------------------------------------------------------------------*/
extern int tle_pos(gtime_t time, const char *name, const char *satno,
                   const char *desig, const tle_t *tle, const erp_t *erp,
                   double *rs)
{
    gtime_t tutc;
    double tsince,rs_tle[6],rs_pef[6],gmst;
    double R1[9]={0},R2[9]={0},R3[9]={0},W[9],erpv[5]={0};
    int i=0,j,k,stat=1;
    
    /* binary search by satellite name */
    if (*name) {
        for (i=j=0,k=tle->n-1;j<=k;) {
            i=(j+k)/2;
            if (!(stat=strcmp(name,tle->data[i].name))) break;
            if (stat<0) k=i-1; else j=i+1;
        }
    }
    /* serial search by catalog no or international designator */
    if (stat&&(*satno||*desig)) {
        for (i=0;i<tle->n;i++) {
            if (!strcmp(tle->data[i].satno,satno)||
                !strcmp(tle->data[i].desig,desig)) break;
        }
        if (i<tle->n) stat=0;
    }
    if (stat) {
        trace(4,"no tle data: name=%s satno=%s desig=%s\n",name,satno,desig);
        return 0;
    }
    tutc=gpst2utc(time);
    
    /* time since epoch (min) */
    tsince=timediff(tutc,tle->data[i].epoch)/60.0;
    
    /* SGP4 model propagator by STR#3 */
    SGP4_STR3(tsince,tle->data+i,rs_tle);
    
    /* erp values */
    if (erp) geterp(erp,time,erpv);
    
    /* GMST (rad) */
    gmst=utc2gmst(tutc,erpv[2]);
    
    /* TEME (true equator, mean eqinox) -> ECEF (ref [2] IID, Appendix C) */
    R1[0]=1.0; R1[4]=R1[8]=cos(-erpv[1]); R1[7]=sin(-erpv[1]); R1[5]=-R1[7];
    R2[4]=1.0; R2[0]=R2[8]=cos(-erpv[0]); R2[2]=sin(-erpv[0]); R2[6]=-R2[2];
    R3[8]=1.0; R3[0]=R3[4]=cos(gmst); R3[3]=sin(gmst); R3[1]=-R3[3];
    matmul("NN",3,1,3,1.0,R3,rs_tle  ,0.0,rs_pef  );
    matmul("NN",3,1,3,1.0,R3,rs_tle+3,0.0,rs_pef+3);
    rs_pef[3]+=OMGE*rs_pef[1];
    rs_pef[4]-=OMGE*rs_pef[0];
    matmul("NN",3,3,3,1.0,R1,R2,0.0,W);
    matmul("NN",3,1,3,1.0,W,rs_pef  ,0.0,rs  );
    matmul("NN",3,1,3,1.0,W,rs_pef+3,0.0,rs+3);
    return 1;
}
