#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <iostream>
#include "TPCMapping.h"
#include <math.h>


#define TPCmappingExit(x) std::cout << "Should exit " << x << std::endl

int TPCMapping::setIROCcon2fec(int i,char **val){
    if (i<8) return 0;
    int index=atoi(val[0]);
    gTPCDataROC[0][index].irow=atoi(val[1]); 
    gTPCDataROC[0][index].ipad=atoi(val[2]); 
    gTPCDataROC[0][index].con=atoi(val[3]); 
    gTPCDataROC[0][index].pin=atoi(val[4]);
    gTPCDataROC[0][index].fec=atoi(val[5]);
    gTPCDataROC[0][index].fecch=atoi(val[6]);
    gTPCDataROC[0][index].feccon=atoi(val[7]);
    if (gTPCDataROC[0][index].status==1){
      std::cout <<  "ERROR STATUS gTPCDataROC[0][index=" << index << "].status" << std::endl;
    }
    gTPCDataROC[0][index].status=1;
    return 1;
}

int TPCMapping::setOROCcon2fec(int i,char **val){
    if (i<8) return 0;
    int index=atoi(val[0]);
    gTPCDataROC[1][index].irow=atoi(val[1]); 
    gTPCDataROC[1][index].ipad=atoi(val[2]); 
    gTPCDataROC[1][index].con=atoi(val[3]); 
    gTPCDataROC[1][index].pin=atoi(val[4]);
    gTPCDataROC[1][index].fec=atoi(val[5]);
    gTPCDataROC[1][index].fecch=atoi(val[6]);
    gTPCDataROC[1][index].feccon=atoi(val[7]);
    if (gTPCDataROC[1][index].status==1){
      std::cout << "ERROR STATUS gTPCDataROC[1][index="<< index << "].status"  << std::endl;
    }
    gTPCDataROC[1][index].status=1; 
    return 1;
}

int TPCMapping::Process(){
  
   
    for (int count=0;count<18;count++){
	for (int side=0;side<2;side++){
	    for (int type=0;type<2;type++){
		int sector=count+side*36+type*18;
		gTPCSECTOR[sector].side=side;
		gTPCSECTOR[sector].count=count;
		gTPCSECTOR[sector].type=type;
	    }
	}
    }

    for (int sector=0;sector<4*18;sector++){
        int type=gTPCSECTOR[sector].type;
        if (!is_connected_sector(sector)) continue;
        
	for (int k=0;k<GetMAXTPCINDEX(type);k++){
            if  (!gTPCDataROC[type][k].status) continue;
	    gmaxTPCDataROC[type]=k+1;
	    u_short irow=    gTPCDataROC[type][k].irow;
	    u_short ipad=    gTPCDataROC[type][k].ipad;
	    u_short fec=     gTPCDataROC[type][k].fec;
	    u_short fecch=   gTPCDataROC[type][k].fecch;
            u_short altroch = gFEC2ALTRO[fecch];
	    u_short rcurow=0;
	    GetRcuRow(type,fec, &rcurow);
            u_short rcuid   = gTPCSECTOR2RCU[sector][rcurow].hwid;
	    u_short fecid   = gTPCSECTOR2FEC[sector][fec].hwid;

	    if ( gTPCSectorChannel[sector][irow][ipad].status==1){
		u_short oldindex=gTPCSectorChannel[sector][irow][ipad].index;
		u_short mrow=    gTPCDataROC[type][oldindex].irow;
		u_short mpad=    gTPCDataROC[type][oldindex].ipad; 
		std::cout << "ERROR STATUS gTPCSectorChannel[sector="<< sector << 
		  "][irow="<< irow <<"][ipad="<< ipad << "].status type=" <<type << 
		  " index=" << k << " oldindex="<< oldindex << " mrow="<< mrow << " mpad="<<mpad << std::endl;
                gTPCSectorChannel[sector][irow][ipad].Print();

		gTPCSectorChannel[sector][mrow][mpad].Print();
		gTPCDataROC[type][k].Print();
		gTPCDataROC[type][oldindex].Print();
	    }


	    gTPCSectorChannel[sector][irow][ipad].status=1;
	    gTPCSectorChannel[sector][irow][ipad].index=k;
	    gTPCSectorChannel[sector][irow][ipad].rcu=rcuid;
	    gTPCSectorChannel[sector][irow][ipad].fec=fecid;
	    gTPCSectorChannel[sector][irow][ipad].altroch=altroch;


	    if (gTPCHWChannel[rcuid][fecid][altroch].status == 1){
	      std::cout << "ERROR STATUS gTPCHWChannel[rcuid="<< rcuid << "][fecid="<< fecid << "][altroch=" << altroch  << "].status" << std::endl;
	    }
	    gTPCHWChannel[rcuid][fecid][altroch].status = 1;
	    gTPCHWChannel[rcuid][fecid][altroch].sector = sector;
	    gTPCHWChannel[rcuid][fecid][altroch].row = irow;
	    gTPCHWChannel[rcuid][fecid][altroch].pad = ipad;
	    gTPCHWChannel[rcuid][fecid][altroch].index = k;

              
            u_short nsector,nrow,npad,ier,mrcu,mfec,maltroch;
            
	    ier=GetHWAddr(sector, irow, ipad, &mrcu, &mfec, &maltroch);	
	    ier=GetPhysAddr( mrcu, mfec, maltroch , &nsector, &nrow, &npad);
	    if (nsector!=sector || nrow!=irow || npad!=ipad)
	      std::cout << "[" << ier << "]->  s="<< sector << " r=" << irow  << " p=" << ipad << " rcu=" << mrcu << " fec=" << 
		mfec <<" ch=" << maltroch <<" ->  s=" << nsector << " r=" << npad << " p=" << std::endl; 
	}

    }
    std::cout << "[ " << gmaxTPCDataROC[0] << " / " << gmaxTPCDataROC[1] << "]" << std::endl; 
    return 0;
}

