#include "funkce.h"
//#include "tracking.h"
//#include "Settings.h"
//#define COM "COM1"
using namespace std;
PICTURE obraz2;
int threshold=20;
int DIFpredchozi=0,DIF,SUMACE;
//pokus------------------------------------------------------------------------

char* pokus(void){return "verze 0.5";}

//vykresleni do okna-----------------------------------------------------------
//picture musi mit velikost podle skutecnych dat snimanych kamerou, stejne jako rows a cols
//pracuje se s adresou obrazovych bodu v pameti, pri nedodrzeni presnych rozmeru aplikace spadne
void show(PICTURE picture,unsigned  char* pixel)
{
int radek=0,sloupec=0;
//pixel=pixel_init;
int pom=0;
for (int i=0;i<3*rows*cols;i++,pixel++,pom++)            //vykreselni obsahu picture
{

 *pixel=picture[rows-1-radek][sloupec];                 //bez 479- obraci obraz vzhuru nohama
 if (pom==3)                                         //lol to funguje jak zoom :)
 {
 pom=0;
 sloupec++;
 if(sloupec>cols-1)
{sloupec=0;radek++;}
 }
}
}


//nacteni dat z kamery-------------------------------------------------------
//picture musi mit velikost podle skutecnych dat snimanych kamerou, stejne jako rows a cols
//pracuje se s adresou obrazovych bodu v pameti, pri nedodrzeni presnych rozmeru aplikace spadne
//mod urcuje ktere baravne kanaly se budou ukladat
void read(PICTURE picture,unsigned  char* pixel,int kanal)
{
int radek=0,sloupec=0;
switch (kanal)
{
case 0:  //vsechny kanaly
        for (int i=0;i<3*rows*cols;i+=3,pixel+=3)
        {
        int a,b,c,bw;
        a=*pixel ;b=*(pixel+1);c=*(pixel+2) ;
        //(int)bw=(a+b+c)/3 ;
        (int)bw=(0.114*a+0.587*b+0.299*c);
        if(sloupec>cols-1)
                {sloupec=0;radek++;}
        picture[rows-1-radek][sloupec]=bw;
        sloupec++;
        };
break;
case 1: //kanal1      //zrejme modry
       for (int i=0;i<3*rows*cols;i+=3,pixel+=3)
        {
        int bw;
        bw=*pixel ;
        if(sloupec>cols-1)
                {sloupec=0;radek++;}
        picture[rows-1-radek][sloupec]=bw;
        sloupec++;
        };
break;
case 2: //kanal2    //zelena
        pixel++;
      for (int i=0;i<3*rows*cols;i+=3,pixel+=3)
        {
        int bw;
        bw=*pixel ;
        if(sloupec>cols-1)
                {sloupec=0;radek++;}
        picture[rows-1-radek][sloupec]=bw;
        sloupec++;
        };
break;
case 3: //kanal3     //cervena
        pixel+=2;
      for (int i=0;i<3*rows*cols;i+=3,pixel+=3)
        {
        int bw;
        bw=*pixel ;
        if(sloupec>cols-1)
                {sloupec=0;radek++;}
        picture[rows-1-radek][sloupec]=bw;
        sloupec++;
        };
break;
}
}


//prahovani obrazu 2------------------------------------------------------------
//umoznuje prahovat i obraz jinych rozmeru nez defaultnich rows a cols
// kanal 0 je cerna cara v obraze, jinak svetla cara v obraze
void thresholding2(PICTURE obraz,int kanal,int minimum_t,int r,int s)
{
int G1=0,G2=0,pocG1=0,pocG2=0;
for(int i=0;i<r;i++)
{for(int j=0;j<s;j++)
{
//cara v obraze bude mit hodnotu 255 okoli 0
/*if(obraz[i][j]<threshold)     //pro svetlou caru
obraz[i][j]=0;
else
obraz[i][j]=255; */

if(obraz[i][j]<threshold)
{
G1+=obraz[i][j];
if(kanal==0)
obraz[i][j]=255;
else
obraz[i][j]=0;
pocG1++;
}
else
{
G2+=obraz[i][j];
if(kanal==0)
obraz[i][j]=0;
else
obraz[i][j]=255;
pocG2++;
}
}
}
if(pocG1==0)
pocG1=1;
if(pocG2==0)
pocG2=1;
threshold=(G1/pocG1+G2/pocG2)/3;    //zmeno velikosti delitele lze regulovat umisteni aut prahu
if(threshold<minimum_t)threshold=minimum_t;       //omezeni minimalniho prahu
}


