import numpy as np
from numpy.linalg import inv
import argparse
import imutils
import cv2
import smbus
import time
import serial
import time
from PIL import Image
import math

# inv = Inverse
# np.array = matrix zu Array fuer bearbeitung
# np.matrix = array zu matrix fuer berechnung
# Array.transpose() = Transponieren

bus = smbus.SMBus(1)

def Ausweichen(var):
    global mu
    global links
    global rechts 
    global Strecke
    global B
    global Winkel
    def writeNumber(value,opt1,opt2,opt3):
        l = False
        while l == False:
            try:
                bus.write_i2c_block_data(address1,value,[opt1,opt2,opt3])
                l = True
            except IOError:
                print "Error"
        #print "writeeee",value,"",speedr,"",speedl
        return -1
        #print "request"
    def Read():
        l = False
        while l == False:
            try:
                number = bus.read_i2c_block_data(address1,0,4)
                l = True
            except IOError:
                print "Error"
        #print "read"
        return number
    if var != 1:
        print " links ",links
        print " rechts ", rechts
        
        r2 = links
        r1 = rechts
        Win = 150
        writeNumber(6,r1,r2, Win)
        time.sleep(1)
        drive(5)
        time.sleep(0.5)
        
        Strecke = 0.0
        while Strecke < 35.0:
            print " Strecke " , Strecke
            drive(6)
        drive(4)
        time.sleep(1)
        r1 = links
        r2 = rechts 
        writeNumber(7,r1,r2,Win)
        time.sleep(1)
        Strecke = 0.0
        while Strecke < 35.0:
            print "Strecke " , Strecke 
            drive(6)
        drive(4)
        time.sleep(1)
        r2 = links
        r1 = rechts
        writeNumber(6,r1,r2,Win)
        time.sleep(1)
        drive(5)
        time.sleep(0.5)
    if var == 1:
        maxum = 0
        for i in range(0,33):
            for j in range(0,100):
                #if B[i][j] > 0:
                #    print "B " , B[i][j] , " i " , i , " j " , j 
                if B[i][j] > maxum:
                    #print " maximum " , maxum 
                    maxum = B[i][j]
                    indexi = i
                    indexj = j
        Winkelw = (math.atan((float(indexj)/10)-5)/(math.pi/180))+90
        print " m ", float(indexj)/10-5 , " Winkel " , Winkelw, " Winkel " , Winkelw+90
        if B[indexi][indexj] > 20:
            if Winkelw >= 90:
                drive(3)
                Winkelw = Winkelw-90
                Winkel = Winkel+90
            opt1 = Winkelw
            if Winkelw < 90 and Winkelw > 0:
                    l = False
                    while l == False:
                        try:
                            bus.write_i2c_block_data(address1,8,[int(opt1)])
                            l = True
                        except IOError:
                            print "Error"
                    Winkel = Winkel + opt1
        else:
            drive(2)
        time.sleep(1)
            
        

def scan(var):
    global maximum
    koord = 0.0
    for i in range(0,91):
        def writeNumber(value):
            l = False
            while l == False:
                try:
                    bus.write_byte(address1,value)
                    l = True
                except IOError:
                    print "write Error number :" , i
            
            return -1
        def request():
            void = bus.write_quick(address1)
            
        def Read():
            l = False
            while l == False :
                try:
                    number = bus.read_i2c_block_data(address1,0,3)
                    if number[1] != 255:
                        l = True
                    #print " number " , number , " i ", i
                except IOError:
                    print "Read Error number : ", i
            
            return number
        
        writeNumber(var)
        time.sleep(0.1)
        #request()
        number = Read()
        scanned[i][1] = number[2]-87
        mw = number[0] + (number[1]*255)
        
        if mw > koord:
            maximum = i
            koord = mw
        scanned[i][0] = 0.00000000109574871446108* int(mw) **4 -0.00000217491220041041* int(mw) **3+0.0016454325041205* int(mw) **2-0.58759856645998* int(mw) +95.4
        if i > 1 and (scanned[i-1][1] != 45 and scanned[i-2][1] != 44):
            if number[2]-87 != scanned[i-1][1]+1 and number[2]-87 != scanned[i-2][1]+2:
                #print "Entf ",scanned[i][0], " Winkel " ,scanned[i][1]
                scanned[i][1] = scanned[i-1][1]+1
                #print " NEU Entf ",scanned[i][0], " Winkel " ,scanned[i][1]+87
def drive(var):
    global ZZiel
    global g
    global mu
    global drehen
    global Encoder
    global Strecke
    p = math.pi/180
    def writeNumber(value):
        l = False
        while l == False:
            try:
                bus.write_i2c_block_data(address1,value,[int(speedl),int(speedr)])
                l = True
            except IOError:
                print "Error"
        #print "writeeee",value,"",speedr,"",speedl
        return -1
        #print "request"
    def Read():
        l = False
        while l == False:
            try:
                number = bus.read_i2c_block_data(address1,0,4)
                l = True
            except IOError:
                print "Error"
        #print "read"
        return number
    #print var
    print "mu ",mu
    #print "drive", var
    #while mu[z][0] != ZZiel[z][0]:
    if var == 2:
    
        speed = 90+(math.sqrt((mu[g][0]-ZZiel[g][0])**2))
        if drehen == 90 or drehen == 180:
            if mu[g][0] < ZZiel[g][0]:
                print " nach rechts"
                speedl = speed 
                speedr = 90
            elif mu[g][0] > ZZiel[g][0]:
                speedr = speed
                speedl = 90
                print " nach links"
            else:
                speedr = 90
                speedl = 90
        elif drehen == 0 or drehen == 270:
            if mu[g][0] > ZZiel[g][0]:
                speedl = speed 
                speedr = 90
                print " nach rechts"
            elif mu[g][0] < ZZiel[g][0]:
                speedr = speed
                speedl = 90
                print " nach links"
            else:
                speedr = 90
                speedl = 90
        
        time.sleep(0.1)
        number = Read()
        if number[1] > 10 or number[3] > 10:
            number[1] = 0
            number[3] = 0
        Encoder1 = number[0]+255.0*number[1]
        Encoder2 = number[2]+255.0*number[3]
        Encoder = float(((Encoder1+Encoder2)/2.0)/28.0)
        print " Encoder " , Encoder 
        #print " Encoder ",number, " Encoder a " , Encoder1, " Encoder b ", Encoder2
        Fahrt()
    elif var == 4:
        speedr = 0
        speedl = 0
        time.sleep(0.1)
        number = Read()
        if number[1] > 10 or number[3] > 10:
            number[1] = 0
            number[3] = 0
        Encoder1 = number[0]+255.0*number[1]
        Encoder2 = number[2]+255.0*number[3]
        Encoder = float(((Encoder1+Encoder2)/2.0)/28.0)
        #print " Encoder ",number, " Encoder a " , Encoder1, " Encoder b ", Encoder2
        Fahrt()
    elif var == 6:
        var = 2
        speedr = 90
        speedl = 90
        number = Read()
        if number[1] > 10 or number[3] > 10:
            number[1] = 0
            number[3] = 0
        Encoder1 = number[0]+255.0*number[1]
        Encoder2 = number[2]+255.0*number[3]
        Encoder = float(((Encoder1+Encoder2)/2.0)/28.0)
        print " Encoder " , Encoder
        Strecke = Strecke + Encoder 
        Fahrt()
    else:
        mu = np.array(mu)
        mu[2][0] = drehen 
        mu = np.matrix(mu)
        speedr = 0
        speedl = 0
    #print " rechts " ,speedr, " links " , speedl
    # wenn er von der festen Zielkoordinate abweicht wird die geschwindigkeit erneut berechnet und angepasst.
    #print "rechts " , speedr , " links ", speedl
    
    print " speed links " , speedl , " speed rechts " , speedr
    writeNumber(var)
    #request()
    #number = Read()
    #print " Encoder ",number
    print ""
    print ""
    
    #print number

