import ROOT,argparse,random,math

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

#get random points
points=[]
mcpoints=[]

xpos=[2,1,0,1]
ypos=[2,1,0,2]
#xpos=[-1.06,-2,-1.03,-0.2,-0.4,1.67]
#ypos=[-2,-1.33,0.25,1.56,-0.87,1.7]

for i in range(len(xpos)):
    #x=random.randrange(-2,2)
    #y=random.randrange(-2,2)
    x=xpos[i]
    y=ypos[i]
    points.append(ROOT.TVector2(x,y))
    mcpoints.append(ROOT.TVector2(x+random.gauss(0,1),y+random.gauss(0,1)))

#calculate mean positions of points and mc points
mx=0
my=0
mcx=0
mcy=0
for p in points:
    mx+=p.X()
    my+=p.Y()
for p in mcpoints:
    mcx+=p.X()
    mcy+=p.Y()


mx/=len(points)
my/=len(points)
mcx/=len(mcpoints)
mcy/=len(mcpoints)
pmean=ROOT.TVector2(mx,my)
mcp=ROOT.TVector2(mcx,mcy)
#mcp=ROOT.TVector2(0.2,0.2)

stdErr=[0,0]
for p in points:
    stdErr[0]+=(p.X()-pmean.X())**2
    stdErr[1]+=(p.Y()-pmean.Y())**2

#calculate covarianz matrix
cov=ROOT.TMatrixD(2,2)
for p in points:
    dist=[0,0]
    dist[0]=abs(p.X()-pmean.X())
    dist[1]=abs(p.Y()-pmean.Y())
    c=ROOT.TMatrixD(2,1)
    '''
    print 'before',dist[0],dist[1]
    for i in range(2):

        if dist[i]<0.5:
            dist[i]=1./math.sqrt(12.)
    print 'after',dist[0],dist[1]    
    '''
    c[0][0]=dist[0]
    c[1][0]=dist[1]
    c_t=ROOT.TMatrixD(ROOT.TMatrixD.kTransposed,c)
    acov=ROOT.TMatrixD(c,ROOT.TMatrixD.kMult,c_t)
    cov+=acov
    #print 'acov:',c[0][0],c[1][0]
    #acov.Print()

'''
for i in range(2):
    for j in range(2):
        if i==j:
            if cov[i][j]==0:
                cov[i][j]=1./12.
#        else:
#            cov[i][j]=0
'''
cov*=1./len(points)

print 'cov='
cov.Print()

#get eigenvectors and eigenvalues of cov
eigenproblem=ROOT.TMatrixDEigen(cov)
eigenwert=eigenproblem.GetEigenValues()
eigenvector=eigenproblem.GetEigenVectors()
teigenvector=ROOT.TVector2(eigenvector[0][0],eigenvector[1][0])
teigenvector2=ROOT.TVector2(eigenvector[0][1],eigenvector[1][1])
eigenvector_i=ROOT.TMatrixD(ROOT.TMatrixD.kInverted,eigenvector)

print 'eigenwerte:',eigenwert[0][0],eigenwert[1][1]
print 'eigenvect: ('+str(teigenvector.X())+','+str(teigenvector.Y())+')','('+str(teigenvector2.X())+','+str(teigenvector2.Y())+')'
print 'errors:',math.sqrt(eigenwert[0][0]),math.sqrt(abs(eigenwert[1][1]))

#create drawable objects
pcov=ROOT.TEllipse(pmean.X(),pmean.Y(),math.sqrt(eigenwert[0][0]),math.sqrt(abs(eigenwert[1][1])),0,360,ROOT.TMath.RadToDeg()*teigenvector.Phi())
pcov.SetLineColor(6)
pcov.SetFillStyle(4000)
pcov_axis1=ROOT.TLine(pmean.X(),pmean.Y(),
                      pmean.X()+teigenvector.X()*math.sqrt(eigenwert[0][0]),
                      pmean.Y()+teigenvector.Y()*math.sqrt(eigenwert[0][0]))
pcov_axis1.SetLineColor(3)
pcov_axis2=ROOT.TLine(pmean.X(),pmean.Y(),
                      pmean.X()+teigenvector2.X()*math.sqrt(abs(eigenwert[1][1])),
                      pmean.Y()+teigenvector2.Y()*math.sqrt(abs(eigenwert[1][1])))
pcov_axis2.SetLineColor(3)


ppmean=ROOT.TPolyMarker()
ppmean.SetNextPoint(pmean.X(),pmean.Y())
ppmean.SetMarkerColor(2)
ppmean.SetMarkerStyle(20)
ppmean.SetMarkerSize(1)

pmcp=ROOT.TPolyMarker()
pmcp.SetNextPoint(mcp.X(),mcp.Y())
pmcp.SetMarkerColor(3)
pmcp.SetMarkerStyle(20)
pmcp.SetMarkerSize(1)

ppoints=ROOT.TPolyMarker()
for p in points:
    ppoints.SetNextPoint(p.X(),p.Y())
ppoints.SetMarkerStyle(20)
ppoints.SetMarkerSize(1)

#draw 
hsize=6
hist=ROOT.TH2D('hist','hist',100,-hsize,hsize,100,-hsize,hsize)
c1=ROOT.TCanvas('matrix','matrix',1000,1000)
c1.cd()
hist.SetStats(0)
hist.Draw()
pcov.Draw("same")
pcov_axis1.Draw("same")
pcov_axis2.Draw("same")
ppmean.Draw('same')
#pmcp.Draw('same')
ppoints.Draw('same')

#c1.SetGrid(5,5)
xlines=[]
ylines=[]
for i in range(-hsize,hsize):
    xlines.append(ROOT.TLine(0.5*(2*i+1),-hsize,0.5*(2*i+1),hsize))
    xlines[-1].SetLineStyle(3)
    xlines[-1].Draw()
    ylines.append(ROOT.TLine(-hsize,0.5*(2*i+1),hsize,0.5*(2*i+1)))
    ylines[-1].SetLineStyle(3)
    ylines[-1].Draw()

c1.Update()
u=raw_input('done?')