#include "Stntuple/obj/TCesDataBlock.hh"
#include "TCesClusterMaker.hh"

Int_t  TCesClusterMaker::ClusterWedgeFixed(
			     Int_t wedge, Int_t side, Int_t view)
{
  int seeds[128];
  float e[128];
  int n = (view==0? 32: 128);
  int nseg = (view==0? 2: 1);
  int istr = 0;
  float eScale = (view==0? 240.0 : 120.0);

  int i,j;

  for(int iseg=0; iseg<nseg; iseg++) {
    if(iseg>0) istr=32;
    for(int ii=0; ii<128; ii++) seeds[ii]=0;
    int nseeds=0;
    // mark potential seeds
    if(view==0) {
      for(i=0; i<n; i++) 
	e[i] = fCesBlock->GetWireData(side,wedge,i+istr)/eScale;
    } else {
      for(i=0; i<n; i++) 
	e[i] = fCesBlock->GetStripData(side,wedge,i+istr)/eScale;
    }
    for(i=0; i<n; i++) {
      if(e[i]>fSeed) { seeds[i] |= 1;  nseeds++; }
    } // end loop for marking seeds


    int highest;
    float emax;

    // mark the first seed
    emax = 0.0;  highest=-1;
    for(i=0; i<n; i++) {
      if(seeds[i]&1 && ((seeds[i]&4)==0) && e[i]>emax) {
	emax = e[i];
	highest = i;
      }
    }

    // make clusters
    while( highest>=0 ) {
      float sume = 0.0;
      float sumex = 0.0;
      int firstStrip,lastStrip;

      // count back while not at end, not used, and within cluster
      firstStrip=highest;
      while(firstStrip>0 && (seeds[firstStrip-1]&4)==0 && 
	    (highest-firstStrip)<(fClusNStrips-1)) firstStrip--;
      lastStrip=highest;
      while(lastStrip<(n-1) && (seeds[lastStrip+1]&4)==0 && 
	    (lastStrip-highest)<(fClusNStrips-1)) lastStrip++;

      if( (lastStrip-firstStrip+1)>=fClusNStrips) {  // then cluster found
	int nOnSide = fClusNStrips/2;
	if((lastStrip-highest)<nOnSide) { // cluster is not centered on seed
	  firstStrip = lastStrip - fClusNStrips + 1;
	} else {
	  firstStrip = highest - nOnSide;
	}
	for(i=0; i<fClusNStrips; i++) {
	  j = firstStrip+i;
	  seeds[j] |= 4; // mark used
	  sume += e[j];
	  sumex+= e[j]*Coord(side,view,j+istr);
	}

	TCesCluster* clu = new((*fClusters)[fNClusters++]) TCesCluster();
	int code = (fClusNStrips<<15) | (firstStrip<<7) | (wedge<<2) 
	                      | (side<<1) | view;
	clu->SetCode(code);
	clu->SetEnergy(sume);
	clu->SetCoord(sumex/sume);

      } else {
	// always mark the seed used or we will just come back to this seed
	seeds[highest] |= 4;
      }

      // find next seed
      emax = 0.0;  highest=-1;
      for(i=0; i<n; i++) {
	if(seeds[i]&1 && ((seeds[i]&4)==0) && e[i]>emax) {
	  emax = e[i];
	  highest = i;
	}
      }
      
    } // end loop for making clusters


  } // end loop over segments

  return 0;
} // end clustering