//filtr sumu-------------------------------------------------------------------
//obycejnym prumerovanim 3*3
void noisefilter(PICTURE obraz,int r,int s)
{
r=r-1;
s=s-1;
//rohy
obraz[0][0]=int(0.25*(obraz[0][1]+obraz[1][1]+obraz[1][0]+obraz[0][0]));
obraz[r][0]=int(0.25*(obraz[r][1]+obraz[r-1][1]+obraz[r-1][0]+obraz[r][0]));
obraz[0][s]=int(0.25*(obraz[0][s]+obraz[1][s]+obraz[0][s-1]+obraz[1][s-1]));
obraz[r][s]=int(0.25*(obraz[r][s]+obraz[r-1][s]+obraz[r-1][s-1]+obraz[r][s-1]));
for (int a=1;a<r;a++)
    {
    obraz[a][0]=int((0.1666)*(obraz[a][0]+obraz[a][1]+obraz[a+1][0]+obraz[a+1][1]+obraz[a-1][0]+obraz[a-1][1]));
    obraz[a][s]=int((0.1666)*(obraz[a][s]+obraz[a][s-1]+obraz[a+1][s]+obraz[a+1][s-1]+obraz[a-1][s]+obraz[a-1][s-1]));
    }
for (int a=1;a<s;a++)
    {
    obraz[0][a]=int((0.1666)*(obraz[0][a]+obraz[1][a]+obraz[0][a+1]+obraz[1][a+1]+obraz[0][a-1]+obraz[1][a-1]));
    obraz[r][a]=int((0.1666)*(obraz[r][a]+obraz[r-1][a]+obraz[r][a+1]+obraz[r-1][a+1]+obraz[r][a-1]+obraz[r-1][a-1]));
    }

for (int x=1;x<r;x++)
    {
    for (int y=1;y<s;y++)
       {
       obraz[x][y]=int((0.1111)*(obraz[x+1][y-1]+obraz[x+1][y]+obraz[x+1][y+1]+obraz[x][y-1]+obraz[x][y]+obraz[x][y+1]+obraz[x-1][y-1]+obraz[x-1][y]+obraz[x-1][y+1]));
       }
    }
}


//sobeluv operator 3x3 s detekci prahu---------------------------------------

void sobel2(PICTURE obraz,int minimum_t,int r,int s)
{
int ax,ay,G1=0,G2=0,pocG1=0,pocG2=0;
int a;
for(int x=1;x<(r-1);x++)
    {for(int y=1;y<(s-1);y++)
    {
        ax=(obraz[x+1][y-1]+2*obraz[x+1][y]+obraz[x+1][y+1]-obraz[x-1][y-1]-2*obraz[x-1][y]-obraz[x-1][y+1]);
        ay=(obraz[x-1][y-1]+2*obraz[x][y-1]+obraz[x+1][y-1]-obraz[x-1][y+1]-2*obraz[x][y+1]-obraz[x+1][y+1]);
        a=(abs(ax)+abs(ay));
        //obraz2[x][y]=a/8;
      if((a)>threshold)     //pro tmavou caru
        {
        G1+=a;
        obraz2[x][y]=255;
        pocG1++;
        }
        else
        {
        G2+=a;
        obraz2[x][y]=0;
        pocG2++;
        }
    }
    }
    if(pocG1==0)
        pocG1=1;
        if(pocG2==0)
        pocG2=1;
        threshold=(G1/pocG1+G2/pocG2)/1.8;    //zmeno velikosti delitele lze regulovat umisteni aut prahu
        if(threshold<minimum_t)threshold=minimum_t;

obraz2[0][0]=0;
obraz2[r][0]=0;
obraz2[0][s]=0;
obraz2[r][s]=0;
for (int a=1;a<r;a++)
    {
    obraz2[a][0]=0;
    obraz2[a][s]=0;
    }
for (int a=1;a<s;a++)
    {
    obraz2[0][a]=0;
    obraz2[r][a]=0;
    }
    for(int x=0;x<r;x++)
    {for(int y=0;y<s;y++)
    {
    obraz[x][y]=obraz2[x][y];
    }
    }
}


