//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TpcSectorProcessor
//      see TpcSectorProcessor.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//
//
//-----------------------------------------------------------

// Panda Headers ----------------------

// This Class' Header ------------------
#include "TpcSectorProcessor.h"

// C/C++ Headers ----------------------
#include <iostream>
#include "assert.h"

// Collaborating Class Headers --------
#include "TpcPadPlane.h"
#include "TpcDigiMapper.h"
#include "TORPadProcessor.h"
#include "TpcCluster.h"

// Class Member definitions -----------


TpcSectorProcessor::~TpcSectorProcessor()
{
  // clean up PadProcessors
  std::map<unsigned int,padprocessor*>::iterator ipad=_pproc.begin();
  while(ipad!=_pproc.end()){
    delete ipad->second;
    ++ipad;
  }
  _pproc.clear();
}

void
TpcSectorProcessor::Init(TpcPadPlane* p,
			 unsigned int id,
			 std::vector<TpcCluster*>* ob)
{
  _padplane=p;
  _SectorId=id;
  _output_buffer=ob;

  // Build PadProcessors
  std::map<unsigned int, TpcPad*>* pads=_padplane->GetSectorList(_SectorId);
  std::map<unsigned int,TpcPad*>::iterator ipad=pads->begin();
  while(ipad!=pads->end()){
    _pproc[ipad->second->id()]=new padprocessor(ipad->second->id());
    _pproc[ipad->second->id()]->setClusterBuffer(&_cluster_buffer);
    ++ipad;
  }
  // Connect PadProcessor neighbours
  ipad=pads->begin();
  while(ipad!=pads->end()){
    unsigned int nneighb=ipad->second->nNeighbours();
    for(unsigned int in=0; in<nneighb; ++in){
      unsigned int neighID=ipad->second->getNeighbour(in);
      if(_pproc[neighID]!=0){
        _pproc[ipad->second->id()]->addNeighbour(_pproc[neighID]);
        //cout << "Pad " << ipad->second->id() << " connects to Pad " << neighID <<endl;
      }
      else std::cout << "Neighbour " << neighID 
		     <<" not in this Sector!!!" << std::endl;
    }
    ++ipad;
  }
}

void
TpcSectorProcessor::process(){
  
  _cluster_buffer.clear();

  unsigned int ndigis=_digi_buffer.size();
  //if(ndigis>0)std::cout<<ndigis<<" digis in sector "<<_SectorId<<std::endl;
  // loop over digis
  for(unsigned int i=0; i<ndigis; ++i){
    // fill padprocessors
    //std::cout<<"fill data in pproc "<<(_digi_buffer[i])->padId()<<std::endl;
    unsigned int digisid=_padplane->GetPad((_digi_buffer[i])->padId())->sectorId();
    //std::cout<<"in sector digisid="<<digisid<<" should be sector"<<_SectorId<<std::endl;
    padprocessor* pp=_pproc[_digi_buffer[i]->padId()];
    assert(pp!=NULL);
    pp->setData(_digi_buffer[i]);
    // activate pad processor by putting it into active list
    // this way only thos pads that have been hit are processing
    _activepads[_digi_buffer[i]->padId()]=pp;
  }


  // run padProcessors
  // TODO: stop criterion, how many heartbeats????
  bool GoOn=true;
  int counter=0;
  while(GoOn && counter<40){
    //cout<<"beat! ***************"<<endl;
    ++counter;
    GoOn=false;
    std::map<unsigned int, padprocessor*>::iterator ip=_activepads.begin();
    for(;ip!=_activepads.end(); ++ip){
      std::string nextstate=ip->second->heartbeat();
      if(nextstate!="initial"){
	GoOn=true;
	//std::cout<<"padproc_"<<ip->first<<" in state: "<<nextstate<<std::endl;
	//std::cout<<"ndata="<<ip->second->ndata()
	// <<"  iscenter="<<ip->second->isCenter()
	//       <<"  domneighb="<<ip->second->domNeighbour()<<std::endl;
      }
    }
  }
  
  //std::cout<<counter<<" heartbeats done"<<std::endl;

  if(_cluster_buffer.size()!=0){
    //std::cout<<"Sector "<<_SectorId<<": "
    //	     <<_cluster_buffer.size()<<" clusters built out of "
    //	     <<ndigis<<" digis."<<std::endl;
  }

  // cleanup ************
  _activepads.clear();
  _digi_buffer.clear();
  
  // calculate cog and create TpcClusters;
  cog();
}

void
TpcSectorProcessor::reset()
{
 _activepads.clear();
 _digi_buffer.clear();
 _cluster_buffer.clear();
 std::map<unsigned int, padprocessor*>::iterator ppit=_pproc.begin();
 while(ppit!=_pproc.end()){
   ppit->second->reset();
   ++ppit;
 }
}

void
TpcSectorProcessor::cog(){
  assert(_padplane!=NULL);
  assert(_output_buffer!=NULL);
  // Calculate COG
  for(unsigned int ic=0;ic<_cluster_buffer.size();++ic){
    std::vector<TpcDigi*>* digis=_cluster_buffer[ic];
    TVector3 pos(0,0,0);
    double amp=0;
    TVector3 sig(0,0,0);
    unsigned int id=0;
    McIdCollection mcid;
    unsigned int ndigis=digis->size();
    for(;id<ndigis;++id){
      TpcDigi* adigi=(*digis)[id];
      mcid.AddIDCollection(adigi->mcId());
      double a=(double)adigi->amp();
      TVector3 thispos;
      double dx;
      double dy;
      TpcDigiMapper::getInstance()->padsize(adigi->padId(),dx,dy);
      TVector3 thissig(dx*dx/12.,dy*dy/12.,0.2);
      sig+=a*a*thissig;

      TpcDigiMapper::getInstance()->map(adigi,thispos);
      pos+=a*thispos;
      amp+=a;
    }
    pos*=1./amp;
    sig.SetX(sqrt(sig.X())/amp);
    sig.SetY(sqrt(sig.Y())/amp);
    sig.SetZ(sqrt(sig.Z())/amp);

    TpcCluster* cl=new TpcCluster(pos,sig,(unsigned int)amp,id,ndigis);
    cl->SetMcId(mcid.DominantID());
    _output_buffer->push_back(cl);
  }
}