#include "trabalho3.h"
#include <math.h>
#define ROTULO_I 1 //menor rotulo, criado para testes de rotulação
#define WINSIZE 3
#define MIN(a,b) ((a<b)? a : b)

void detectaSensorBar (Imagem1C* img, Coordenada* l, Coordenada* r, double* angulo){
        int i,j, r_bola1=ROTULO_I, r_bola2;
        Imagem1C *matrizTemporaria =(Imagem1C *)malloc(sizeof(Imagem1C));
        matrizTemporaria = criaImagem1C(img->largura, img->altura);
         for(i=0;i<matrizTemporaria->altura;i++)
                for(j=0;j<matrizTemporaria->largura;j++)
            matrizTemporaria->dados[i][j]=0;

    filtroMedia(img, matrizTemporaria);
        binariza(matrizTemporaria, matrizTemporaria, 163);
        *img=*matrizTemporaria;
        r_bola2 = atribuiRotulos(matrizTemporaria, img);
        organiza_coordenada(l, r, r_bola2, img);
        calculaAngulo(r, l, angulo);
}

void calculaAngulo(Coordenada* r, Coordenada* l, double *angulo){
    double horizontal, vertical;
    horizontal =(double) r->x - l->x;
    vertical =(double) abs(r->y - l->y);
    *angulo = atan2(vertical,horizontal)* 180 / M_PI;
}


void calculaCentro(Coordenada* bola,Imagem1C* img, int rotulo){
    int i, j, verificador=0;
    int x1, x2, y1, y2;
    Coordenada centro;
    for(i=1; i<img->altura-1&&verificador==0; i++){
        for(j=1; j<img->largura-1; j++){
            if(img->dados[i][j]==rotulo){
                verificador=1;
                x1=j;
                y1=i;
                break;
                }
        }
    }
    verificador=0;
    for(i=img->altura-2; i>0&&verificador==0; i--){
        for(j=img->largura-2; j>0; j--){
            if(img->dados[i][j]==rotulo){
                verificador=1;
                x2=j;
                y2=i;
                break;
            }
        }
    }
    bola->x = (x1+x2)/2;
    bola->y = (y1+y2)/2;
    return &centro;

}

void organiza_coordenada(Coordenada* l, Coordenada* r, int rotulo2, Imagem1C* img){
    int i, j;
    Coordenada bola1, bola2;
    calculaCentro(&bola1, img, ROTULO_I);
    calculaCentro(&bola2, img, rotulo2);
    if(bola1.x > bola2.x){
        l->x=bola2.x;
        l->y=bola2.y;
        r->x=bola1.x;
        r->y=bola1.y;
    }
    else{
        l->x=bola1.x;
        l->y=bola1.y;
        r->x=bola2.x;
        r->y=bola2.y;
    }
}

//Função ra calcular o menor valor diferente de 0

int menor_valor(a, b, c, d){
    int v[4], menor, i, cont=0;
    if(a!=0){
        v[cont]=a;
        cont++;
    }
    if(b!=0){
        v[cont]=b;
        cont++;
    }
    if(c!=0){
        v[cont]=c;
        cont++;
    }
    if(d!=0){
        v[cont]=d;
        cont++;
    }
    menor=v[0];
    for(i=1; i<cont; i++){
        menor =MIN(menor, v[i]);
    }
    return menor;
}


void varreduraForward(Imagem1C* in, Imagem1C* out, int *verificador){
    int i, j, inicial;
    for(i=1; i<in -> altura-1; i++){
        for(j=1; j< in ->largura-1; j++){
            if(in->dados[i][j] > 0){
                if(in->dados[i-1][j-1]!=0||in->dados[i][j-1]!=0 || in->dados[i-1][j]!=0 ||in->dados[i-1][j+1]!=0){
                    inicial=out->dados[i][j];
                    out->dados[i][j]= menor_valor(in->dados[i-1][j-1], in->dados[i][j-1], in->dados[i-1][j], in->dados[i-1][j+1]);

                }
            }
            if(*verificador==0){
                if(in->dados[i][j]!=out->dados[i][j]){
                    *verificador=1;
                }
            }
        }
    }

}