// precteni caroveho kodu z cary------------------------------------------------
//cte Industrial interleaved 2/5  , pomer 1:0 je 2:1,pro prahovany obraz
//Pro hranovany obraz je nutná mírná úprava
//predpoklada se dokonale detekovana cara bez chyb v prahovani
//mozna upravit detekci delky 0a1 zade se bere jako prvni detekovany symbol
//pouze pro přímku rovnobeznou s pohybem robota
int GetCrossNumber(PICTURE obraz,LINE primka,int con)
{
int s,z=-1;
float Lof0=0;
int i,i_prev=0,x=0;
int l=1,l_prev=1;
int linelenght=0;
int linelenghts[cols],bits[cols]; //vektor detekovanych šířek segmentů
for(i=0;i<(cols);i++)
{
        float pom4=sin(primka[0]/180*3.14159);
        if(pom4==0)
        pom4=0.0001;
        int row=(((primka[1]-i*cos(primka[0]/180*3.14159))/pom4));
        if(row>=0 & row<rows)
        {
                l=obraz[rows-row-1][i];  //postup po bodech cary
                if(l==128)
                l=(l_prev);   //ochrana pred sedymi body z detekce cary
                //obraz[rows-row-1][i]=128;      //označení bodů kterými procházíme
                if(l_prev!=l)            //hledani zmeny barvy
                {
                        linelenght=i-i_prev;   //vypocet delky segmentu
                        i_prev=i;
                        //if(linelenght!=0 & l_prev==0)   //Pro hranovaný obraz
                         if(linelenght!=0)                //Pro prahovaný obraz
                        {
                        linelenghts[x]=linelenght;  //ulozeni vsech delek
                        x++;
                        }
                }
        }
l_prev=l;
}


int finaln=-1;
for(int w=0;w<x;w++)
{
//hlavni cyklus, vsechny zjistene delky jsou pouzity jako delka znaku 0
//pokud nejaka dalka vyhovi (dostaneme cislo), je cyklus ukoncen
z=-1;
//rozdeleni na 1 a 0
for(int v=w;v<x;v++)
{
                if(v==w)
                Lof0=linelenghts[v];
                if(Lof0!=0)
                {
                //rozdeleni na 1 a 0 podle delek segmentu
                //v prahovanem obraze muhou hrany vyrazne zkracovat delku 0
                //je nutno peclive urcit meze
                if(linelenghts[v]>(Lof0*1.3) & linelenghts[v]<(Lof0*3))
                {z++;bits[z]=1;}
                if(linelenghts[v]<(Lof0*1.3) )//& linelenghts[v]>(Lof0*0.3))
                {z++;bits[z]=0;}

                }
}

int numbers=0,number=-1;
int code,power=0;
int Pom[cols];
if(z==16 | z==26) //kod ma dostatecnou delku (zde predpokladame pouze 2 nebo 4 cislice
{
        //pocet predpokladanech cislic ulozenych v kodu
        numbers=(z-6)/5;
        // otoceni pokud cteme pospatku
        if(bits[0]==0 &  bits[1]==0 & bits[2]==1)
        {
                for(int j=0;j<(z+1);j++)
                Pom[j]=bits[j];

                for(int j=0;j<(z+1);j++)
                bits[j]=Pom[z-j];
        }
        //start a stop znak, predpoklada se kod precteny bez chyb
        if(bits[0]==0 &  bits[1]==0 & bits[2]==0 & bits[3]==0 & bits[4+5*numbers]==1 &  bits[5+5*numbers]==0 & bits[6+5*numbers]==0)
        {finaln=0;
                //v kodu muuze byt libovolny sudy pocet cislic,urceni kazde zvlast
                for(int k=0;k<(numbers/2);k++)
                //for(int k=1;k<2;k++)
                {
                        for(int j=0;j<2;j++)
                        //for(int j=0;j<1;j++)
                        {
                        code=bits[4+j+k*10]*10000+bits[6+j+k*10]*1000+bits[8+j+k*10]*100+bits[10+j+k*10]*10+bits[12+j+k*10];

                        switch (code)
                                {
                                case 110: number=0;break;
                                case 10001: number=1;break;
                                case 1001: number=2;break;
                                case 11000: number=3;break;
                                case 101: number=4;break;
                                case 10100: number=5;break;
                                case 1100: number=6;break;
                                case 11: number=7;break;
                                case 10010: number=8;break;
                                case 1010: number=9;break;
                                default: number=-1;
                                }
                        if(number!=(-1))
                        {finaln=finaln+number*pow(10,numbers-1-power);
                        power++;}
                        else
                        {finaln=-1;goto end;}
                        }
                }
        }

}
else
finaln=-1;
end:;
if(finaln!=-1)
break;
}
return finaln;

}