int TPCMapping::setsector2rcu(int i,char **val){
    if (i<3) return 0;
    char *s=val[0];
    char *s1=&s[2];
    u_short count = atoi(s1);
    u_short side  = (s[0]=='C');
    u_short type  = (s[1]=='O');
    u_short sector=count+ side *36+ type *18;

    if (sector>= MAXTPCSECTORS )  return 0;

    u_short sub=atoi(val[1]);
    if (sub>= MAXTPCRCUPERSECTOR ) return 0;

    u_short rcu=atoi(&val[2][1]);
    if (rcu>= MAXTPCRCU ) return 0;

    gTPCSECTOR[sector].status = 1;

    gTPCRCU2SECTOR[rcu].sector=sector;
    gTPCRCU2SECTOR[rcu].rcurow=sub;
    if (gTPCRCU2SECTOR[rcu].status==1){
      std::cout << "ERROR STATUS gTPCRCU2SECTOR[rcu=" << rcu << "].status" << std::endl; 
    }
    gTPCRCU2SECTOR[rcu].status=1;


    gTPCSECTOR2RCU[sector][sub].hwid=rcu;
    if (gTPCSECTOR2RCU[sector][sub].status==1){
      std::cout << "ERROR STATUS gTPCSECTOR2RCU[sector=" << sector << "][sub=" << sub << "].status" << std::endl;

    }   
    gTPCSECTOR2RCU[sector][sub].status=1;
    return 1;
}


int TPCMapping::GetRcuRow(u_short type,u_short fec, u_short *rcurow){
    
    int oo[5]={0,18,38,58,78};
    int ii[3]={0,18,43};
    int *ix;
    int ik=0;
    *rcurow=0;
    //  fprintf(stderr,"TPCMapping::GetRcuRowAndFecID: %d %d",type,fec);
    if (type) {
	ix = oo;
        ik =4;
    } else {
        ix = ii;
        ik = 2;
    }
    int k;
    for (k=0;k<ik;k++){
	if (fec<ix[k+1]&& fec>=ix[k] ){
            *rcurow=k;
	    //  fprintf(stderr," ix[%d]=%d < fec=%d < ix[%d]=%d\n",k, ix[k],fec,k+1 , ix[k+1]);
	}
    } 
//     fprintf(stderr,"TPCMapping::GetRcuRow:-> %d %d %d\n", *rcurow,fec, *fecid);
    return 0;
}


