# -*- coding: utf-8 -*-


import numpy as np
import cv2
from TheSquaire import TheSquaire
def nothing(*x):
    pass

def stable_match(ft_vec1,ft_vec2):

 

    
    obj1,dim1=ft_vec1.shape
    obj2,dim2=ft_vec2.shape
    
    if dim1!=dim2:
        print "dimensionen stimmen ned, machs nochmal!..missgeburt."
        return None
    if obj1<obj2:
        print "{} objekte im gegenwärtigen Zeitschritt zu viel detektiert".format(obj2-obj1)
        return None
    elif obj1>obj2:
        print "{} objekte fehlen im gegenwärtigen Zeitschritt".format(obj1-obj2)
        return None
        
    distances=np.zeros((obj1,obj2),dtype=np.float64)
    pref1=np.empty((obj1,obj2),dtype=np.uint8)
    pref2=np.empty((obj2,obj1),dtype=np.uint8)


    for i in xrange(obj1):
        for j in xrange(obj2):
            distances[i,j]=np.sum(np.square(np.subtract(ft_vec1[i],ft_vec2[j])))
    for i in xrange(obj1):
        pref1[i,:]=np.argsort(distances[i,:])
        
    for j in xrange(obj2):
        pref2[j,:]=np.argsort(distances[:,j])
    
    
    for i in xrange(obj2):    
        erstewahl=pref1[:,0]
        reps = np.where(np.bincount(erstewahl)> 1)[0]
        if len(reps)==0:
            break
        while reps.shape[0]>0:
            last, reps = reps[-1], reps[:-1]
            cur_rep_idx=np.argwhere(erstewahl==last)
            
            roll_idx=np.delete(cur_rep_idx,np.argmin(np.argwhere(pref2[last]==cur_rep_idx)[:,1]))
            for idx in roll_idx:
                pref1[idx,:]=np.roll(pref1[idx,:],-1)
    
    result=pref1[:,0]
    for i in range(obj1):
        print ft_vec1[i]," --> ",ft_vec2[result[i]]
    return result
    
rohling=np.zeros((576,512),dtype=np.uint8)

ts=TheSquaire()
cv2.namedWindow("rechteck",cv2.WINDOW_NORMAL)
cv2.namedWindow("rechteck2",cv2.WINDOW_NORMAL)
cv2.namedWindow("table",cv2.WINDOW_AUTOSIZE)
cv2.namedWindow("detected",cv2.WINDOW_AUTOSIZE)
cv2.createTrackbar("x1","rechteck",360,500,nothing)
cv2.createTrackbar("y1","rechteck",107,500,nothing)
cv2.createTrackbar("angle1","rechteck",12,360,nothing)
cv2.createTrackbar("a1","rechteck",123,500,nothing)
cv2.createTrackbar("b1","rechteck",63,500,nothing)
cv2.createTrackbar("x2","rechteck",354,500,nothing)
cv2.createTrackbar("y2","rechteck",405,500,nothing)
cv2.createTrackbar("angle2","rechteck",0,360,nothing)
cv2.createTrackbar("a2","rechteck",144,500,nothing)
cv2.createTrackbar("b2","rechteck",144,500,nothing)
cv2.createTrackbar("x3","rechteck2",105,500,nothing)
cv2.createTrackbar("y3","rechteck2",313,500,nothing)
cv2.createTrackbar("angle3","rechteck2",6,360,nothing)
cv2.createTrackbar("a3","rechteck2",78,500,nothing)
cv2.createTrackbar("b3","rechteck2",160,500,nothing)
cv2.createTrackbar("w1","rechteck2",20,100,nothing)
cv2.createTrackbar("w2","rechteck2",20,100,nothing)
cv2.createTrackbar("mir","rechteck2",25000,50000,nothing)
cv2.createTrackbar("mar","rechteck2",100000,100000,nothing)
key=cv2.waitKey(1)
old=[0,0,0,0]
attributes=["A ","cm","H1","H2"]
texte=None