//Oznacovani stredu moznych primek----------------------------------------------------------
//oznaci stred mezi dvema bilymi carami (hrany) pokud je jejich vzdalenost mezi min a max
//volba min a max by mela respektovat zuzeni cary ke konci pokud se nedivame kolmo
//a take vetsi sirku cary pokud je cara naklonena

void MarkPosLine(PICTURE obraz,int max,int min)
{
int pom1=0;
for(int r=0;r<(rows-1);r=r+5)
        {
        for(int s=0;s<(cols-1);s++)               //postupuj po radcich prochazej sloupce
                {
                if(obraz[r][s]==0 & obraz[r][s+1]==255)    //najdes-li natupnou hranu
                        {
                        if((s+max)>cols)     //osetreni maximalni adresy v sloupci
                        pom1=cols-1;
                        else
                        pom1=s+max;
                        for(int l=(s+min);l<(pom1);l++)   //hledej ve vzdalenosti mezi min a max
                                {
                                if(obraz[r][l]==255 & obraz[r][l+1]==0)     //setupnou
                                {
                                        obraz[r][(l+s)/2]=128;              //nales-li, oznac stred mezi hranami sedou barvou
                                        l=s+max;
                                }
                                }
                        }
                }
        }
int pom2=0;
for(int s=0;s<(cols);s=s+5)                  //obdobne projdeme po sloupcich a procahzime radky
        {                                     //protoze primka muze prochazet i podel sloupcu
        for(int r=0;r<(rows-1);r++)
                {
                if(obraz[r][s]==0 & obraz[r+1][s]==255)
                        {
                        if((r+max)>rows)
                        pom2=rows-1;
                        else
                        pom2=r+max;
                        for(int l=(r+min);l<(pom2);l++)
                                {
                                if(obraz[l][s]==255 & obraz[l+1][s]==0)
                                {
                                        obraz[(l+r)/2][s]=128;
                                        l=r+max;
                                }
                                }
                        }
                }
        }
}

// houghova transformace--------------------------------------------------------
void hough(PICTURE picture,LINE primka1,LINE primka2,int *con1,int *con2,int b_row,int b_col,int e_row,int e_col,int row,int col)
{
//uhel po 3 stupnich od 0 do 360, pouzijeme pouze kladne hodnoty
//mozny pocet hodnot r je podelen 5
//sloupce jsou uhel, radky r
//pro 640x480
const int max_r=160;   //maximalni velikos r
int matrix[160][120];  //0-119*3 je 0-357 stupnu
for(int i=0;i<160;i++)
      for(int j=0;j<120;j++)
      matrix[i][j]=0;            //nulovani matice
for(int s=b_col;s<e_col;s++)//x
     {
     for(int r=b_row;r<e_row;r++)//y
      {
      if(picture[row-r][s]==128) //adresace radku a sloupcu  ,row-r protoze matice obrazu zacina vlevo nahore
        {//       y     x

         for(int j=0;j<120;j++)
                {
                float fi=((j*3)/180.0)*3.14159;//ziskani stupnu v radianech
                int rfi=(s*cos(fi)+r*sin(fi))/5;
                if(rfi>=0 & rfi<max_r)
                (matrix[rfi][j])++;
                }
        }
       }
      }

      int maxfi1=-1,maxr1=-1,matrixvalue1=0;       //hledani nejsilnejsi primky
      for(int i=0;i<160;i++)
      for(int j=0;j<120;j++)
      if(matrix[i][j]>matrixvalue1)
      {matrixvalue1=matrix[i][j];maxfi1=j*3;maxr1=i*5;}
      primka1[0]=maxfi1;primka1[1]=maxr1;  //ulozeni parametru do promenne
      *con1=matrixvalue1;

      //nulovani okoli jiz nalezeneho uhlu a jeho protejsku
      //protoze pouzivame pouze kladne hodnoty, pokracuje primka s fi 130 jako 310...
      int uhel=0,uhel_primka1=0,rozdil=0;
      uhel_primka1=(int)primka1[0];
      if(uhel_primka1>180)
      uhel_primka1=uhel_primka1-360;
      for(int v=0;v<120;v++)
      {
       uhel=v*3;
       if(uhel>180)
       uhel=uhel-360;
       rozdil=abs(uhel-uhel_primka1);
       //vymazani hodnot v okoli +-15 stupnu kolem nejsilnejsi primky
       if( rozdil<20 | (rozdil>160 & rozdil<200) )
       for(int i=0;i<160;i++)
       matrix[i][v]=0;
      }

      //hledani druhe silne primky
      int maxfi2=-1,maxr2=-1,matrixvalue2=matrixvalue1*0.3;      //cara musi by mit urcitou silu vyhledem k nejsilnejsi care
      for(int i=0;i<160;i++)                    //hledani pouze silne primky
      for(int j=0;j<120;j++)
      {
      if(matrix[i][j]>matrixvalue2)
      {matrixvalue2=matrix[i][j];maxfi2=j*3;maxr2=i*5;}
      }
      primka2[0]=maxfi2;primka2[1]=maxr2;
      if(maxfi2!=(-1))
      *con2=matrixvalue2;
      else
      *con2=0;
}