Int_t  TCesClusterMaker::ClusterWedgeVariable(
			     Int_t wedge, Int_t side, Int_t view)
{
  int seeds[128];
  float e[128];
  int n = (view==0? 32: 128);
  int nseg = (view==0? 2: 1);
  int istr = 0;
  float eScale = (view==0? 240.0 : 120.0);

  int i,j,k;

  for(int iseg=0; iseg<nseg; iseg++) {
    if(iseg>0) istr=32;
    for(int ii=0; ii<128; ii++) seeds[ii]=0;
    int nseeds=0;
    // mark potential seeds
    if(view==0) {
      for(i=0; i<n; i++) 
	e[i] = fCesBlock->GetWireData(side,wedge,i+istr)/eScale;
    } else {
      for(i=0; i<n; i++) 
	e[i] = fCesBlock->GetStripData(side,wedge,i+istr)/eScale;
    }
    for(i=0; i<n; i++) {
      if(e[i]>fSeed 
	 && (i==0 || e[i-1]<e[i]) 
	 && (i==n || e[i+1]<e[i]) 
	   ) { seeds[i] |= 1;  nseeds++; }
      if(e[i]>fShoulder) seeds[i] |= 2;
    } // end loop for marking seeds

    // check that the separation between peaks is good enough
    // to make separate clusters
    for(i=0; i<n-2; i++) {
      if(seeds[i]&1) {
	j=i+2;
	bool sep = false; 
	while(j<n && (seeds[j]&1)==0) j++;
	if(j<n) {
	  for(k=i+1; k<j; k++) {
	    if(e[k]<e[i]-fSeparationT && e[k]+1.0<e[j]-fSeparationT &&
	       e[k]/e[i]<fSeparationF && e[k]/e[j]<fSeparationF) {
	      sep=true;
	    }
	  } //loop over intermediate wires
	  if(!sep) { // not separated, kill smaller cluster
	    if(e[i]<e[j]) {
	      seeds[i] = seeds[i]&0xFFFFFFFE;
	    } else {
	      seeds[j] = seeds[j]&0xFFFFFFFE;
	    }
	  }
	} // found second seed
      } // endif this is a seed
    } // end loop for removing seeds


    // make clusters
    for(i=0; i<n; i++) {
      if(seeds[i]&1) {
	float sume = 0.0;
	float sumex = 0.0;
	int nstr = 0, firstStrip;
	sume += e[i];
	sumex+= e[i]*Coord(side,view,i+istr);

	// search below for the end of the cluster
	j=i-1;
	firstStrip = istr+i;
	while(j>=0 && (seeds[j]&1)==0 && (seeds[j]&2)!=0 ) j--;
	if(j<0 || (seeds[j]&2)==0) { // then just ran to edge
	  for(k=j+1; k<i; k++) {
	    sume += e[k];
	    sumex+= e[k]*Coord(side,view,k+istr);
	    nstr++;
	  }
	  firstStrip = istr+j+1;
	} else {  // hit another cluster
	  int d = (i-j-1)/2;  // number of full strips to assign to cluster
	  for(k=i-d; k<i; k++) {
	    sume += e[k];
	    sumex+= e[k]*Coord(side,view,k+istr);
	    nstr++;
	  }
	  firstStrip = istr+i-d;

	  if(2*d != (i-j-1) ) {
	    k = i-d-1;
	    float etemp = e[k];
	    etemp = etemp*(e[i]/e[j]);
	    sume += etemp;
	    sumex+= etemp*Coord(side,view,k+istr);
	    nstr++;
	    firstStrip = istr+k;
	  }
	}

	// search above for the end of the cluster
	j=i+1;
	while(j<=n && (seeds[j]&1)==0 && (seeds[j]&2)!=0 ) j++;
	if(j>n || (seeds[j]&2)==0) { // then just ran to edge
	  for(k=i+1; k<j; k++) {
	    sume += e[k];
	    sumex+= e[k]*Coord(side,view,k+istr);
	    nstr++;
	  }
	  firstStrip = istr+j-1;
	} else {  // hit another cluster
	  int d = (j-i-1)/2;  // number of full strips to assign to cluster
	  for(k=i+1; k<i+d+1; k++) {
	    sume += e[k];
	    sumex+= e[k]*Coord(side,view,k+istr);
	    nstr++;
	  }

	  if(2*d != (j-i-1) ) {
	    k = i+d+1;
	    float etemp = e[k];
	    etemp = etemp*(e[i]/e[j]);
	    sume += etemp;
	    sumex+= etemp*Coord(side,view,k+istr);
	    nstr++;
	  }
	}

	TCesCluster* clu = new((*fClusters)[fNClusters++]) TCesCluster();

	int code = (nstr<<15) | (firstStrip<<7) | (wedge<<2) 
	                      | (side<<1) | view;
	clu->SetCode(code);
	clu->SetEnergy(sume);
	clu->SetCoord(sumex/sume);

      } // endif this is a seed
    } // end loop for making clusters


  } // end loop over segments

  return 0;
} // end clustering


// channel # to local coord.  x is always in increasing phi
// unlike CES code.  z is same as CDF z
Float_t  TCesClusterMaker::Coord(int side, int view, int wire) { 
  if(view==0) {
    float x;
    int w = wire;
    if(wire>=32) w=wire-32;
    x = 1.45484*(float(w)-15.5);
    // edge wires are not ganged, therefore 1/2 pitch closer
    if(w==0) x += 1.45484/2.0;
    if(w==31) x -= 1.45484/2.0;
    if(side==0) x = -x;
    return x;
  } else {
    float z;
    int s = wire;
    if(s<64) {
      z = 6.16 + float(s)*1.667;
    } else {
      z = 121.183 + float(s-64)*2.007;
    }
    return (side==0? -z: z);
  } // endif on view
}

Int_t  TCesClusterMaker::ClusterWedge(Int_t wedge, Int_t side, Int_t view) { 
  int wed0=0,wed1=24,sid0=0,sid1=2,vew0=0,vew1=2;
  if(wedge>=0) { wed0=wedge; wed1=wedge+1;}
  if(side >=0) { sid0=side;  sid1=side+1; }
  if(view >=0) { vew0=view;  vew1=view+1; }
  int stat=0;
  for(int wed=wed0; wed<wed1; wed++) {
    for(int sid=sid0; sid<sid1 ; sid++) {
      for(int vew=vew0; vew<vew1 ; vew++) {
	if(fVariable) {
	  stat |= ClusterWedgeVariable(wed,sid,vew);
	} else {
	  stat |= ClusterWedgeFixed(wed,sid,vew);
	}
      }
    }
  }
  return stat;
}


Int_t  TCesClusterMaker::ClusterAll() { 
  for(int wed=0; wed<24; wed++) {
    for(int sid=0; sid<2 ; sid++) {
      for(int vew=0; vew<2 ; vew++) {
	ClusterWedge(wed,sid,vew);
      }
    }
  }
  return 0;
}

Int_t  TCesClusterMaker::NClusters(Int_t wedge, Int_t side, Int_t view) {
  int n=0;
  for(int i=0; i<NClusters(); i++) {
    if(wedge<0 || Cluster(i)->Wedge()==wedge) {
      if(side<0 || Cluster(i)->Side()==side) {
	if(view<0 || Cluster(i)->View()==view) {
	  n++;
	}
      }
    }
  }
  return n;
}