from gamera.core import *
from gamera.core import Point,FloatPoint, ONEBIT, RGBPixel
import numpy as np

class Searcher2:
    '''Algoritmus pro trasovani trajektorii linek v jakemkoli uhlu.
    Moznost pouziti jakehokoli uhlu zajistuje natoceni obrazu v zadanem smeru
    a pouziti trasovani v sestupnem smeru.
    Rotace obrazu zmeni souradnice vstupniho bodu.
    Natoceni obrazu knihovnou GAMERA automaticky prizpusobi okraje obrazku
    a je potreba tuto skutecnost zahrnout do rovnice transformace.
    Pro jine uhly jak nasobky 90 knihovna pridava offset 2px od okraje.
    
    Vstupy:
    
    range - startovni sledovaci body
    
    img - vstupni obrazek
    
    imgrot - rotovany obrazek
    
    angle - uhel o ktery je obrazek rotovany
    
    sub - sirka sledovaciho kanalu
    
    jump - maximalni delka bile mezery (preruseni linky)
    
    black - minimalni pocet cernych pixelu v kanalu    
    '''
    def __init__(self, range, img, imgrot, angle, sub, jump, black):
        self.imgrot = imgrot
        self.img = img
        self.angle = angle
        self.subindex = sub
        self.whitejump = jump
        self.blackness = black
        self.lines = []
        self.border = imgrot.height
        if angle in [0, 90, 180, -90, -180]:
            factor = 0
        else:
            factor = 2
        self.a = np.cos(-angle * np.pi / 180)
        self.b = np.sin(-angle * np.pi / 180)
        self.cx = self.img.ncols / 2
        self.cy = self.img.nrows / 2
        self.shiftx = ((((1 - self.a) * self.cx) - (self.b * self.cy) + (
                    (np.abs(self.b) * self.img.height + np.abs(self.a) * self.img.width) / 2) - self.cx) + factor)
        self.shifty = (((self.b * self.cx) + ((1 - self.a) * self.cy) + (
                    (np.abs(self.a) * self.img.height + np.abs(self.b) * self.img.width) / 2) - self.cy) + factor)
        self.rangerx = self.transformpoint(range)

    def transformpoint(self, points):
        '''Vypocet afinni transformace rotace pro jeden bod v obraze, ktery ma orezane okraje
        '''
        return [[int(self.a * p[0] + self.b * p[1] + self.shiftx),
                 int(-self.b * p[0] + self.a * p[1] + self.shifty)] for p in points]

    def detransformpoint(self, points):
        '''Vypocet zpetne afinni transformace rotace pro jeden bod v obraze, ktery ma orezane okraje
        '''
        return [[int(self.a * p[0] - self.b * p[1] - self.a * self.shiftx + self.b * self.shifty),
                 int(self.b * p[0] + self.a * p[1] - self.b * self.shiftx - self.a * self.shifty)] for p in points]

    def search2(self, maxlen):
        '''Trasovani probiha smerem dolu v sirce sledovaciho kanalu po vsechny body.
        Algoritmus postupuje, dokud neni prekrocena podminka maximalni mezery.
        Cerne pixely v radku nesplnujici podminku stanoveneho minimalniho poctu
        jsou povazovany za bile.
        Vysledna stanocena delka je ve vzdalenosti od pocatku do posledni platne detekce cernych pixelu.
        Algoritmus take urci pribliznou horizontalni polohu stredu linky, na zaklade
        nejcasteji se vyskytujicich cernych pixelu v kanalu.
        
        Vystupy:
        
        list argumentu
        
        Hodnoty v indexech jednoho argumentu pro jeden bod:
           
        argument[0] - vzdalenost
        
        argument[1] - zpetne transformovany pocatecni bod
        
        argument[2] - index bodu
        
        argument[3] - faul - primost linky
        
        argument[4] - zpente transformovany konecny bod linky
        
                        x - stredni vyskyt cernych pixelu v horizontalni poloze
                        
                        y - konecna poloha linky
        '''
        for i, lin in enumerate(self.rangerx):
            point = lin[1]
            faul = 0
            whitenum = 0
            middle = lin[0]
            minindex = lin[0] - self.subindex
            maxindex = lin[0] + self.subindex + 1
            if maxindex < 0:
                minindex = 0
            if maxindex > self.imgrot.width:
                maxindex = self.imgrot.width
            minmaxrange = range(minindex, maxindex)
            minmaxrange = sorted(minmaxrange, key=lambda x: (abs(x-lin[0])))

            while True:
                black = 0
                finfedix = -1
                finfedi = -1
                if (point > self.border):
                    point = self.border
                    break
                if (point - lin[1] >= maxlen):
                    break
                for i, ix in enumerate(minmaxrange):
                    pixel = self.imgrot.get(Point(ix, point))
                    if (pixel > 0):
                        black = black + 1
                        if( finfedix == -1):
                            finfedix = ix
                            finfedi = i
                if black >= self.blackness:
                    middle = middle + finfedix
                    faul = faul + finfedi - self.subindex
                    whitenum = 0
                    point = point + 1
                    continue
                if (whitenum < self.whitejump):
                    point = point + 1
                    middle = middle + lin[0]
                    faul = faul + self.subindex*2
                    whitenum = whitenum + 1
                    continue
                else:
                    break
            point = point - ((whitenum))
            middle = middle - whitenum*lin[0]
            disty = point - lin[1]
            self.lines.append([disty, self.detransformpoint([lin]), i, faul, self.detransformpoint([[middle/(abs(disty)+1),point]])])
        return self.lines