def readNumber():
    number= bus.read_byte(address1)
    return number
    
def komp():
    def read_byte(adr):
     return bus.read_byte_data(address, adr)

    def read_word(adr):
        high = bus.read_byte_data(address, adr)
        low = bus.read_byte_data(address, adr+1)
        val = (high << 8) + low
        return val

    def read_word_2c(adr):
        val = read_word(adr)
        if (val >= 0x8000):
            return -((65535 - val) + 1)
        else:
            return val

    def write_byte(adr, value):
        bus.write_byte_data(address, adr, value)

    write_byte(0, 0b01110000)
    write_byte(1, 0b00100000) 
    write_byte(2, 0b00000000) 

    scale = 0.92

    x_offset = 71
    y_offset = 190
    x_out = (read_word_2c(3) - x_offset) * scale
    y_out = (read_word_2c(7) - y_offset) * scale
    z_out = (read_word_2c(5)) * scale

    bearing  = math.atan2(y_out, x_out) 
    if (bearing < 0):
        bearing += 2 * math.pi
    return math.degrees(bearing)
    
def Kamera():
    for i in range(0,1):
        global lafa
        (grabbed, frame) = camera.read()
        frame = imutils.resize(frame, width=600)
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    
        mask = cv2.inRange(hsv, greenLower, greenUpper)
        mask = cv2.erode(mask, None, iterations=2)
        mask = cv2.dilate(mask, None, iterations=2)
        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2]
        center = None
        maskr = cv2.inRange(hsv, redLower, redUpper)
        maskr = cv2.erode(maskr, None, iterations=2)
        maskr = cv2.dilate(maskr, None, iterations=2)
        cntsr = cv2.findContours(maskr.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2]
        centerr = None
    
        if len(cnts) > 0:
            print " gruen "
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            M = cv2.moments(c)
            center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
            if radius > 10:
                lafa = 1

        elif len(cntsr) > 0:
            print " rot "
            cr = max(cntsr, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(cr)
            M = cv2.moments(cr)
            centerr = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
            if radius > 10:
                lafa = 2
        else:
            print " wand "
            lafa = 0
            
def Fahrt():
    global p 
    global Winkel
    global NWinkel
    global Encoder
    global G
    global I
    global F
    global Ft
    global Gt
    global mu
    global map
    global maps
    global E
    global R
    
    mu = np.matrix(mu)
    F = np.matrix(F)
    Ft = F.transpose()
    I = np.matrix(I)
    E = np.matrix(E)
    R = np.matrix(R)
    #K = np.matrix(K)
    #Q = np.matrix(Q)
    Winkel = drehen #komp()
    Winkel = Winkel - NWinkel
    print " drehen " , drehen , " Winkel " , Winkel , " Nwinkel ", NWinkel
    G = [[0,0, -math.sin(Winkel*p)*Encoder],[0,0,math.cos(Winkel*p)*Encoder],[0,0,0]]
    G = I+Ft*G*F
    Gt = G.transpose()
    
    # if blabla dann drive(2) und ub sonst drive(3) und ud
    

    #if Winkel < 0.0:
    #    Winkel = Winkel +360.0
    #Winkel = 360.0 - Winkel 
    #print "Winkel ", Winkel
    #print " Encoder", Encoder
    print " mu 2 ",mu[2][0]
    p = math.pi/180
    u = [[math.cos(Winkel*p)*Encoder],[math.sin(Winkel*p)*Encoder],[Winkel-mu[2][0]]] # Motion Model Bewegung
    R = np.array(R)
    R[0][0] = Encoder*0.083
    R[1][1] = Encoder*0.019
    R[2][2] = 3.6
    R = np.matrix(R)
    print "Winkel ", Winkel, "p " , p
    #ud = [[0],[0],[Winkel-mu[2][0]]] # motion Model Drehung
    # Winkel Messung durch Kompass 
    
    u = np.matrix(u)
    #print "u",u
    #print u 
    
    mu = mu+(Ft*u) 
    #print mu
    #R = 0
    #R = 0
    mu = np.array(mu)
    while mu[2][0] >= 360.0:
        mu[2][0] = mu[2][0]-360.0
    mu = np.matrix(mu)
    #
    # Zeile 2 Prediction
    E = Gt*E*G+Ft*R*F
    #
    #print mu
    #print E
    # Berechnung von z
    
    #Kamera sucht Landmarke 
    #print lafa
    
    mu = np.array(mu)
    p = math.pi/180
    #print " Winkel ", mu[2][0] , " Y " ,mu[1][0] , " X ", mu[0][0]
    Eckpunkte[0][0] = mu[0][0]+math.cos((mu[2][0]-68.311)*p)*9.47
    Eckpunkte[0][1] = mu[1][0]+math.sin((mu[2][0]-68.311)*p)*9.47
    #print "00",Eckpunkte[0][0]
    #print "01",Eckpunkte[0][1]
    Eckpunkte[1][0] = mu[0][0]+math.cos((mu[2][0]+68.311)*p)*9.47
    Eckpunkte[1][1] = mu[1][0]+math.sin((mu[2][0]+68.311)*p)*9.47
    #print " "
    #print "10",Eckpunkte[1][0]
    #print "11",Eckpunkte[1][1]
    Eckpunkte[2][0] = mu[0][0]+math.cos((mu[2][0]-150.075)*p)*23.654
    Eckpunkte[2][1] = mu[1][0]+math.sin((mu[2][0]-150.075)*p)*23.654
    #print ""
    #print "20",Eckpunkte[2][0]
    #print "21",Eckpunkte[2][1]
    Eckpunkte[3][0] = mu[0][0]+math.cos((mu[2][0]+150.075)*p)*23.654
    Eckpunkte[3][1] = mu[1][0]+math.sin((mu[2][0]+150.075)*p)*23.654
    #print " "
    #print "30",Eckpunkte[3][0]
    #print "31",Eckpunkte[3][1]
    
    for i in range(0,4):
        Count = 0
        for j in range (0,4):
            if Eckpunkte[i][0] == Eckpunkte[j][0] and i != j:
                Eckpunkte[i][0] = Eckpunkte[i][0]+0.0001
            if Eckpunkte[i][0]>Eckpunkte[j][0]:
                Count = Count +1
        Reihenfolge[Count] = i
    #print Reihenfolge
    
    X = int(Eckpunkte[Reihenfolge[3]][0])-int(Eckpunkte[Reihenfolge[0]][0])
    #print X
    Xk = Eckpunkte[Reihenfolge[0]][0]-int(Eckpunkte[Reihenfolge[0]][0])
    Xk = 1-Xk
    
    if (Eckpunkte[Reihenfolge[0]][1] < Eckpunkte[Reihenfolge[1]][1] and Eckpunkte[Reihenfolge[3]][1] < Eckpunkte[Reihenfolge[1]][1] and  Eckpunkte[Reihenfolge[0]][1] < Eckpunkte[Reihenfolge[2]][1] and Eckpunkte[Reihenfolge[3]][1] < Eckpunkte[Reihenfolge[2]][1]):
        #beide groesser
        mun = (Eckpunkte[Reihenfolge[3]][1]-Eckpunkte[Reihenfolge[0]][1])/(Eckpunkte[Reihenfolge[3]][0]-Eckpunkte[Reihenfolge[0]][0])
        mf = (Eckpunkte[Reihenfolge[1]][1]-Eckpunkte[Reihenfolge[0]][1])/(Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0])
        ms = (Eckpunkte[Reihenfolge[2]][1]-Eckpunkte[Reihenfolge[1]][1])/(Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[1]][0])
        mt = (Eckpunkte[Reihenfolge[3]][1]-Eckpunkte[Reihenfolge[2]][1])/(Eckpunkte[Reihenfolge[3]][0]-Eckpunkte[Reihenfolge[2]][0])
        for i in range(-1,int(X)):
            x = i+Xk
            jo = 1
            
            yu = mun*x+1
            yu2 = mun*x
            if yu < yu2:
                yu = yu
            else:
                yu = yu2
                
            if x+1 < Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    yo = mf*(x+1)
                    #print "yo",yo
                    yo = Eckpunkte[Reihenfolge[0]][1]+yo
                    
            elif x+1 < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0] and x > Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0] and ms < 0:
                    nx = (x-ao)+nko
                    yo = ms*(nx)
                    yo = Eckpunkte[Reihenfolge[1]][1]+yo
            elif x > Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0] and x+1 < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0] and ms > 0:
                    nx = (x-ao)+nko
                    yo = ms*(nx+1)
                    yo = Eckpunkte[Reihenfolge[1]][1]+yo
            elif x > Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    nx = (x-ao)+nko
                    yo = mt*nx
                    yo = Eckpunkte[Reihenfolge[2]][1]+yo
            
            if i == -1:
                x = 0
                jo = Xk
                yu = mun*Xk
                yo = mf*Xk
                yo = Eckpunkte[Reihenfolge[0]][1]+yo
                
            if x+jo > Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0] and x < Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    ao = x+1
                    nko = ao - (Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0])
                    yo = Eckpunkte[Reihenfolge[1]][1]
                    #print "Eckpunkt", Eckpunkte[Reihenfolge[1]][0]
            if x+jo > Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0] and x < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    ao = x+1
                    nko = ao - (Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0])
                    yo = Eckpunkte[Reihenfolge[2]][1]
                    #print "Eckpunkt", Eckpunkte[Reihenfolge[2]][0]
            
            yu = Eckpunkte[Reihenfolge[0]][1]+yu
            #print yu," und",yo , " und ", (Eckpunkte[Reihenfolge[0]][0]+x) 
            for j in range(int(yu),int(yo)+1):
                mas = maps[int(Eckpunkte[Reihenfolge[0]][0]+x)][j]
        
                maps[int(Eckpunkte[Reihenfolge[0]][0]+x)][j] = mas+1
                map[int(Eckpunkte[Reihenfolge[0]][0]+x)][j] = (map[int(Eckpunkte[Reihenfolge[0]][0]+x)][j]*mas)/(mas+1)
            
    elif ( Eckpunkte[Reihenfolge[0]][1] > Eckpunkte[Reihenfolge[1]][1] and Eckpunkte[Reihenfolge[3]][1] > Eckpunkte[Reihenfolge[2]][1] and Eckpunkte[Reihenfolge[0]][1] > Eckpunkte[Reihenfolge[2]][1] and Eckpunkte[Reihenfolge[3]][1] > Eckpunkte[Reihenfolge[1]][1]):
        #beide kleiner
        mon = (Eckpunkte[Reihenfolge[3]][1]-Eckpunkte[Reihenfolge[0]][1])/(Eckpunkte[Reihenfolge[3]][0]-Eckpunkte[Reihenfolge[0]][0])
        mf = (Eckpunkte[Reihenfolge[1]][1]-Eckpunkte[Reihenfolge[0]][1])/(Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0])
        ms = (Eckpunkte[Reihenfolge[2]][1]-Eckpunkte[Reihenfolge[1]][1])/(Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[1]][0])
        mt = (Eckpunkte[Reihenfolge[3]][1]-Eckpunkte[Reihenfolge[2]][1])/(Eckpunkte[Reihenfolge[3]][0]-Eckpunkte[Reihenfolge[2]][0])
        for i in range(-1,int(X)):
            x = i+Xk
            jo = 1
            
            yo = mon*x
            yo2 = mon*x+1
            if yo > yo2:
                yo = yo
            else:
                yo = yo2
                
            if x+1 < Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    yu = mf*(x+1)
                    #print "yu",yu
                    yu = Eckpunkte[Reihenfolge[0]][1]+yu
                    
            elif x+1 < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0] and x > Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0] and ms < 0:
                    nx = (x-au)+nku
                    yu = ms*(nx+1)
                    yu = Eckpunkte[Reihenfolge[1]][1]+yu
            elif x > Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0] and x+1 < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0] and ms > 0:
                    nx = (x-au)+nku
                    yu = ms*(nx)
                    yu = Eckpunkte[Reihenfolge[1]][1]+yu
            elif x > Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    nx = (x-au)+nku
                    yu = mt*nx
                    yu = Eckpunkte[Reihenfolge[2]][1]+yu
            
            if i == -1:
                x = 0
                jo = Xk
                yu = mf*Xk
                yo = mon*Xk
                yu = Eckpunkte[Reihenfolge[0]][1]+yu
                
            if x+jo > Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0] and x < Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    au = x+1
                    nku = au - (Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0])
                    yu = Eckpunkte[Reihenfolge[1]][1]
                    #print "Eckpunkt", Eckpunkte[Reihenfolge[1]][0]
            if x+jo> Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0] and x < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    au = x+1
                    nku = au - (Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0])
                    yu = Eckpunkte[Reihenfolge[2]][1]
                    #print "Eckpunkt", Eckpunkte[Reihenfolge[2]][0]
            
            yo = Eckpunkte[Reihenfolge[0]][1]+yo
            #print yu," und",yo , " und ", (Eckpunkte[Reihenfolge[0]][0]+x)
            for j in range(int(yu),int(yo)+1):
                
                #print int(Eckpunkte[Reihenfolge[0]][0]+x) , " und " , j , " mo " , mon , " yo " , yo , " yu " , yu 
                mas = maps[int(Eckpunkte[Reihenfolge[0]][0]+x)][j]
        
                maps[int(Eckpunkte[Reihenfolge[0]][0]+x)][j] = mas+1
                map[int(Eckpunkte[Reihenfolge[0]][0]+x)][j] = (map[int(Eckpunkte[Reihenfolge[0]][0]+x)][j]*mas)/(mas+1)
    
        
    else:
        #print Xk
        m = (Eckpunkte[Reihenfolge[1]][1]-Eckpunkte[Reihenfolge[0]][1])/(Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0])
        m2 = (Eckpunkte[Reihenfolge[2]][1]-Eckpunkte[Reihenfolge[0]][1])/(Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0])
        if m < 0:
            mf = m
            ms = m2
            w = 1
        else:
            ms = m
            mf = m2
            w = 2
        m = (Eckpunkte[Reihenfolge[3]][1]-Eckpunkte[Reihenfolge[2]][1])/(Eckpunkte[Reihenfolge[3]][0]-Eckpunkte[Reihenfolge[2]][0])
        m2 = (Eckpunkte[Reihenfolge[3]][1]-Eckpunkte[Reihenfolge[1]][1])/(Eckpunkte[Reihenfolge[3]][0]-Eckpunkte[Reihenfolge[1]][0])
        if m < 0:
            mt = m
            mv = m2
        else :
            mv = m
            mt = m2
        #print mf, "", ms, "",mt,"",mv
        
        for i in range(-1,int(X)):
            x = i+Xk
            jo = 1
            if w == 1:
                if x+1 < Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    yu = mf*(x+1)
                    #print "yu",yu
                    yu = Eckpunkte[Reihenfolge[0]][1]+yu
                    #print " Ecke ",Eckpunkte[Reihenfolge[0]][1], " yu " , yu
                elif x > Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    nx = (x-au)+nku
                    yu = mv*nx
                    yu = Eckpunkte[Reihenfolge[1]][1]+yu
                if x+1 < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    yo = ms*(x+1)
                    yo = Eckpunkte[Reihenfolge[0]][1]+yo
                    
                    #print "yo 1",yo
                elif x > Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    nx = (x-ao)+nko
                    yo = mt*nx
                    yo = Eckpunkte[Reihenfolge[2]][1]+yo
                    #print "yo 2",yo
                    
                    
            if w == 2:
                if x+1 < Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    yo = ms*(x+1)
                    yo = Eckpunkte[Reihenfolge[0]][1]+yo
                    
                    #print "yo 3",yo , "ms " , ms , " x+1 " , x+1
                elif x > Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    nx = (x-ao)+nko
                    yo = mt*nx
                    yo = Eckpunkte[Reihenfolge[1]][1]+yo
                    
                    #print "yo 4",yo , " mt " , mt , " x " , x
                if x+1 < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    yu = mf*(x+1)
                    yu = Eckpunkte[Reihenfolge[0]][1]+yu
                elif x > Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    nx = (x-au)+nku
                    yu = mv*nx
                    yu = Eckpunkte[Reihenfolge[2]][1]+yu
                
            if i == -1:
                x = 0
                jo = Xk
                yu = mf*Xk
                yo = ms*Xk
                yu = Eckpunkte[Reihenfolge[0]][1]+yu
                yo = Eckpunkte[Reihenfolge[0]][1]+yo
            
            if w == 1:
                if x+jo > Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0] and x < Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    au = x+1
                    nku = au - (Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0])
                    yu = Eckpunkte[Reihenfolge[1]][1]
                    #print "Eckpunkt", Eckpunkte[Reihenfolge[1]][0]
                if x+jo > Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0] and x < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    ao = x+1
                    nko = ao - (Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0])
                    yo = Eckpunkte[Reihenfolge[2]][1]
                    #print "Eckpunkt", Eckpunkte[Reihenfolge[2]][0]
        
            if w == 2:
                if x+jo> Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0] and x < Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    ao = x+1
                    nko = ao - (Eckpunkte[Reihenfolge[1]][0]-Eckpunkte[Reihenfolge[0]][0])
                    #print "Eckpunkt 1", Eckpunkte[Reihenfolge[1]][0] , "nko",nko,"ao",ao
                    yo = Eckpunkte[Reihenfolge[1]][1]
                if x+jo > Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0] and x < Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0]:
                    au = x+1
                    nku = au - (Eckpunkte[Reihenfolge[2]][0]-Eckpunkte[Reihenfolge[0]][0])
                    #print "Eckpunkt 2", Eckpunkte[Reihenfolge[2]][0],"nku",nku,"au",au
                    yu = Eckpunkte[Reihenfolge[2]][1]
            
            
            #print yu," und",yo , " und ", (Eckpunkte[Reihenfolge[0]][0]+x)
            for j in range(int(yu),int(yo)+1):
                #print "Koordinatenen",j,"", int(Eckpunkte[Reihenfolge[0]][0]+x) , " yo ", yo
                mas = maps[int(Eckpunkte[Reihenfolge[0]][0]+x)][j]
        
                maps[int(Eckpunkte[Reihenfolge[0]][0]+x)][j] = mas+1
                map[int(Eckpunkte[Reihenfolge[0]][0]+x)][j] = (map[int(Eckpunkte[Reihenfolge[0]][0]+x)][j]*mas)/(mas+1)
    