while(key!=ord('q')):
    x1=cv2.getTrackbarPos("x1","rechteck")
    y1=cv2.getTrackbarPos("y1","rechteck")
    ang1=cv2.getTrackbarPos("angle1","rechteck")
    a1=cv2.getTrackbarPos("a1","rechteck")
    b1=cv2.getTrackbarPos("b1","rechteck")
    x2=cv2.getTrackbarPos("x2","rechteck")
    y2=cv2.getTrackbarPos("y2","rechteck")
    ang2=cv2.getTrackbarPos("angle2","rechteck")
    a2=cv2.getTrackbarPos("a2","rechteck")
    b2=cv2.getTrackbarPos("b2","rechteck")
    x3=cv2.getTrackbarPos("x3","rechteck2")
    y3=cv2.getTrackbarPos("y3","rechteck2")
    ang3=cv2.getTrackbarPos("angle3","rechteck2")
    a3=cv2.getTrackbarPos("a3","rechteck2")
    b3=cv2.getTrackbarPos("b3","rechteck2")
    w1=cv2.getTrackbarPos("w1","rechteck2")
    w2=cv2.getTrackbarPos("w2","rechteck2")
    minar=cv2.getTrackbarPos("mir","rechteck2")
    maxar=cv2.getTrackbarPos("mar","rechteck2")
    pts1=cv2.ellipse2Poly(center=(x1,y1),axes=(a1,b1),angle=ang1,arcStart=0,arcEnd=360,delta=1)
    rect1=cv2.minAreaRect(pts1)
    box1=np.int0(cv2.boxPoints(rect1))
    pts2=cv2.ellipse2Poly(center=(x2,y2),axes=(a2,b2),angle=ang2,arcStart=0,arcEnd=360,delta=1)
    rect2=cv2.minAreaRect(pts2)
    box2=np.int0(cv2.boxPoints(rect2))
    pts3=cv2.ellipse2Poly(center=(x3,y3),axes=(a3,b3),angle=ang3,arcStart=0,arcEnd=360,delta=1)
    rect3=cv2.minAreaRect(pts3)
    box3=np.int0(cv2.boxPoints(rect3))
    mask=rohling.copy()
    detect=rohling.copy()
    cv2.drawContours(mask, [box1,box2,box3], -1, 255, -1 )
    if texte is not None:
        for idn in texte:
            cv2.putText(mask,idn[0],(int(idn[1][0]),int(idn[1][1])), cv2.FONT_HERSHEY_SIMPLEX,0.5,0,1,cv2.LINE_AA)

        
    
    r=ts.rechtecke(src=mask,minarea=minar,maxarea=maxar)
    
    ind=0
    for item in r:
        m=cv2.moments(item)
        
        cv2.putText(mask,"{}".format(ind),(int(m['m10']/m['m00']),int(m['m01']/m['m00'])), cv2.FONT_HERSHEY_SIMPLEX,0.5,128,1,cv2.LINE_AA)
        ind+=1
        
    cv2.drawContours(detect , r, -1, 255, 3 )
    cv2.imshow("table",mask)
    cv2.imshow("detected",detect)
    key=cv2.waitKey(1)
    if key==ord("r"):
        print "recording detection session. {} rechtecks detected".format(len(r))
        print "assigning new identity..."
        ftvec1=np.empty((len(r),5),dtype=np.float64)
        texte=np.empty(len(r),dtype=object)
        index=0
        for item in r:
            m=cv2.moments(item)
            h=cv2.HuMoments(m)
            ftvec1[index]=[m['m00'],m['m10']/m['m00'],m['m01']/m['m00'],h[0][0]*1000,h[1][0]*1000]
            texte[index]=["{}".format(index),ftvec1[index][1:3:1]]
            
            
            
            index+=1
        
    if key==ord("t"):
        print "tracking with respect to previous recording"
        ftvec2=np.empty((len(r),5),dtype=np.float64)
        
        index=0
        for item in r:
            m=cv2.moments(item)
            h=cv2.HuMoments(m)
            ftvec2[index]=[m['m00'],m['m10']/m['m00'],m['m01']/m['m00'],h[0][0]*1000,h[1][0]*1000]
            index+=1
        res=stable_match(ftvec1,ftvec2)
        if res is not None:
            index=0
            for i in res:
                texte[index]=["{}".format(i),ftvec2[i][1:3:1]]
                index+=1
        else: print "FUCKED"   
        
    if key==ord("c"):
        print "compomentwise tracking with respect to previous recording"
        ftvec2=np.empty((len(r),5),dtype=np.float64)
        res=np.empty(4,dtype=np.object)
        index=0
        for item in r:
            m=cv2.moments(item)
            h=cv2.HuMoments(m)
            ftvec2[index]=[m['m00'],m['m10']/m['m00'],m['m01']/m['m00'],h[0][0]*1000,h[1][0]*1000]
            index+=1
        
        res[0]=stable_match(np.array(ftvec1[:,0]).reshape((len(r),1)),np.array(ftvec2[:,0]).reshape((len(r),1)))
        res[1]=stable_match(np.array(ftvec1[:,1:3:1]),np.array(ftvec2[:,1:3:1]))
        res[2]=stable_match(np.array([ftvec1[:,3]]).reshape((len(r),1)),np.array([ftvec2[:,3]]).reshape((len(r),1)))
        res[3]=stable_match(np.array([ftvec1[:,4]]).reshape((len(r),1)),np.array([ftvec2[:,4]]).reshape((len(r),1)))
        print res[0],res[1],res[2],res[3]
        std1=np.std(np.array(ftvec1[:,0]).reshape((len(r),1)))/np.mean(np.array(ftvec1[:,0]).reshape((len(r),1)))
        std2=np.std(np.array(ftvec1[:,1:3:1]))/np.mean(np.array(ftvec1[:,1:3:1]))
        std3=np.std(np.array([ftvec1[:,3]]).reshape((len(r),1)))/np.mean(np.array([ftvec1[:,3]]).reshape((len(r),1)))
        std4=np.std(np.array([ftvec1[:,4]]).reshape((len(r),1)))/np.mean(np.array([ftvec1[:,4]]).reshape((len(r),1)))
        print std1,std2,std3,std4
        final=(std1*res[0]+std2*res[1]+std3*res[2]+std4*res[3])/(std1+std2+std3+std4)
        print final
        arranged=np.zeros(len(final),dtype=np.uint8)
        index=0
        for i in np.argsort(final):
            arranged[i]=index
            index+=1
        print arranged


        
cv2.destroyAllWindows()