int TPCMapping::setsector2FECid(int i,char **val){
    if (i<4) return 0;
    char *s=val[0];
    char *s1=&s[2];
    u_short sector=atoi(s1)+(s[0]=='C')*36+(s[1]=='O')*18;
    if (sector>= MAXTPCSECTORS ) return 0;

    u_short sectorfecid=atoi(val[1]);
    if (sectorfecid>= MAXTPCFECPERSECTOR ) return 0;

    u_short fecid=atoi(val[2]);
    if (fecid>= MAXTPCFECPERRCU ) return 0;

    u_short fecsn=atoi(&val[3][1]);
    if (fecsn>= MAXTPCFEC ) return 0;

    gTPCSECTOR2FEC[sector][sectorfecid].hwid  =fecid;
    gTPCSECTOR2FEC[sector][sectorfecid].fecsn =fecsn;
    if (gTPCSECTOR2FEC[sector][sectorfecid].status==1)
      std::cout << "ERROR STATUS gTPCSECTOR2FEC[sector=" << sector  << "][sectorfecid=" << sectorfecid << "].status "<< std::endl;
    
    gTPCSECTOR2FEC[sector][sectorfecid].status=1;
    u_short type=0;
    if (s[1]=='O') type=1;
    u_short rcurow=0;
    GetRcuRow(type,sectorfecid, &rcurow);

    TPCSECTOR2RCU* rcu= &gTPCSECTOR2RCU[sector][rcurow];
    u_short rcuid=rcu->hwid;

    gTPCFEC2SECTOR[rcuid][fecid].sector = sector;
    gTPCFEC2SECTOR[rcuid][fecid].sectorfecid = sectorfecid;
    gTPCFEC2SECTOR[rcuid][fecid].fecsn  = fecsn;
    gTPCFEC2SECTOR[rcuid][fecid].rcurow = rcurow;
    if (gTPCFEC2SECTOR[rcuid][fecid].status == 1)
      std::cout << "ERROR STATUS gTPCFEC2SECTOR[rcuid=" << rcuid << "][fecid=" << fecid << "].status " << s << " sector=" << 
	sector << " type=" << type << " sectorfecid=" << sectorfecid << " rcurow=" << rcurow <<std::endl;
    gTPCFEC2SECTOR[rcuid][fecid].status= 1;

    return 1;
}

int TPCMapping::setaltro2fec(int i,char **val){
    if (i<7) return 0;
    if (atoi(val[0])<0) return 0;
    unsigned int hwaddr=atoi(val[6])+atoi(val[5])*16;
    // fprintf(stderr,"FEC2ALTRO %d -> %d\n",atoi(val[0]),hwaddr);
    gALTRO2FEC[hwaddr]=atoi(val[0]);
    gFEC2ALTRO[atoi(val[0])]=hwaddr;
    // unsigned int hwpasa=atoi(val[4])+atoi(val[3])*16;

    gALTRO2PASA[hwaddr]=atoi(val[0]);
    return 1;
}