def Scanner():  
    global scanned
    global la
    global mu
    global landmarks
    global I
    global F
    global E
    global Fj
    global K 
    #global map 
    #global maps
    global H
    global Q
    maximum = 0.0
    scan(1)
    scanned = np.matrix(scanned)
    #print scanned
    scanned = np.array(scanned)
    maximum = 0
    if scanned[0][0] <30.0 and scanned[1][0] > 30.0 and scanned[2][0] > 30.0:
        scanned[0][0] = 30.1
    if scanned[90][0] < 30.0 and scanned[89][0] > 30.0 and scanned[88][0] > 30.0:
        scanned[90][0] = 30.1
        if scanned[90][0] < scanned[maximum][0]:
            maximum = 90
    for i in range(1,90):
        if scanned[i][0] < 30.0 and scanned[i+1][0] > 30.0 and scanned[i-1][0] > 30.0:
            scanned[i][0] = 30.1
        if scanned[i][0] < scanned[maximum][0]:
            maximum = i
    la = 0
    
    #if (scanned[maximum-1][0] < 30.0 and scanned[maximum-2][0] < 30.0 and scanned[maximum-3][0] < 30.0) or (scanned[maximum+1][0] < 30.0 and scanned[maximum+2][0] < 30.0 and scanned[maximum+3][0] < 30.0):
    if scanned[maximum][0] < 30.0:
        Winkell = scanned[maximum][1]
        r=3
        Entf = scanned[maximum][0]+r
        la = 1
    print "la ",la
    
    
    #print Winkell
    
    #Winkell= 15 # Winkel zur Landmarke gemessen durch den Servo
    #Entf = 12 # gemessene Entfernung zur Landmarke
    Kamera()
    print "lafa", lafa
    if la == 1 and Entf <= 30.0 and lafa !=0:
        seen =0
        
        for j in range(0,len(landmarks)):
            #print len(landmarks)
            #print j
            
            if len(landmarks) != 0 :
                if landmarks[j][2] == lafa:
                    seen = 1
        mu = np.array(mu)
        Pos = ([mu[0][0],mu[1][0]])
        Winkel = mu[2][0]
        mu = np.matrix(mu)
        
        if Winkell != 0:
            yla = (math.cos(Winkell*p)*(Entf+konst))+konst
            xla = (math.sin(Winkell*p)*(Entf+konst))
        
            Winkelr = math.atan((xla/yla))/p
            Entfr = xla/math.sin(Winkelr*p)
        else: 
            xla = 0
            yla = Entf + 6
            Winkelr = 0
            Entfr = xla
        
        Winkelg = Winkel + Winkelr
        
        if Winkelg < 0:
            Winkelg = Winkelg + 360
        if Winkelg > 0:
            Winkelg = Winkelg - 360
        
        yr = math.sin(Winkelg*p)*Entfr
        xr = math.cos(Winkelg*p)*Entfr

        yl = Pos[1]+yr
        xl = Pos[0]+xr
        
        zm = [[Entfr],[Winkelg]]
        
        if seen == 0:
            
            mu = np.array(mu)
            mu = np.resize(mu,((len(mu)+2),1))
            mu[len(mu)-2][0] = xl
            mu[len(mu)-1][0] = yl
            I = [[0 for col in range(len(mu))] for row in range(len(mu))]
            for i in range(len(mu)):
                for j in range(len(mu)):
                    if i == j:
                        I[i][j] = 1
            En = [[0.0 for col in range(len(mu))] for row in range(len(mu))]
            En = np.array(En)
            E = np.array(E)
            for i in range(0,3):
                for j in range(0,3):
                    En[i][j] = E[i][j]
            E = En
            
            En = [[0 for col in range(3)] for row in range(((len(mu))-3)/2)]
            En = np.array(En)
            landmarks = np.array(landmarks)
            if len(landmarks) != 0:
                for i in range(0,len(landmarks)+1):
                    for j in range(0,3):
                        En[i][j] = landmarks[i][j]
            landmarks = En
            
            landmarks[len(landmarks)-1][0] = len(mu)-2
            landmarks[len(landmarks)-1][1] = len(mu)-1
            landmarks[len(landmarks)-1][2] = lafa
            
            F = [[0 for col in range(len(mu))] for row in range(0,3)] 
            F[0][0] = 1
            F[1][1] = 1
            F[2][2] = 1
            Fj = [[0 for col in range(len(mu))] for row in range(len(mu))] 
            Fj[0][0] = 1
            Fj[1][1] = 1
            Fj[2][2] = 1
        
            H = [[0 for col in range(len(mu))] for row in range(0,2)]
    
            
            mu = np.matrix(mu)
            F = np.matrix(F)
            Ft = F.transpose()
            I = np.matrix(I)
            E = np.matrix(E)
            K = np.matrix(K)
            Q = np.matrix(Q)
            zm = np.matrix(zm)   
            #seen = 1
        elif seen == 1:
            for i in range(0,len(landmarks)):
                if landmarks[i][2] == lafa:
                    lx = landmarks[i][0]
                    ly = landmarks[i][1]
            
            
            mu = np.array(mu)
            delta = [[mu[lx][0]-mu[0][0]],[mu[ly][0]-mu[1][0]]]
    
            mu = np.matrix(mu)
            delta = np.matrix(delta)
            deltat = delta.transpose()
    
            q = deltat*delta
            q = np.array(q)
            z = [[math.sqrt(q)],[math.atan((delta[1][0]/delta[0][0]))/p]]
            z = np.matrix(z)
            #
    
            # Erstellung von Fj
            Fj = np.array(Fj)
            for i in range(0,len(mu)):
                for j in range(0,len(mu)):
                    if (i == lx and j == lx) or ( i == ly and j == ly):
                        Fj[i][j] = 1
                    else:
                        Fj[i][j] = 0 
            Fj[0][0] = 1
            Fj[1][1] = 1
            Fj[2][2] = 1
            Fj = np.matrix(Fj)
                #

            #Berechnung von H
            delta = np.array(delta)
            H = np.array(H)
            for j in range (len(mu)):
                if j == lx: 
                    H[0][j] = math.sqrt(q)*delta[0][0]
                    H[1][j] = -delta[1][0]
                elif j == ly:
                    H[0][j] = math.sqrt(q)*delta[1][0]
                    H[1][j] = delta[0][0]
                else : 
                    H[0][j] = 0
                    H[1][j] = 0
            H[0][0] = -math.sqrt(q)*delta[0][0]
            H[0][1] =  -math.sqrt(q)*delta[1][0]
            H[1][0] = delta[1][0]
            H[1][1] = -delta[0][0]
            H[1][2] = -q
            H = 1/q*(H) 
            H = np.matrix(H)
            Ht = H.transpose()
            #
    
            # Zeile 3 Correction    
            A = (H*E*Ht+Q)
    
            K = E*Ht*inv((H*E*Ht+Q))
            #   
    
            # Zeile 4 Correction

            mu = mu + K * (zm-z)
            if mu[2][0] < 0.0:
                mu[2][0] = mu[2][0] + 360.0
            while mu[2][0] >= 360.0:
                mu[2][0] = mu[2][0]-360.0
                mu = np.matrix(mu)
            #
    
            # Zeile 5 Correction
    
            E = (I-K*H)*E
            
        # Karten Update
        print landmarks
        print mu
        mu = np.array(mu)
        Pos = ([mu[0][0],mu[1][0]])
        Winkel = mu[2][0]
        #globale Entfernung
        
        #scan 
        #Winkell= 15 # Winkel zur Landmarke gemessen durch den Servo
        #Entf = 12 # gemessene Entfernung zur Landmark
    for i in range(0,91):
        mu = np.array(mu)
        Pos = ([mu[0][0],mu[1][0]])
        Winkel = mu[2][0]
        Entf = scanned[i][0]
        if scanned[i][1]!=0:
            Winkell = scanned[i][1]*-1
        if Winkell != 0:
            yla = (math.cos(Winkell*p)*(Entf+konst))+konst
            xla = (math.sin(Winkell*p)*(Entf+konst))
        
            Winkelr = math.atan((xla/yla))/p
            Entfr = xla/math.sin(Winkelr*p)
        else: 
            xla = 0
            yla = Entf + 6
            Winkelr = 0
            Entfr = xla
        
        Winkelg = Winkel + Winkelr
        
        for i in range(0,100):
            mii = (float(i)/10)-5
            bii = -xla*mii+yla
            #print "m " , mii , " b " , bii
            if bii < 33.0 and bii > 5.0:
                
                B[int(bii)][i] = B[int(bii)][i]+1
        
        if Winkelg < 0:
            Winkelg = Winkelg + 360
        if Winkelg > 0:
            Winkelg = Winkelg - 360
        
        yr = math.sin(Winkelg*p)*Entfr
        xr = math.cos(Winkelg*p)*Entfr

        yl = Pos[1]+yr
        xl = Pos[0]+xr
        if Entf > 35.0: 
            Entf = 35.0

        xi = math.cos(Winkel*p)*3
        yi = math.sin(Winkel*p)*3

        Posir[0] = Pos[0]+xi #Position des Sensors
        Posir[1] = Pos[1]+yi
        Posin[0] = Pos[0]+(xi*-1)
        Posin[1] = Pos[1]+(yi*-1)


        # der globale Winkel der Landmarke
        Winkelg = Winkel + Winkelr

        Winkelg = Winkel + Winkell
        if Winkelg < 0:
            Winkelg = Winkelg + 360

        yr = math.sin(Winkelg*p)*(Entf+konst)
        xr = math.cos(Winkelg*p)*(Entf+konst)

        yl = Posir[1]+yr
        xl = Posir[0]+xr

        m = yr/xr
        
        mas = maps[int(Posir[0])][int(Posir[1])]
        
        maps[int(Posir[0])][int(Posir[1])] = mas+1
        map[int(Posir[0])][int(Posir[1])] = (map[int(Posir[0])][int(Posir[1])]*mas)/(mas+1)
        if Entf < 35.0:
            v = 1
        else:
            v = 0
        mas = maps[int(xl)][int(yl)] 
        #print "mas" , mas
        maps[int(xl)][int(yl)] = mas+1
        map[int(xl)][int(yl)] = (map[int(xl)][int(yl)]*mas+v)/(mas+1)
        
        
        
        if xl > Posir[0]:
            xz = xl
            yz = yl
            xg = Posir[0]
            yg = Posir[1]
        if xl < Posir[0]:
            xz = Posir[0]
            yz = Posir[1]
            xg = xl
            yg = yl
        xk = 1- (xg-int(xg))
        xt = xz-int(xz)
        b = yg - int(yg)

        if m >0:
            if xr < 0:
                xr = xr*-1
            if (int(xz)-int(xg)) > 0 :
                for i in range(-1,(int(xz)-int(xg))):
                    y = m*(i+xk)
                    yn = m*(i+1+xk)
                    x = i
                    if i == ((int(xz)-int(xg))-1):
                        x = i -xt
                        xk = xk + xt 
                        yn = m*(i+xk)
                    if i == -1:
                        y = m*(0)
                        yn = m*xk
                        x = -xk
                    if yn > yz:
                        yn = yz-yg
                    #print " y " , y+b , " yn " , yn+b , " b " , b , " x " , x , "Winkel " , Winkell
                    for j in range(int(y+b),int(yn+b+1)):
                        if not ((int(xg+x+xk) == int(xg) and int(yg+j) == int(yg)) or (int(xg+x+xk) == int(xz) and int(yg+j) >= int(yz))):
                            #print " X ", int(xg+x+xk) , "Y" , int(yg+j)
                            mas = maps[int(xg+x+xk)][int(yg+j)]
                            maps[int(xg+x+xk)][int(yg+j)] = mas+1
                            map[int(xg+x+xk)][int(yg+j)] = (map[int(xg+x+xk)][int(yg+j)]*mas)/(mas+1)
            elif (int(xz)-int(xg)) == 0:
                    y = m*(0)
                    yn = m*(xr)
                    for j in range(int(y+b),int(yn+b+1)):
                        if not ((int(xg+xr) == int(xg) and int(yg+j) == int(yg)) or (int(xg+xr) == int(xz) and int(yg+j) == int(yz))):
                            #print " Xi ", int(xg+xr) , "Y" , int(yg+j)
                            mas = maps[int(xg+xr)][int(yg+j)]
                            maps[int(xg+xr)][int(yg+j)] = mas+1
                            map[int(xg+xr)][int(yg+j)] = (map[int(xg+xr)][int(yg+j)]*mas)/(mas+1) 
        if m <0:
            if xr < 0:
                xr = xr*-1
            if (int(xz)-int(xg)) > 0 :
                for i in range(-1,(int(xz)-int(xg))):
                    y = m*(i+xk)
                    yn = m*(i+1+xk)
                    x = i
                    if i == ((int(xz)-int(xg))-1):
                        x = i -xt
                        xk = xk + xt 
                        yn = m*(i+xk)
                    if i == -1:
                        y = m*(0)
                        yn = m*xk
                        x = -xk
                    #print " x P " , (xg+x+xk) , " bis ", (xg+i+1+xk) , "y P" , (yg+y) , " bis " , (yg+yn)
                    for j in range(int(yg+yn),int(yg+y)+1):
                        if not ((int(xg+x+xk) == int(xg) and int(j) == int(yg)) or (int(xg+x+xk) == int(xz) and int(j) <= int(yz))):
                            #print " X ", int(xg+x+xk) , "Y" , int(j)
                            
                            mas = maps[int(xg+x+xk)][int(j)]
                            maps[int(xg+x+xk)][int(j)] = mas+1
                            map[int(xg+x+xk)][int(j)] = (map[int(xg+x+xk)][int(j)]*mas)/(mas+1)
            elif (int(xz)-int(xg)) == 0:
                    y = m*(0)
                    yn = m*(xr)
                    for j in range(int(yg+yn),int(yg+y)+1):
                        if not ((int(xg+xr) == int(xg) and int(j) == int(yg)) or (int(xg+xr) == int(xz) and int(j) <= int(yz))):
                            #print " X ", int(xg+xr) , "Y" , int(j)
                            mas = maps[int(xg+xr)][int(j)]
                            maps[int(xg+xr)][int(j)] = mas+1
                            map[int(xg+xr)][int(j)] = (map[int(xg+xr)][int(j)]*mas)/(mas+1)
    