void varreduraBackward(Imagem1C* in, Imagem1C* out, int *verificador){

    int i, j, inicial;
    for(i=in->altura-1; i>0; i--){
        for(j=in->largura-1; j>0; j--){
            if(in->dados[i][j]>0){
                if(in->dados[i+1][j+1]!=0||in->dados[i][j+1]!=0 || in->dados[i+1][j]!=0 ||in->dados[i+1][j-1]){
                    inicial = out->dados[i][j];
                    out->dados[i][j]= menor_valor(in->dados[i+1][j+1], in->dados[i][j+1], in->dados[i+1][j], in->dados[i+1][j-1]);
                }
            }
            if(*verificador=0)
                if(in->dados[i][j]!=out->dados[i][j]){
                    *verificador = 1;

            }
        }
    }

}


int atribuiRotulos(Imagem1C* in, Imagem1C* out){
    int rotulo = ROTULO_I, verificador, i, j;
    for(i=1; i<in->altura-1; i++){
        for(j=1; j< in->largura-1 ; j++){
            if(in->dados[i][j]>0){
                if(in->dados[i-1][j-1]==0 && in->dados[i][j-1]==0 && in->dados[i-1][j]==0&&in->dados[i-1][j+1]==0){
                    out->dados[i][j]=rotulo;
                    rotulo++;
                }
                else if(in->dados[i-1][j-1]!=0||in->dados[i][j-1]!=0 || in->dados[i-1][j]!=0 ||in->dados[i-1][j+1]!=0){
                    out->dados[i][j]= menor_valor(in->dados[i-1][j-1], in->dados[i][j-1], in->dados[i-1][j], in->dados[i-1][j+1]);
                }
            }
        }

    }


    do{
        *in = *out;
        verificador=0;
        varreduraBackward(in, out, &verificador);
        varreduraForward(in, out, &verificador);
    }while(verificador==1);
    for(i=0; i<out->altura; i++){
        for(j=0; j<out->largura; j++){
            if(rotulo>out->dados[i][j]&&out->dados[i][j]>ROTULO_I)
                rotulo=out->dados[i][j];
        }
    }

    return rotulo;
}

void filtroMedia(Imagem1C* img, Imagem1C* out){
        int i, j, k, media, tamanho = WINSIZE / 2;

        for(i=0;i<tamanho;i++)
                for(j=0;j<(img->largura-tamanho);j++)
                        out->dados[i][j] = 0;
        for(i=((img->altura)-tamanho);i<img->altura;i++)
                for(j=0;j<(img->largura-tamanho);j++)
                        out->dados[i][j] = 0;

        for(i=tamanho; i<img->altura-tamanho; i++)
                for(j=0;j<tamanho;j++)
                        out->dados[i][j] = 0;

        for(i=tamanho; i<img->altura-tamanho; i++)
                for(j=img->largura-tamanho;j<img->largura;j++)
                        out->dados[i][j] = 0;

        for (i = tamanho; i < (img->altura-tamanho); i++){
                for (j = tamanho; j < (img->largura-tamanho); j++){
                        media = img->dados[i][j];
                        for(k = 1; k <= tamanho; k++){
                                media += img->dados[i+k][j];
                                media += img->dados[i][j+k];
                                media += img->dados[i+k][j+k];
                                media += img->dados[i-k][j];
                                media += img->dados[i][j-k];
                                media += img->dados[i-k][j-k];
                                media += img->dados[i+k][j-k];
                                media += img->dados[i-k][j+k];
                        }
                        media /= (WINSIZE*WINSIZE);
                        out->dados[i][j] = media;
                }
        }
}

void binariza(Imagem1C* img, Imagem1C* out, int limiar)
{
        int i, j;

        for (i = 0; i < img->altura; i++)
                for (j = 0; j < img->largura; j++)
                        if(img->dados[i][j]>limiar)
                                out->dados[i][j] = 255;
                        else
                                out->dados[i][j] = 0;
}