int TPCMapping::readMap(char *path, char *fname,int whichmap){
    char filename[256];
    if (strlen(path)>0)
	sprintf(filename,"%s/%s",path,fname);
    else
	sprintf(filename,"%s",fname);

    FILE *fp=fopen(filename,"r");
    int n=100;
    char line[n];
    if (!fp) {
      std::cout << "Cannot open file " << filename << "! Exiting..." <<std::endl;
      TPCmappingExit(0);
    } else {
//	fprintf(stderr,"File %s opened.\n",filename);
    }

    int nch=0;
    int imax=10;
    char  *val[10];
    for (int i=0;i<10;i++) val[i] = (char *) malloc(20);

    int nline=0;
    while (!feof(fp)){
        fgets(line,n,fp);
        if (line[0]=='#') continue;
        nline++;
	char *s=line;
	int i=0;	
	char *token=strtok(s," ");

	while (token!=NULL && i< imax){
            strncpy(val[i],token,20);	    
	    i++;	    
	    token = strtok(NULL, " ");	    

	}
//	fprintf(stdout,"i=%d %d\n",i,whichmap);
        switch (whichmap){

	    case 1:  nch+=setsector2rcu(i,val);break;
	    case 2:  nch+=setsector2FECid(i,val);break;
	    case 3:  nch+=setaltro2fec(i,val);break; 
	    case 4:  nch+=setIROCcon2fec(i,val);break; 
	    case 5:  nch+=setOROCcon2fec(i,val);break; 
	}
        
    }

    if (fp) fclose(fp);
    std::cout << "[" << whichmap << "]   " << nch << " entries [total " << nline << "  lines] have been read from " << filename <<std::endl; 
    return 0;
}




 TPCMapping::TPCMapping(char *path){
    std::cout << "TPCMapping::TPCMapping(" << path << ")" << std::endl;
    int err;

    TPCr0[0]=800;
    TPCr0[1]=1320;
    TPCr0[2]=2600;

    Reset();
    err=readMap(path,"SECTOR2RCU.map",1); 
    err=readMap(path,"module2FECid.map",2); 
    err=readMap(path,"FEC2ALTRO.map",3);
    err=readMap(path,"data_iroc.map",4);
    err=readMap(path,"data_oroc.map",5); 
    Process();
}


int TPCMapping::Reset(){


    for ( int i=0;i< MAXTPCINDEX;i++)  gTPCDataROC[0][i].status=0;
    for ( int i=0;i< MAXTPCINDEX;i++)  gTPCDataROC[1][i].status=0;

    TPCHWChannel *ch;

    for (int ircu=0;ircu<GetNumberOfRCU();ircu++){
	gTPCRCU2SECTOR[ircu].status=0;
	for (int ifec=0;ifec<GetNumberOfFECPerRcu();ifec++){

	    gTPCFEC2SECTOR[ircu][ifec].status =0;
	    for (int ich=0;ich<GetNumberOfFECCh();ich++){
		ch = &gTPCHWChannel[ircu][ifec][ich];
                ch->status=0;
	    } 
	}
    }

    
    return 0;
}

int TPCMapping::testMapping(FILE *fp){

    TPCHWChannel *ch;

    for (u_short ircu=0;ircu<GetNumberOfRCU();ircu++){
	if (!is_connected_rcu(ircu)) continue;
	
	for (u_short ifec=0;ifec<GetNumberOfFECPerRcu();ifec++){
	    if (!is_connected_fec(ircu,ifec)) continue;
	    
	    for (u_short ich=0;ich<GetNumberOfFECCh();ich++){
		ch = &gTPCHWChannel[ircu][ifec][ich];
		u_short nsector, nrow, npad, mrcu, mfec, mch;

		int ier=GetPhysAddr( ircu, ifec, ich , &nsector, &nrow, &npad);
		    ier=GetHWAddr(nsector, nrow, npad, &mrcu, &mfec, &mch);	

	    if (ircu!=mrcu || ifec!=mfec || ich!=mch)
		fprintf(fp,"[%d]->  s=%d r=%d p=%d rcu=%d fec=%d ch=%d ->  rcu=%d fec=%d ich=%d \n",ier,nsector, nrow, npad, mrcu, mfec, mch, ircu, ifec, ich );

//		fprintf(fp,"RCU=%d FEC=%d CH=%d\n",ircu,ifec,ich);
	    } 
	}
    }

    

    return 0;
}

 u_short TPCMapping::GetHWAddr(u_short sector, u_short row, u_short col, 
			     u_short *rcu, u_short *fec, u_short *altroch  ){
	TPCSectorChannel *ch= &gTPCSectorChannel[sector][row][col];
   
	*rcu = ch->rcu;
	*fec = ch->fec;
	*altroch = ch->altroch;
	return ch->status;
    };