address1 = 0x04
address = 0x1e

drehen = 0

p = math.pi/180 # Konstante fuer Winkel
Winkel = 0#komp() # Winkel Messung durch Kompass 
Encoder = 0.0 #Entfernungsmessung durch Encoder
Bewegung = 1 # Bewegungs ausfuehrung welche Bewegung ?
la = 1
seen = 0
a = 100 # Groesse der Karte in cm
b = 1 # Groesse eines Feldes in cm
n = 1
konst = 3 # Abstandsmessung
Posir = ([0,0])
Posin = ([0,0])
Eckpunkte = [[0,0],[0,0],[0,0],[0,0]]
Reihenfolge = [0,0,0,0]
c = ((math.sqrt(2)*(a/b))*2)+1
c = c+20
#print c
map = [[0.5 for col in range(int(c))] for row in range(int(c))] # Erstellung Karte
maps = [[0 for col in range(int(c))] for row in range(int(c))]
lafa = 0
greenLower = (29, 86, 6)
greenUpper = (64, 255, 255)
redLower = (0,111,129)
redUpper =(4,239,276)
camera = cv2.VideoCapture(0)
scanned = [[0 for col in range(0,2)] for row in range(0,91)]
B = [[0 for col in range(0,100)] for row in range(0,33)]

# Deklaration
Winkel = drehen #komp()
NWinkel = Winkel

