import ROOT, argparse, math, sys, os, copy, random, time
pandapath=os.environ.get('PANDAPATH')
sys.path.append(pandapath+'/macro/tpc/FOPI/mberger')


def circIntersection(m1,r1,m2,r2):
    positions=[]
    direction=[m2[0]-m1[0],m2[1]-m1[1]]
    
    #print m1,m2
    d=math.sqrt( (m2[0]-m1[0])**2 + (m2[1]-m1[1])**2 )
    x=( d**2 -r2**2 +r1**2 ) / (2 * d)
    a=math.sqrt( 4*(d**2)*(r1**2) - (d**2 - r2**2 + r1**2)**2 )/d

    direction[0]/=d
    direction[1]/=d

    perp_direction=[-direction[1],direction[0]]
    
    positions.append([m1[0] + x*direction[0] + a/2.*perp_direction[0],
                     m1[1] + x*direction[1] + a/2.*perp_direction[1]])
    positions.append([m1[0] + x*direction[0] - a/2.*perp_direction[0],
                     m1[1] + x*direction[1] - a/2.*perp_direction[1]])
    
    return positions

parser=argparse.ArgumentParser(description='check pad response')
parser.add_argument('--padf',help='the padplane file to use',type=str,default='tpc/parfiles/padPlane_FOPI.dat')
parser.add_argument('--pshape',help='pad shape file',type=str,default='tpc/parfiles/PadShapePrototype.dat')
parser.add_argument('--padres',help='padresponse file anme',type=str,default='tpc/FOPI/par/padresponse.txt')
parser.add_argument('--clsize',help='which clustersize to correct',type=int,default=2)

args=parser.parse_args()

ROOT.gROOT.ProcessLine(".x rootlogon.C") 
ROOT.gROOT.ProcessLine("gStyle->SetPalette(1)") 
ROOT.gROOT.ProcessLine('gROOT->SetStyle("Plain")')

DigiMapper = []
file = open(args.padf, "r")
lines=file.readlines()
file.close()
lines.reverse()
for line in lines:
    koord = line.split(" ") 
    DigiMapper.append([float(koord[3]), float(koord[4])])


gem=ROOT.TpcGem(2000,0.027)
padShapePool=ROOT.TpcPadShapePool(args.pshape,args.padres)
padplane=ROOT.TpcPadPlane(args.padf,padShapePool)



#rand=ROOT.TRandom3(4)

hitpads=[]
print 'get involved pads (fixed to a certain size)'
while len(hitpads)!=args.clsize:
    mcX=0
    mcY=0
    print 'generate a MC posistion of the avalanche'
    while (math.sqrt(mcX**2+mcY**2)<5 or math.sqrt(mcX**2+mcY**2)>15): 
        mcX=random.uniform(-15,15)
        mcY=random.uniform(-15,15)
    mcpos=ROOT.TVector3(mcX,mcY,0)
    print 'mc pos=',mcX,mcY
    hitpads=padplane.GetPadList(mcX,mcY,gem.spread())
    print len(hitpads),gem.spread()



print 'draw the involved pads and the MC position'
canvas=ROOT.TCanvas("cpadplane","padplane",1000,1000)
padplaneHist=ROOT.TH2D("hpadplane","padplane",3200,-16,16,3200,-16,16)
padplaneHist.GetXaxis().SetRangeUser(mcX-.5,mcX+.5)
padplaneHist.GetYaxis().SetRangeUser(mcY-.5,mcY+.5)
padplaneHist.Draw()
'''
print 'draw all pads'
for ipad in range(len(DigiMapper)-1):
    padplane.GetPad(ipad).Draw(1)
'''
print 'draw all hit pads and determine their amplitude and weight'
padamp=[]
padpos=[]
padweight=[]
for pad in hitpads:
    padamp.append(padplane.GetPad(pad).GetValue(mcX,mcY))
    padpos.append([DigiMapper[pad][0],DigiMapper[pad][1]])
    #padweight.append(padplane.GetPad(pad).GetWeight(
    padplane.GetPad(pad).Draw(2)
dmcpoint=ROOT.TPolyMarker()
dmcpoint.SetPoint(0,mcX,mcY)  
dmcpoint.SetMarkerStyle(7)    
dmcpoint.SetMarkerColor(4)    
dmcpoint.Draw('same')         

print 'calc weighted mean cluster pos'
clpos=ROOT.TVector3(0,0,0)
clamp=0
for pad in range(len(padpos)):
    clpos.SetX(clpos.X()+padpos[pad][0]*padamp[pad])
    clpos.SetY(clpos.Y()+padpos[pad][1]*padamp[pad])
    clamp+=padamp[pad]
clpos*=1./clamp

dclpos=ROOT.TPolyMarker()
dclpos.SetPoint(0,clpos.X(),clpos.Y())  
dclpos.SetMarkerStyle(7)    
dclpos.SetMarkerColor(2)    
dclpos.Draw('same')         

print 'get weight of digis and their correspondig circles'
padweight=[]
padcircs=[]
for ipad in range(len(hitpads)):
    dirX=padpos[ipad][0]-clpos.X()
    dirY=padpos[ipad][1]-clpos.Y()
    padweight.append(math.sqrt(dirX**2+dirY**2)*padplane.GetPad(hitpads[ipad]).GetWeight(padamp[ipad]/clamp,
                                                                                         dirX,
                                                                                         dirY))
    padcircs.append(ROOT.TEllipse(padpos[ipad][0],padpos[ipad][1],padweight[-1],0,0,360))
    padcircs[-1].SetFillStyle(4000)
    padcircs[-1].SetLineColor(2)
    padcircs[-1].Draw('same')
    print padweight[-1]

print 'calc circle radius weighted position'
clpos_circ=ROOT.TVector3(0,0,0)
clamp_circ=0
for ipad in range(len(hitpads)):
    clpos_circ.SetX(clpos_circ.X()+padpos[ipad][0]*1./padweight[ipad])
    clpos_circ.SetY(clpos_circ.Y()+padpos[ipad][1]*1./padweight[ipad])
    clamp_circ+=1./padweight[ipad]
clpos_circ*=1./clamp_circ

dclpos_circ=ROOT.TPolyMarker()
dclpos_circ.SetPoint(0,clpos_circ.X(),clpos_circ.Y())  
dclpos_circ.SetMarkerStyle(7)    
dclpos_circ.SetMarkerColor(3)    
dclpos_circ.Draw('same')       

clpos=ROOT.TVector3(clpos_circ)

print 'find intersection points'
intersect_pos=[]
for ipad in range(len(hitpads)):
    for jpad in range(ipad+1,len(hitpads)):
        intersect_pos.append(circIntersection(padpos[ipad],padweight[ipad],padpos[jpad],padweight[jpad]))

dintersect=ROOT.TPolyMarker()

for pair in intersect_pos:
    for inter in pair:
        print inter
        dintersect.SetNextPoint(inter[0],inter[1])
dintersect.SetMarkerStyle(5)
dintersect.SetMarkerColor(1)
dintersect.Draw('same')



canvas.Update()
u=raw_input()