u_short TPCMapping::GetInfo(u_short rcuid, u_short fecid, 
			   u_short *fecsn, u_short *sectorfecid, u_short *rcurow){
    TPCFEC2SECTOR *ch= & gTPCFEC2SECTOR[rcuid][fecid];	
    *sectorfecid= ch->sectorfecid;
    *fecsn      = ch->fecsn;
    *rcurow     = ch->rcurow;
    return ch->status;
}

   u_short TPCMapping::GetPhysAddr(  u_short rcu, u_short fec, u_short altroch ,
				 u_short *sector, u_short *row, u_short *col ){
	TPCHWChannel *ch= &gTPCHWChannel[rcu][fec][altroch];
   
	*sector = ch->sector;
	*row = ch->row;
	*col = ch->pad;
	return ch->status;
    };    
u_short TPCMapping::GetPhysAddr(  u_short rcu, u_short chid ,
				 u_short *sector, u_short *row, u_short *col ){

    return GetPhysAddr( rcu,  (chid>>7),  chid & 0x7f ,sector,row, col );
}
 u_short TPCMapping::GetSectorIDFromName(const char *s){
     const char *s1=&s[2];
     u_short count = atoi(s1);
     u_short side  = (s[0]=='C');
     u_short type  = (s[1]=='O');
     u_short sector=count+ side *36+ type *18;
     return sector;
 }

int  TPCMapping::RowPad2XY(int oroc, int ir, int ip, float *x, float *y){
    static  float k1=10/6*tan(10/180.*M_PI);
    static  float k2=1.5*k2;
    int nir;
    float dpad;
    if (oroc==0){
	dpad=7.5;
	if (ir) nir=66+2*int((ir+2)/3.);
	else    nir=68;
	*x=2*nir-4*ip-2;
	*y=852.25+dpad*ir;
    } else {
	if (ir>63) {
	    dpad=15.; 
	    nir = 2* int( k2*(ir-64) + 56.66);
	    *y=dpad*(ir-64)+1993.5;
	} else {
	    dpad=10.;
	    nir = 2* int( k1*ir + 37.75);
	    *y   = dpad*ir+1351; 
	}
	*x=3*nir-6*ip-3;
    }
    return 0;
}

int   TPCMapping::XY2IRIP(int local ,int oroc,float x,float y,u_short &irow,u_short &ipad){

// conversion of the coordinates in the row-pad system
// x,y should be in the local coordinate system local=1

    int nir;
    float dpad;
    int ir, ip;
    static  float k1=10/6*tan(10/180.*M_PI);
    static  float k2=1.5*k2;
   
    if (oroc==0){
	dpad=7.5;
        if ( local  ) y=y+852.25; // move the o,o to the roc fixed point
	ir= int( ( y-852.25 ) / dpad);
	if (ir) nir=66+2*int((ir+2)/3.);
	else    nir=68;
	if (ir>62) return -1;
    } else {
	//if ( local  ) y=y+1993.5; // move the o,o to the roc fixed point
	if ( local  ) y=y+1351; // move the o,o to the roc fixed point
	if (y>= 1993.5) {
	    dpad=15.; 
	    ir  = int( (y-1993.5) / dpad);
	    if (ir<0) return -1;
	    ir=ir+64;	    
	    nir = 2* int( k2*(ir-64) + 56.66);
	} else {
	    dpad=10.;
	    ir  = int( (y-1351) / dpad  );
	    nir = 2* int( k1*ir + 37.75);
	}
	if (ir>95) return -1;
    }
    if (ir<0) return -1;
   

    ip = int(-(x/(2*(oroc+2))-0.5*(nir-1)));

    if (ip<0) return -1;
    if (ip>nir) return -1; 
    irow=ir;
    ipad=ip;
    fprintf(stdout,"%d y=%f ir=%d ip=%d\n",local,y,ir,ip);
    return 0;
}