mu = [[int(c/2)+0.5],[int(c/2)+0.5],[0]] # eintragen von Werten der Landmarken [221.63],[221.83]


Ziel = [[0],[0],[0],[0]]

Ziel[0][0] = mu[0][0]
Ziel[1][0] = mu[1][0]

Grenzen=[[0],[0],[0],[0]]

Grenzen[0] = mu[0][0]+24*((-1)**n)*-n
Grenzen[1] = mu[0][0]+24*((-1)**n)*-n
Grenzen[2] = Grenzen[1]+24*((-1)**(n+1))*-(n+1)
Grenzen[3] = Grenzen[1]+24*((-1)**(n+1))*-(n+1)+24

ZZiel = [[0],[0],[0],[0]]

ZZiel[0][0] = mu[0][0]
ZZiel[1][0] = mu[1][0]

landmarks = [[0 for col in range(3)] for row in range(((len(mu))-3)/2)]

R = [[math.cos(Winkel)*Entf*-0.009 + math.cos(Winkel-90)*Entf*0.017,0,0],[0,math.sin(Winkel)*Entf*-0.009 + math.sin(Winkel-90)*Entf*0.017,0],[0,0,0.9]] # bei Fahrten

Q = [[0.21,0],[0,0]] #

F = [[0 for col in range(len(mu))] for row in range(0,3)] 
F[0][0] = 1
F[1][1] = 1
F[2][2] = 1
Fj = [[0 for col in range(len(mu))] for row in range(len(mu))] 
Fj[0][0] = 1
Fj[1][1] = 1
Fj[2][2] = 1