//transformace reprezentaci-----------------------------------------------------
//prevadi primky aby P1 byla, pokud je to mozne, mezi -45 a 45 stupni
void Transform(/*LINE primka11,LINE primka22,*/LINE primka33,LINE primka44,int *con1,int *con2)
{
int x1,x2;
LINE pomp;
int pomc;

if((primka33[0]<45 | primka33[0]>=315))
{
primka33=primka33;
primka44=primka44;
}
else
{
pomp[0]=primka33[0];
pomp[1]=primka33[1];
primka33[0]=primka44[0];
primka33[1]=primka44[1];
primka44[0]=pomp[0];
primka44[1]=pomp[1];
pomc=*con1;
*con1=*con2;
*con2=pomc;
}
}

//Kalibrace sirky cary----------------------------------------------------------
int Cal(PICTURE obraz,int *max,int *min)
{
int leva[48],prava[48],sirka[48],re=-1,pom1,pom2;
for(int r=0;r<rows;r=r+10)
        {
        for(int s=1;s<(cols-1);s++)               //postupuj po radcich prochazej sloupce
                {
                if(obraz[r][s]==0 & obraz[r][s+1]==255)    //najdes-li natupnou hranu
                        {
                         leva[r/10]=s;
                         break;
                        }
                }

        }
for(int r=0;r<rows;r=r+10)
        {
        for(int s=1;s<(cols-1);s++)               //postupuj po radcich prochazej sloupce
                {
                if(obraz[r][cols-s]==0 & obraz[r][cols-s-1]==255)    //najdes-li natupnou hranu
                        {
                         prava[r/10]=cols-s;
                         break;
                        }
                }

        }

for(int r=0;r<48;r++)
sirka[r]=prava[r]-leva[r];
        pom1=(sirka[1]+sirka[2]+sirka[3])/3;
        pom2=(sirka[47]+sirka[46]+sirka[45])/3;
        if(pom1>pom2)
        {
         *max=(pom1*1.5)+(0.3*pom1*1.5);  //s tol 30% a pro max sklon 50°
         *min=(pom2)-(0.3*pom1);
        }
        else
        {
         *max=(pom2*1.5)+(0.3*pom2*1.5);  //s tol 30% a pro max sklon 50°
         *min=(pom1)-(0.3*pom1);
        }
        if(*max>cols) *max=cols;
        if(*min<1) *min=1;
return re;
}

//vypocteni smeru1 hough--------------------------------------------------------------
//kladny vlevo, zaporny vpravo
float smer_h(LINE primka,int r,int s)
{
float dolnimez,hornimez;
float pom=cos(primka[0]/180*3.14159);
if(pom==0)
pom=0.0001;
for (int i=0;i<r;i++)
{
    if ((((primka[1]-i*sin(primka[0]/180*3.14159))/pom)<(s))&(((primka[1]-i*sin(primka[0]/180*3.14159))/pom)>0))
        {dolnimez=(float)i;
        break ;}
}
for (int i=dolnimez;i<r;i++)
{
    if ((((primka[1]-i*sin(primka[0]/180*3.14159))/pom)>(s))|(((primka[1]-i*sin(primka[0]/180*3.14159))/pom)<0))
        {hornimez=(float)i;
        break;}
    if (i==(r-1))
        hornimez=(float)i;
}
float delta=hornimez-dolnimez;
float krok=(delta*0.1111);
float pocatek=hornimez-3*krok;
float uhel=(atan(((primka[1]-pocatek*sin(primka[0]/180*3.14159))/pom-320)/pocatek))/3.14159*180;
return uhel;
}

//vypocteni smeru 2------------------------------------------------------------
//Zaporny vlevo, kladnyvpravo
float smer_kolmy_h(LINE primka,int r,int s)
{
float dolnimez,hornimez,pomocna;
float pom2=sin(primka[0]/180*3.14159);
if(pom2==0)
pom2=0.0001;
for (int i=0;i<s;i++)
{
    if ((((primka[1]-i*cos(primka[0]/180*3.14159))/pom2)<r)&(((primka[1]-i*cos(primka[0]/180*3.14159))/pom2)>0))
        {dolnimez=((primka[1]-i*cos(primka[0]/180*3.14159))/pom2);
        //dolnimez=i;
        pomocna=i;
        break;}
}
for (int i=pomocna;i<s;i++)
{
    if ((((primka[1]-i*cos(primka[0]/180*3.14159))/pom2)>r)|(((primka[1]-i*cos(primka[0]/180*3.14159))/pom2)<0))
        {hornimez=((primka[1]-i*cos(primka[0]/180*3.14159))/pom2);
        //hornimez=i;
        break;}
    if (i==(s-1))
        //hornimez=i;
        hornimez=((primka[1]-i*cos(primka[0]/180*3.14159))/pom2);
}
float pom;
if(dolnimez>hornimez)
    {pom=dolnimez;
    dolnimez=hornimez;
    hornimez=pom;}
float delta=hornimez-dolnimez;
float krok= (delta*0.1111);
float pocatek=hornimez-3*krok;
float pom3=cos(primka[0]/180*3.14159);
if(pom3==0)
pom3=0.0001;
float uhel=(atan(((primka[1]-pocatek*sin(primka[0]/180*3.14159))/pom3-320)/(pocatek)))/3.14159*180;
return uhel;
}

//konecny smer a rozhodnuti se na krizovatce (cesta - 0 rovne,1doleva,2doprava)--------
float kam_h(LINE primka1,LINE primka2,int con1,int con2,int cesta,bool *je_kriz)
{                                    //return value smeru2
float pom,kam1,rozdil;
int fi1p,fi2p;

//zakomentovat pokud bude delat problemy ------------
//opozdeni zacatku odbocovani na krizovatce
int c_row,c_col,cross;
cross=CrossPos(primka1,primka2,&c_row,&c_col);
//Form1->Edit10->Text=c_row;
//Form1->Edit11->Text=c_col;
//Form1->Edit12->Text=cross; 
// --------------------------------------------------

bool krizovatka;
    fi1p=primka1[0];
    fi2p=primka2[0];
    if(fi1p>180)
    fi1p=fi1p-360;
    if(fi2p>180)
    fi2p=fi2p-360;
if(fi1p>fi2p)
rozdil=fi1p-fi2p;
if(fi2p>fi1p)
rozdil=fi2p-fi1p;
//if( con2>20 & con1>20 & rozdil<110 & rozdil>70 & c_row<(360) & cross==1 )
if( con2>20 & con1>20 & rozdil<110 & rozdil>70)
    {krizovatka=1;
    *je_kriz=true;
    }
else
    {krizovatka=0;
    *je_kriz=false;
    }

if (krizovatka==0)
   {
   if(con1>=con2)
   kam1=smer_h(primka1);
   if(con2>con1)
   kam1=smer_kolmy_h(primka2);
   if(con1<20 & con2<20)
   kam1=255;
   }
else{
//opozdeni zacatku odbocovani na krizovatce
//zakomentovat pokud bude delat problemy ------------
        if (c_row>(380) )
        {
        if(con1>20)
        kam1=smer_h(primka1);
        if(con1<20)
        kam1=smer_kolmy_h(primka2);
        if(con1<20 & con2<20)
        kam1=255;
        }
        else
// --------------------------------------------------
    switch (cesta)
    {
        case 0:
            kam1=smer_h(primka1);break;
        case 1:
            pom=smer_kolmy_h(primka2);
            kam1=sqrt(pom*pom);break;
        case 2:
            pom=smer_kolmy_h(primka2);
            kam1=-sqrt(pom*pom);break;
 //       case 3:
   //         kam1=254;break;
    }
    }
    return kam1;
}