I = [[0 for col in range(len(mu))] for row in range(len(mu))]
for i in range(len(mu)):
    for j in range(len(mu)):
        if i == j:
            I[i][j] = 1

E = [[0.0 for col in range(len(mu))] for row in range(len(mu))]
En = E

K = [[0 for col in range(len(mu))] for row in range(len(mu))]

H = [[0 for col in range(len(mu))] for row in range(0,2)]

#q = [[0,0],[0,0]]

# Umwandlung in Matrizen


# Zeile 1 Prediction
    
    
for i in range(0,4):
        
    Grenzen = np.array(Grenzen)
    if Ziel[1][0] == Ziel[0][0]:
        zi = 0
        Ziel[0][0] = Ziel[0][0]+24*((-1)**n)*-n
        if n%2 == 0:
            Grenzen[1] = Grenzen[1]+24
        if n%2!=0:
            Grenzen[3] = Grenzen[3]-24
        print "1.",Grenzen[0]," 2." , Grenzen[1], " 3." , Grenzen[2] , " 4.", Grenzen[3]
        #print n
        #print (-1)**n
    else:
        Ziel[1][0] = Ziel[0][0]
        zi = 1
        if n % 2 == 0:
            Grenzen[2] = Grenzen[2]-24
        if n % 2 != 0:
            Grenzen[0] = Grenzen[0]+24
       
    #print "Ziel 0" , Ziel[0][0] , "Ziel 1", Ziel[1][0]
    
    
    for i in range(0,n):
        
        time.sleep(0.5)
        Scanner()
        time.sleep(0.5)
        if la ==0:
            #print "    scan   "
            # wenn keine Landmarke oder Wand in diesem Bereich gefunden wurde, dann 
            if Ziel[1][0] == Ziel[0][0]:
                ZZiel[1][0] = ZZiel[1][0]+24*((-1)**n)*-1
                #print " X bleibt gleich "
                mu = np.array(mu)
                
                g = 0
                ZZiel[0][0]
                l = False
                while l == False:
                    try:
                        
                        if drehen == 0 or drehen == 90:
                            mu = np.array(mu)
                            while mu[1][0] < ZZiel[1][0]-2:
                                print " mu " ,mu, " Ziel " , ZZiel, " drehen " , drehen
                                #print "     erstens "
                                drive(2)
                        elif drehen == 180 or drehen == 270:
                            mu = np.array(mu)
                            while mu[1][0] > ZZiel[1][0]+2:
                                print " mu " ,mu, " Ziel " , ZZiel, " drehen " , drehen
                                #print "     erstens "
                                drive(2)
                        l = True
                    except IndexError:
                        print " Error index", mu, " Ziel: " ,ZZiel
            else:
                ZZiel[0][0] = ZZiel[0][0]+24*((-1)**n)*-1
                #print " Y bleibt gleich "
                mu = np.array(mu)
                g = 1
                ZZiel[1][0]
                l = False
                while l == False:
                    try:
                        if drehen == 0 or drehen == 90:
                            mu = np.array(mu)
                            while mu[0][0] < ZZiel[0][0]-2:
                                print " mu " ,mu, " Ziel " , ZZiel, " drehen " , drehen 
                                #print "     erstens "
                                drive(2)
                        elif drehen == 180 or drehen == 270:
                            mu = np.array(mu)
                            while mu[0][0] > ZZiel[0][0]+2:
                                print " mu " ,mu," Ziel " , ZZiel, " drehen " , drehen 
                                #print "     erstens "
                                drive(2)
                        l = True
                    except IndexError:
                        print" Error index" , mu, " Ziel ", ZZiel
        #while mu[0] or mu[1] != ZZiel[0] or ZZiel[1] Fahre if mu[1] or mu[0] != ZZiel[1] or ZZiel[0] korrigiere 
        if la == 1:
            if lafa == 0:
                print " hallo "
                Ausweichen(1)
            if lafa != 0:
                links = 0
                rechts = 0
                if scanned[maximum][1] > 0:
                    print " links ",scanned[maximum][1]
                    links = 1
                    rechts = 0
                    Ausweichen(2)
                if scanned[maximum][1] <= 0:
                    rechts = 1
                    links = 0
                    Ausweichen(2)
                    print "rechts ",scanned[maximum][1]
        drive(4)
        time.sleep(0.5)
        #print "    Fahrt nach X " ,ZZiel[0][0] , " Y " ,ZZiel[1][0] 
        # wenn eine Landmarke gefunden wurde dann ausweichen
    #print " stop"
    if Ziel[1][0] == Ziel[0][0]:
        n = n+1
    #print "    scan " 
    drehen = drehen + 90
    if drehen == 360:
        drehen = 0
    
    Winkel = drehen 
    #print "     Drehung"
    time.sleep(0.5)
    Scanner()
    time.sleep(1)
    drive(3)
    time.sleep(1)
    #print " drive = 5"
    drive(5)
    time.sleep(1)
    img = Image.new( 'L', (int(c),int(c)), "black") # create a new black image
    pixels = img.load() # create the pixel map

    map = np.array(map)
    for i in range(img.size[0]):    # for every pixel:
        for j in range(img.size[1]):
            if map[i][j] < 0.5:
                map[i][j] = 0
            pixels[i,j] = 255.0-int(map[i][j]*255.0) # set the colour accordingly
    if drehen == 90:
        img.save('/var/tmp/out.bmp')
    elif drehen == 180:
        img.save('/var/tmp/out2.bmp')
    elif drehen == 270:
        img.save('/var/tmp/out3.bmp')
    elif drehen == 0:
        img.save('/var/tmp/out4.bmp')
    
    #print "     Stop"
    #print "    Drehung um 90 Grad, Winkel = ", drehen
    #Kamera()
    #print lafa
    #drive(2)

# 391 1110 1309
#img = Image.new( 'L', (int(c),int(c)), "black") # create a new black image
#pixels = img.load() # create the pixel map

#map = np.array(map)
#for i in range(img.size[0]):    # for every pixel:
#    for j in range(img.size[1]):
#        if map[i][j] < 0.5:
 #           map[i][j] = 0
 #       pixels[i,j] = 255.0-int(map[i][j]*255.0) # set the colour accordingly

#img.save('/var/tmp/out.bmp')