//prusecik primek---------------------------------------------------------------
//vraci 0 pokud je prusecik mimo obraz nebo neexistuje ,1 pokud je spocten
//c_col sloupec (x), c_row radek (y)
int CrossPos(LINE primka1,LINE primka2,int *c_row,int *c_col)
{
 if(primka1[0]==(-1) | primka2[0]==(-1))
 {*c_col=-1;*c_row=-1;return 0;}
 float pom=sin(primka1[0]/180*3.14159)*cos(primka2[0]/180*3.14159)-sin(primka2[0]/180*3.14159)*cos(primka1[0]/180*3.14159);
 if(pom==0)
 pom=0.000000001;
 *c_col=(primka2[1]*sin(primka1[0]/180*3.14159)-primka1[1])/pom;
 float pom2=sin(primka2[0]/180*3.14159);
 if(pom2==0)
 pom2=0.00001;
 *c_row=(primka2[1]-*c_col*cos(primka2[0]/180*3.14159))/pom2;
  if(*c_row<0 | *c_row>rows | *c_col<0 | *c_col>cols)
 {*c_col=-1;*c_row=-1;return 0;}
 else
 return 1;
}

//uprava pro zvyrazneni baravneho kanalu---------------------------------------
void ColorProc(PICTURE picture,unsigned  char* pixel,int kanal,float comp)
{
int radek=0,sloupec=0;
//vsechny kanaly
        for (int i=0;i<3*rows*cols;i+=3,pixel+=3)
        {
        int r,g,b,bw,mixture;
        b=*pixel ;g=*(pixel+1);r=*(pixel+2) ;
        if(sloupec>cols-1)
                {sloupec=0;radek++;}
switch (kanal)
{
case 0: {(int)bw=(0.114*b+0.587*g+0.299*r);       //černobílý obraz
        picture[rows-1-radek][sloupec]=bw;} break;
case 1: {mixture=(b-(comp*r+comp*g));             //Modrý
                if(mixture<0)
                mixture=0;
        picture[rows-1-radek][sloupec]=mixture;} break;
case 2: {mixture=(g-(comp*r+comp*b));             //Zelený
                if(mixture<0)
                mixture=0;
        picture[rows-1-radek][sloupec]=mixture;} break;
case 3: {mixture=(r-(comp*b+comp*g));             //Cerveny
                if(mixture<0)
                mixture=0;
        picture[rows-1-radek][sloupec]=mixture;} break;
}
        sloupec++;
        }
}

//Regulace rychlosti pohybu-----------------------------------------------------
//vraci rychlost
int speed_final=0;
int Speed(int speed_max,int speed_min,int crosspos_index,int con1,int con2,int uhel)
{
if(speed_max>255)speed_max=255;
if(speed_min<0)speed_min=0;
int speed=0;

if(crosspos_index==1)
speed=speed_min;
else if(abs(uhel)>25)
        {
                speed=speed_max/((abs(uhel)-18)/8.0);
                if(speed<speed_min) speed=speed_min;
        }
        else speed=speed_max;
        //------------------------------------
        /*else
        {
                if(con1>con2)
                {if(con1<40) speed=speed_max*9.0/(48-con1);
                else speed=speed_max;}
                else
                {if(con2<40) speed=speed_max*9.0/(48-con2);
                else speed=speed_max;}

        } */
        //-------------------------------------
        speed_final=(speed_final*3+speed)/4.0;
        return speed_final;
}
