package algoritmo;

import auxiliar.Conversor;
import java.util.Vector;

public class Agente {

    /**
     * @param args
     */
    private double[] posicao;//em coord cartesiana
    private double[] posicaoDestino;//em coord cartesiana
    private double fitness;
    private int idAgente;

    //Sensores
    private SensorDeVizinhanca sensorDeVizinhanca;
    private SensorFitness sensorFitness;
    private SensorAntiColisao sensorAntiColisao;

    //Dimensoes
    private double comprimentoHorizontal;
    private double comprimentoVertical;

    //Atributos de estado
    private boolean estadoDePausa;
    private boolean alvoEncontrado;
    private boolean escolheuPosicaoAleatoria;
    private boolean estadoPSO; //true se o agente tiver executando pso
    private boolean wallCollision;
    private int contadorParaAtivarPuloEscape=9000;

    //Atributos de robô móvel
    private double[] velocidadePolar;
    private double[] velocidadeCartesiana;
    private double moduloVelocidadeMax=5;
    private double moduloVelocidadeMin=EspacoDeBusca.getInstance().getVeloCorrentezaPolar()[0];
    private double raioDeCurvaturaMax;//Indica o quanto o agente pode virar � esquerda ou � direita
    private boolean mostrarTrajetoria;

    private ConjuntoDeSolucoes memoriaDeSolucoes;
    private ConjuntoDeSolucoes memoriaDeSolucoesRecebidas;

    public Agente(int id) {
        posicao = new double[2];
        posicaoDestino = posicao.clone();
        alvoEncontrado = false;
        idAgente = id;
        int tamanhoDoConjuntoUltimasSolucoes=5;//num. max. de soluções que serão utilizadas por vez
        memoriaDeSolucoes = new ConjuntoDeSolucoes(tamanhoDoConjuntoUltimasSolucoes);//numero maximo de soluções que serão tranmitidas ao vizinho e que serão testadas  durante a movimentação
        //inicializa pbest e lbest, deixando o agente pronto para a execução do pso
        //memoriaDeSolucoes.getSolucaoPbest().setOrigemVetorIndicacaoPosicao(posicao);
        //memoriaDeSolucoes.getSolucaoPbest().setVetorIndicacaoPosicao(new double[2]);
        //memoriaDeSolucoes.getSolucaoLbest().setOrigemVetorIndicacaoPosicao(posicao);
        //memoriaDeSolucoes.getSolucaoLbest().setVetorIndicacaoPosicao(new double[2]);

        int tamanhoDoConjuntoUltimasSolucoesRecebidas=20;//atenção, esse não é o núm. máximo de soluções que foram ou que podem ser recebidas, mas sim o num. max. de soluções que serão utilizadas
        memoriaDeSolucoesRecebidas = new ConjuntoDeSolucoes(tamanhoDoConjuntoUltimasSolucoesRecebidas);//numero das n ultimas soluções recebidas que serão testadas durante a movimentação
        sensorDeVizinhanca = new SensorDeVizinhanca();
        sensorFitness = new SensorFotografico(memoriaDeSolucoes,idAgente,null);
        sensorAntiColisao = new SensorAntiColisao();
        estadoDePausa = false;
        estadoPSO = false;
        comprimentoHorizontal = 0;
        comprimentoVertical = 0;
        velocidadePolar = new double[2];//valor default:{5,0} definido em PainelFerramentaAgentes(velocidadePolarPadraoAgente)
        raioDeCurvaturaMax = 2;
        mostrarTrajetoria = true;
        wallCollision = false;
        escolheuPosicaoAleatoria =false;

        moduloVelocidadeMin=EspacoDeBusca.getInstance().getVeloCorrentezaPolar()[0];
        moduloVelocidadeMax=5;
    }

    public double atualizarAngulo(int t, int anguloAtual, int anguloFinal, double velocidadeLinear) {

        double novoAngulo = 0;
        if (anguloAtual == 360) {
            anguloAtual = 0;
        }
        if (anguloFinal == 360) {
            anguloFinal = 0;
        }

        int variacaoAngular = anguloFinal - anguloAtual;

        //w = v/r nos dará a variação angular por ciclo
        double velocAngular = velocidadeLinear / raioDeCurvaturaMax;

        if (variacaoAngular == 0) {//continua na mesma direção
            novoAngulo = velocidadePolar[1];
        } /*else if (anguloAtual < 90 && anguloFinal >= 270) {//gira para a direita
        novoAngulo = giraParaDireita(velocAngular, variacaoAngular, anguloFinal, t);
        } else if (anguloAtual >= 270 && anguloFinal < 90) {//gira para a esquerda
        novoAngulo = giraParaEsquerda(velocAngular, variacaoAngular, anguloFinal, t);
        } else if (variacaoAngular < 0) { //gira para a direita
        novoAngulo = giraParaDireita(velocAngular, variacaoAngular, anguloFinal, t);
        } else if (variacaoAngular > 0) {//gira para a esquerda
        novoAngulo = giraParaEsquerda(velocAngular, variacaoAngular, anguloFinal, t);
        }*/ else if (variacaoAngular > 0 && Math.abs(variacaoAngular) <= 180) {//gira para a esquerda
            novoAngulo = giraParaEsquerda(velocAngular, variacaoAngular, anguloFinal, t);
        } else if (variacaoAngular > 0 && Math.abs(variacaoAngular) > 180) {//gira para a direita
            novoAngulo = giraParaDireita(velocAngular, variacaoAngular, anguloFinal, t);
        } else if (variacaoAngular < 0 && Math.abs(variacaoAngular) <= 180) { //gira para a direita
            novoAngulo = giraParaDireita(velocAngular, variacaoAngular, anguloFinal, t);
        } else if (variacaoAngular < 0 && Math.abs(variacaoAngular) > 180) {//gira para a esquerda
            novoAngulo = giraParaEsquerda(velocAngular, variacaoAngular, anguloFinal, t);
        } else {
            System.out.println("ERRO: atualizarAngulo");
        }

        return novoAngulo;
    }

    private double giraParaDireita(double velocAngular, int variacaoAngular, int anguloFinal, int t) {
        double novoAngulo;

        novoAngulo = velocidadePolar[1] - velocAngular * t;
        novoAngulo = Conversor.ajustarAnguloParaEscala_0_360(novoAngulo);
        if ((360 - Math.abs(variacaoAngular)) < velocAngular)//para não ultrapassar o angulo final
        {
            novoAngulo = anguloFinal;
        }

        return novoAngulo;
    }

    private double giraParaEsquerda(double velocAngular, int variacaoAngular, int anguloFinal, int t) {
        double novoAngulo;

        novoAngulo = velocidadePolar[1] + velocAngular * t;
        novoAngulo = Conversor.ajustarAnguloParaEscala_0_360(novoAngulo);
        if (Math.abs(variacaoAngular) < velocAngular)//para não ultrapassar o angulo final
        {
            novoAngulo = anguloFinal;
        }

        return novoAngulo;
    }

    public void mover(double[] correntezaCoordPolar, double[] posicaoAlvo,int limiteMaxCoordX, int limiteMaxCoordY) {
        double Vc = correntezaCoordPolar[0];
        double angC = correntezaCoordPolar[1];
        double Vb = this.velocidadePolar[0];
        double angB = this.velocidadePolar[1];
        double[] posicaoAgente = new double[2];

        double deslocamentoX = Vb * Math.cos(Math.toRadians(angB)) + Vc * Math.cos(Math.toRadians(angC));
        double deslocamentoY = Vb * Math.sin(Math.toRadians(angB)) + Vc * Math.sin(Math.toRadians(angC));

        posicaoAgente[0] = posicao[0] + deslocamentoX;//Px
        posicaoAgente[1] = posicao[1] + deslocamentoY;//Py

        if (posicaoAlvo != null) {
            //Para evitar que passe direto
            if ((Math.abs(posicaoAlvo[0] - posicao[0]) < Math.abs(deslocamentoX)) && (Math.abs(posicaoAlvo[1] - posicao[1]) < Math.abs(deslocamentoY))) {
                posicaoAgente[0] = posicaoAlvo[0];
                posicaoAgente[1] = posicaoAlvo[1];
                System.out.println("Agente "+idAgente+" - Essa parte evita que passe direto pelo alvo");
            }
            /*else if(posicaoAgente[0]-(comprimentoHorizontal/2)<posicaoAlvo[0]
                    &&posicaoAgente[0]+(comprimentoHorizontal/2)>posicaoAlvo[0]
                    &&posicaoAgente[1]-(comprimentoVertical/2)<posicaoAlvo[0]
                    &&posicaoAgente[1]+(comprimentoVertical/2)>posicaoAlvo[0]){
                posicaoAgente[0] = posicaoAlvo[0];
                posicaoAgente[1] = posicaoAlvo[1];
                System.out.println("Agente "+idAgente+" - Essa parte evita que passe direto pelo alvo´- testa volume");
            }*/
        }
        /*
        //Verifica se encontra-se dentro do espaço de busca
        if (posicaoAgente[0] >= 0 && posicaoAgente[0] <= limiteMaxCoordX &&
            posicaoAgente[1] >= 0 && posicaoAgente[1] <= limiteMaxCoordY) {
                setPosicao(posicaoAgente);
                wallCollision =false;
        }
        else {
                wallCollision = true;
                setPosicao(posicaoAgente);
                System.out.println("Agente "+idAgente+" (método mover) diz: Fora do limite do espaco de busca!  MaxX: " + limiteMaxCoordX + " MaxY: " + limiteMaxCoordY+ " Posição a ir: "+posicaoAgente[0]+","+posicaoAgente[1]);
        }
        */
       setPosicao(posicaoAgente);

    }

    //Com este movimento, o barco tenta ficar na direção oposta à da correnteza com igual velocidade
    public void estabilizarPosicao(double[] correntezaCoordPolar,int limiteMaxCoordX, int limiteMaxCoordY) {

        int anguloAtual = (int) Math.round(velocidadePolar[1]);

        double ang = correntezaCoordPolar[1] - 180;//Angulo contrário à correnteza
        ang = Conversor.ajustarAnguloParaEscala_0_360(ang);
        int anguloFinal = (int) (Math.round(ang));


        if (! estadoDePausa && (correntezaCoordPolar[0] != 0)) {
            double[] novaVelocPolar = new double[2];
            novaVelocPolar[0] = correntezaCoordPolar[0];
            int t = 1;//significa que cada chamada desse método equivale a um ciclo real, onde o período "t" ode estar em segundo, horas, etc.

            novaVelocPolar[1] = atualizarAngulo(t, anguloAtual, anguloFinal, novaVelocPolar[0]);
            setVelocidadePolar(novaVelocPolar);
            mover(correntezaCoordPolar, null, limiteMaxCoordX, limiteMaxCoordY);

            if (correntezaCoordPolar[0] == velocidadePolar[0] && Math.abs(correntezaCoordPolar[1] - velocidadePolar[1]) == 180) {
                 estadoDePausa = true;
            }
        }
    }

    public boolean alcancarDestino(double[] correntezaCoordPolar,int limiteMaxCoordX, int limiteMaxCoordY) {
        
        //Se não encontrou o alvo
        if (!alvoEncontrado ) {
            
            int anguloAgente = (int) Math.round(velocidadePolar[1]);
            double[] coordDestino = new double[2];//Verlocidade Resultante cartesiana
            coordDestino[0] = posicaoDestino[0] - posicao[0];//Pabx = Pbx - Pax
            coordDestino[1] = posicaoDestino[1] - posicao[1];//Paby = Pby - Pay

            //Se estivermos em cima do ponto de destino
            if (coordDestino[0] == 0 && coordDestino[1] == 0) {
                alvoEncontrado = true;
            } else {
                //double[] destinoCoordPolar = Conversor.coordCartesianaParaPolar(coordDestino[0],coordDestino[1]);
                //int anguloResultante = (int) Math.round( destinoCoordPolar[1]);
                double[] correntezaCoordCart = Conversor.coordPolarParaCartesiana(correntezaCoordPolar[0], correntezaCoordPolar[1]);
                double[] velocBarcoCoordCart = new double[2];
                velocBarcoCoordCart[0] = coordDestino[0] - correntezaCoordCart[0];
                velocBarcoCoordCart[1] = coordDestino[1] - correntezaCoordCart[1];
                double[] velocBarcoCoordPolar = Conversor.coordCartesianaParaPolar(velocBarcoCoordCart[0], velocBarcoCoordCart[1]);
                int anguloFinalBarco = (int) Math.round(velocBarcoCoordPolar[1]);
                int t = 1;//cada ciclo(chamada a esse método equivale a 1 período de tempo, por exemplo, 0.1 segundo
                double[] novaVelocPolar = new double[2];
                //Módulo da nova velocidade (velocidade constante = Vmax)
                novaVelocPolar[0] =moduloVelocidadeMax; /////////////falta estrategia para atualizar modulo

                boolean detectou = sensorAntiColisao.sensorResponse(posicao);
                if(detectou){
                    double[] vetorRepulsaoResultante = sensorAntiColisao.getVetorRepulsaoResultante();
                    // se o módulo do vetorRepulsaoResultante não for nulo
                    if(vetorRepulsaoResultante[0] !=0 || vetorRepulsaoResultante[1] !=0){
                        anguloFinalBarco = sensorAntiColisao.getAnguloVetorRepulsaoResultante();
                   /*
                        //---Estrategia para atualizar modulo no caso de obstaculo---//
                        //Calcula a diferença dos angulos
                        int diferencaAngular = (int) Math.abs(anguloFinalBarco - velocidadePolar[1]);
                        if(diferencaAngular>180)
                            diferencaAngular = 360-diferencaAngular;
                        //http://www.e-escola.pt/topico.asp?id=156
                        int cargaObstaculo = 105;
                        int cargaAgente = 25;
                        double distAoQuadrado = Math.pow(sensorAntiColisao.getVetorRepulsaoResultante()[0],2) + Math.pow(sensorAntiColisao.getVetorRepulsaoResultante()[1],2);
                        double aceleracao = (cargaAgente*cargaObstaculo)/ distAoQuadrado;
                        if(diferencaAngular>90)
                            novaVelocPolar[0] = velocidadePolar[0]-aceleracao*t;//v = vo - at 9 -> diminui a velocidade ao se aproximar do centro do obstaculo
                        else if(diferencaAngular<90)
                            novaVelocPolar[0] = velocidadePolar[0]+aceleracao*t;//v = vo + at -> aumenta a velocidade ao se distanciar do centro do obstaculo
                        else if(diferencaAngular==90)
                            aceleracao=0;//a=0

                        //Evita que a velocidade exceda o limite mínimo ou o máximo
                        if(novaVelocPolar[0]<moduloVelocidadeMin){
                            novaVelocPolar[0]=moduloVelocidadeMin;
                        }
                        if(novaVelocPolar[0]>moduloVelocidadeMax){
                            novaVelocPolar[0]=moduloVelocidadeMax;
                        }

                        double[] novaVelocCartesiana= Conversor.coordPolarParaCartesiana(novaVelocPolar[0], novaVelocPolar[1]);
                        //Evita que a o barquinho seja arrastado pela correnteza e bata no obstáculo
                        //Para evitar ser levado pela correnteza, |Vb|>|Vc| quando eles tiverem sinais opostos (direções opostas)
                        double temp1 = novaVelocCartesiana[0]*correntezaCoordCart[0];//se<0, sinais diferentes, se não sinais iguais
                        double temp2 = novaVelocCartesiana[1]*correntezaCoordCart[1];
                        if( (temp1<0 && Math.abs(novaVelocCartesiana[0])<Math.abs(correntezaCoordCart[0]))
                            || (temp2<0 && Math.abs(novaVelocCartesiana[1])<Math.abs(correntezaCoordCart[1])) ){
                            novaVelocPolar[0] = velocidadePolar[0];
                        }
                    */
                    }
                }
                System.out.println("Agente "+idAgente+" diz: anguloFinalBarco = "+anguloFinalBarco);
                /*if(wallCollision){
                    velocBarcoCoordPolar = Conversor.coordCartesianaParaPolar(coordDestino[0], coordDestino[1]);
                    anguloFinalBarco = (int) Math.round(velocBarcoCoordPolar[1]);
                     System.out.println("Agente "+idAgente+" diz: Colisao! anguloFinalBarco = "+anguloFinalBarco);
                }*/

                novaVelocPolar[1] = atualizarAngulo(t, anguloAgente, anguloFinalBarco, novaVelocPolar[0]);
                //novaVelocPolar[0] = velocidadePolar[0];/////////////
                        
                setVelocidadePolar(novaVelocPolar);
                //Mover com nova direcao(veloc. polar)
                mover(correntezaCoordPolar, posicaoDestino, limiteMaxCoordX, limiteMaxCoordY);

                if ((int) Math.round(posicao[0]) == posicaoDestino[0] && (int) Math.round(posicao[1]) == posicaoDestino[1]) {
                    alvoEncontrado = true;
                }else if((posicao[0]-(comprimentoHorizontal/2)<posicaoDestino[0]
                    &&posicao[0]+(comprimentoHorizontal/2)>posicaoDestino[0]
                    &&posicao[1]-(comprimentoVertical/2)<posicaoDestino[1]
                    &&posicao[1]+(comprimentoVertical/2)>posicaoDestino[1])){
                    
                    alvoEncontrado = true;
                }
            }
        }
        return alvoEncontrado;
    }

    public void explorarAmbiente(double[] correntezaCoordPolar,int limiteMaxCoordX, int limiteMaxCoordY) {
        //Escolhe-se um angulo aleatoriamente entre anguloAgente -90 e anguloAgente+90
        int anguloAgente = (int) Math.round(velocidadePolar[1]);
        int angulo;
        double raioDeslocamento;
        if( estadoDePausa){//como ele escolhe o ângulo de destino na exploração:
            contadorParaAtivarPuloEscape++;
            angulo = (anguloAgente -180)  + (int) Math.round(Math.random() * 360);
            if(contadorParaAtivarPuloEscape>500){//se o agente estiver em loop
                raioDeslocamento = 1000*Math.random();
            }
            else
                raioDeslocamento = Math.abs(angulo)*Math.random();
            //angulo = (anguloAgente -135)  + (int) Math.round(Math.random() * 270);
             estadoDePausa = false;
        }
        else{
            contadorParaAtivarPuloEscape=0;
            angulo = (anguloAgente -30)  + (int) Math.round(Math.random() * 60);
            raioDeslocamento = sensorFitness.getSensorDiametro();//Rd = 2*RaioSensor
        }
        angulo = (int) Conversor.ajustarAnguloParaEscala_0_360(angulo);
        

        double[] deslocamento = Conversor.coordPolarParaCartesiana(raioDeslocamento, angulo);
        posicaoDestino[0] = posicao[0] + deslocamento[0];
        posicaoDestino[1] = posicao[1] + deslocamento[1];
        //Estabelece comunicação com outros agentes
        comunicacao();
        //Se a podicaoDestino pertence a uma área que já foi avaliada, espera 1 ciclo caso sim
        if(seJaAvaliouPosicao() ||sePosicaoDestinoJaFoiAvaliadaPorVizinho()){
            System.out.println("Agente"+idAgente+" ("+raioDeslocamento+","+angulo+") Posicao Destino Ja Avaliada!");
            posicaoDestino[0] = posicao[0];
            posicaoDestino[1] = posicao[1];
            estadoDePausa = true;
        }
        //Verifica se a podicaoDestino não encontra-se dentro do espaço de busca, espera 1 ciclo caso sim
        if(!sePosicaoDestinoPertenceAoEspacoBusca(limiteMaxCoordX, limiteMaxCoordY)){
            System.out.println("Agente"+idAgente+" ("+raioDeslocamento+","+angulo+") Posicao Destino Fora do Espaco!");
            posicaoDestino[0] = posicao[0];
            posicaoDestino[1] = posicao[1];
            estadoDePausa = true;
        }
        //Se a podicaoDestino pertence a área de repulsao de um obstáculo
        if(sePosicaoDestinoPertenceAreaDeRepulsaoDeObstaculo()){
            System.out.println("Agente"+idAgente+" ("+raioDeslocamento+","+angulo+") Posicao Destino Pertence a Campo de Repulsao!");
            posicaoDestino[0] = posicao[0];
            posicaoDestino[1] = posicao[1];
            estadoDePausa = true;
        }
        alvoEncontrado = false;


    }


    public void exploracaoInteligente(int limiteMaxCoordX, int limiteMaxCoordY, int w){
        if( estadoDePausa){//nova tetntativa de escolher ponto de destino válido
             estadoDePausa = false;
        }
        double[] deslocamento = new double[2];
        velocidadeCartesiana = Conversor.coordPolarParaCartesiana(getVelocidadePolar()[0], getVelocidadePolar()[1]);
        //double[] pBestPosicao = memoriaDeSolucoes.getSolucaoPbest().getMelhorPosicaoQuadrante1();
        //double[] lBestPosicao = memoriaDeSolucoes.getSolucaoLbest().getMelhorPosicaoQuadrante1();
        /*System.out.println("Agente"+idAgente
                            +" pBest: "+pBestPosicao[0]+","+pBestPosicao[1]
                            +" | lBest: "+lBestPosicao[0]+","+lBestPosicao[1]);
         */
        /*
        System.out.println("Agente"+idAgente
                            +" termo indv: ("+2*Math.random()*(pBestPosicao[0] - posicao[0])+","+2*Math.random()*(pBestPosicao[1] - posicao[1])
                            +") termo social:" +2*Math.random()*(lBestPosicao[0] - posicao[0])+","+2*Math.random()*(lBestPosicao[1] - posicao[1]));
        */
        //deslocamento[0] = velocidadeCartesiana[0]*w +2*Math.random()*(pBestPosicao[0] - posicao[0])+2*Math.random()*(lBestPosicao[0] - posicao[0]);
        //deslocamento[1] = velocidadeCartesiana[1]*w +2*Math.random()*(pBestPosicao[1] - posicao[1])+2*Math.random()*(lBestPosicao[1] - posicao[1]);

        //posicaoDestino[0] = posicao[0] + deslocamento[0];
        //posicaoDestino[1] = posicao[1] + deslocamento[1];



        //-------------------------
        /*
        posicaoDestino[0] = memoriaDeSolucoes.getSolucaoLbest().getMelhorPosicaoQuadrante1()[0];
        posicaoDestino[1] = memoriaDeSolucoes.getSolucaoLbest().getMelhorPosicaoQuadrante1()[1];

        //se o agente não é o mais indicado para ir até a posição, por conta da distância ou por outro agente já ter
        if(!seAgenteMaisAdequadoParaPosicaoDestino()){
            System.out.println("Agente "+idAgente+" não é o mais indicado para ir à  posição: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]);
            posicaoDestino = memoriaDeSolucoes.getSolucaoPbest().gerarPosicaoAproximada();
            //posicaoDestino = memoriaDeSolucoes.getSolucaoLbest().gerarPosicaoAproximada();
        }*/
        //----------------------



        Solucao solucaoLbest = memoriaDeSolucoes.getSolucaoLbest();
        
        boolean destinoValidoQuadrante1 = (!solucaoLbest.isQuadrante1Fotografado()) && (solucaoLbest.getVetorIndicacaoPosicaoQuadrante1()[0]!=0 || solucaoLbest.getVetorIndicacaoPosicaoQuadrante1()[1]!=0);
        boolean destinoValidoQuadrante2 = (!solucaoLbest.isQuadrante2Fotografado()) && (solucaoLbest.getVetorIndicacaoPosicaoQuadrante2()[0]!=0 || solucaoLbest.getVetorIndicacaoPosicaoQuadrante2()[1]!=0);
        boolean destinoValidoQuadrante3 = (!solucaoLbest.isQuadrante3Fotografado()) && (solucaoLbest.getVetorIndicacaoPosicaoQuadrante3()[0]!=0 || solucaoLbest.getVetorIndicacaoPosicaoQuadrante3()[1]!=0);
        boolean destinoValidoQuadrante4 = (!solucaoLbest.isQuadrante4Fotografado()) && (solucaoLbest.getVetorIndicacaoPosicaoQuadrante4()[0]!=0 || solucaoLbest.getVetorIndicacaoPosicaoQuadrante4()[1]!=0);

        escolheuPosicaoAleatoria = false;
        posicaoDestino = solucaoLbest.getMelhorPosicaoQuadrante1();
        boolean posicaoJaAvaliada = seJaAvaliouPosicao();
        boolean posicaoJaAvaliadaPorVizinho = sePosicaoDestinoJaFoiAvaliadaPorVizinho();
        if( !(destinoValidoQuadrante1 && seAgenteMaisAdequadoParaPosicaoDestino()) || posicaoJaAvaliada || posicaoJaAvaliadaPorVizinho){//testa se posicao de destino nao eh  valida ou se é posicao destino de outro agente e se a posivao ja foi avaliada
           solucaoLbest.setQuadrante1Fotografado(true);System.out.println("Agente "+idAgente+" diz: eu ("+posicaoJaAvaliada+") ou outro agente("+posicaoJaAvaliadaPorVizinho+") foi para quadrante1. este quadrante é válido? ("+destinoValidoQuadrante1+"), nao tem indicacao");
           posicaoDestino = solucaoLbest.getMelhorPosicaoQuadrante2();
           posicaoJaAvaliada = seJaAvaliouPosicao();
           posicaoJaAvaliadaPorVizinho = sePosicaoDestinoJaFoiAvaliadaPorVizinho();

           if( !(destinoValidoQuadrante2 && seAgenteMaisAdequadoParaPosicaoDestino())|| posicaoJaAvaliada || posicaoJaAvaliadaPorVizinho ){
               solucaoLbest.setQuadrante2Fotografado(true);System.out.println("Agente "+idAgente+" diz: eu ("+posicaoJaAvaliada+") ou outro agente("+posicaoJaAvaliadaPorVizinho+") foi para quadrante2. este quadrante é válido? ("+destinoValidoQuadrante2+"), nao tem indicacao");
               posicaoDestino = solucaoLbest.getMelhorPosicaoQuadrante3();
               posicaoJaAvaliada = seJaAvaliouPosicao() ;
               posicaoJaAvaliadaPorVizinho = sePosicaoDestinoJaFoiAvaliadaPorVizinho();

               if( !(destinoValidoQuadrante3 && seAgenteMaisAdequadoParaPosicaoDestino())|| posicaoJaAvaliada || posicaoJaAvaliadaPorVizinho){
                   solucaoLbest.setQuadrante3Fotografado(true);System.out.println("Agente "+idAgente+" diz: eu ("+posicaoJaAvaliada+") ou outro agente("+posicaoJaAvaliadaPorVizinho+") foi para quadrante3. este quadrante é válido? ("+destinoValidoQuadrante3+"), nao tem indicacao");
                   posicaoDestino = solucaoLbest.getMelhorPosicaoQuadrante4();
                   posicaoJaAvaliada = seJaAvaliouPosicao() ;
                   posicaoJaAvaliadaPorVizinho = sePosicaoDestinoJaFoiAvaliadaPorVizinho();

                   if( !(destinoValidoQuadrante4 && seAgenteMaisAdequadoParaPosicaoDestino())|| posicaoJaAvaliada || posicaoJaAvaliadaPorVizinho){
                        solucaoLbest.setQuadrante4Fotografado(true);System.out.println("Agente "+idAgente+" diz: eu ("+posicaoJaAvaliada+") ou outro agente("+posicaoJaAvaliadaPorVizinho+") foi para quadrante4. este quadrante é válido ("+destinoValidoQuadrante4+"), nao tem indicacao");
                        //posicaoDestino = solucaoLbest.gerarPosicaoAoRedor();//se não pode ser nenhuma das posicoes, escolhe destino aleatoriamente...

                        solucaoLbest.setUtilidade( solucaoLbest.getUtilidade() - 100);
                        solucaoLbest.calculaFitness();
                        double[] posicaoLbestAntigo = solucaoLbest.getPosicao();
                        posicaoDestino[0] = posicao[0];
                        posicaoDestino[1] = posicao[1];
                        estadoDePausa = true;
                        memoriaDeSolucoes.elegeOutroLbestDaMemoria();
                        
                        if(memoriaDeSolucoes.getSolucaoLbest().equals(posicaoLbestAntigo)){//se não atualizou o lbest, POIS TODOS FORAM DESCARTADOS
                            memoriaDeSolucoes.elegeMelhorSolucaoDescartadaComoLbest();
                            posicaoDestino = solucaoLbest.gerarPosicaoAproximada();
                            //escolheuPosicaoAleatoria = true;
                            estadoDePausa = false;
                            System.out.println("Agente "+idAgente+" escolheu quadrante aleatorio");
                        }else{
                            System.out.println("Agente "+idAgente+" ficou parado, diminuiu o fitness do lbest e elegeu outro lbest");
                        }
                        //solucaoLbest = memoriaDeSolucoes.getSolucaoLbest();//novo lbest
                        //posicaoDestino = solucaoLbest.gerarPosicaoAproximada();
                        /*posicaoJaAvaliada = seJaAvaliouPosicao();
                        posicaoJaAvaliadaPorVizinho = sePosicaoDestinoJaFoiAvaliadaPorVizinho();
                        if(posicaoJaAvaliada || posicaoJaAvaliadaPorVizinho){
                            posicaoDestino[0] = posicao[0];
                            posicaoDestino[1] = posicao[1];
                             estadoDePausa = true;
                            System.out.println("Agente "+idAgente+" escolheu quadrante aleatorio invalido - espera 1 ciclo");
                        }else{
                            solucaoLbest.setUtilidade( solucaoLbest.getUtilidade() - 100);//diminui a utilidade já que não fornece mais nenhum vetor de indicação
                            solucaoLbest.calculaFitness();//atualiza fitness
                            System.out.println("Agente "+idAgente+" foi para quadrante aleatorio e diminuiu o fitness do lbest");
                        }*/
                   }else{solucaoLbest.setQuadrante4Fotografado(true);   System.out.println("Agente "+idAgente+" foi para quadrante4");}//foi para o quadrante 4
               }else{solucaoLbest.setQuadrante3Fotografado(true);   System.out.println("Agente "+idAgente+" foi para quadrante3");}//foi para o quadrante 3
           }else{solucaoLbest.setQuadrante2Fotografado(true);   System.out.println("Agente "+idAgente+" foi para quadrante2");}//foi para o quadrante 2
        }else{solucaoLbest.setQuadrante1Fotografado(true);  System.out.println("Agente "+idAgente+" foi para quadrante1");}//foi para o quadrante 1
        

        //-----------------




        System.out.println("Agente "+idAgente+" posicaoDestino: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]);
        if(posicaoDestino[0]==posicao[0]&& posicaoDestino[1]==posicao[1]){
            System.out.println("Agente "+idAgente+" não deve permanecer na  posição: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]+" pois ela ja foi avaliada PARADA");
            //posicaoDestino = memoriaDeSolucoes.getSolucaoLbest().gerarPosicaoAproximada();
            //System.out.println("Agente "+idAgente+" posicaoDestino: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]);
          
        }
        /*
        //Se a podicaoDestino pertence a uma área que já foi avaliada, espera 1 ciclo caso sim
        if(sePosicaoDestinoJaFoiAvaliada()){
            System.out.println("Agente "+idAgente+" não deve ir para posição: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]+" pois ela ja foi avaliada ");
            posicaoDestino = solucaoLbest.gerarPosicaoAoRedor();
            estadoDePausa = true;
            System.out.println("Agente "+idAgente+" posicaoDestino: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]);
        }*/

        //Verifica se a podicaoDestino não encontra-se dentro do espaço de busca, espera 1 ciclo caso sim
        if(!sePosicaoDestinoPertenceAoEspacoBusca(limiteMaxCoordX, limiteMaxCoordY)){
            System.out.println("Agente "+idAgente+" ("+posicaoDestino[0]+","+posicaoDestino[1]+") Posicao Destino Fora do Espaco!");
            posicaoDestino[0] = posicao[0];
            posicaoDestino[1] = posicao[1];
            estadoDePausa = true;//marca para evitar que ele fique tirando foto do mesmo lugar
        }
        //Se a podicaoDestino pertence a área de repulsao de um obstáculo
        if(sePosicaoDestinoPertenceAreaDeRepulsaoDeObstaculo()){
            System.out.println("Agente "+idAgente+" ("+posicaoDestino[0]+","+posicaoDestino[1]+") Posicao Destino Pertence a Campo de Repulsao!");
            posicaoDestino[0] = posicao[0];
            posicaoDestino[1] = posicao[1];
            estadoDePausa = true;
        }
        alvoEncontrado = false;
    }

    public void exploracaoInteligentePSO(int limiteMaxCoordX, int limiteMaxCoordY, int w){
        if( estadoDePausa){//nova tetntativa de escolher ponto de destino válido
             estadoDePausa = false;
        }
        double[] deslocamento = new double[2];
        velocidadeCartesiana = Conversor.coordPolarParaCartesiana(getVelocidadePolar()[0], getVelocidadePolar()[1]);
        double[] pBestPosicao = memoriaDeSolucoes.getSolucaoPbest().getMelhorPosicaoQuadrante1();
        double[] lBestPosicao = memoriaDeSolucoes.getSolucaoLbest().getMelhorPosicaoQuadrante1();
        /*System.out.println("Agente"+idAgente
                            +" pBest: "+pBestPosicao[0]+","+pBestPosicao[1]
                            +" | lBest: "+lBestPosicao[0]+","+lBestPosicao[1]);
         */
        /*
        System.out.println("Agente"+idAgente
                            +" termo indv: ("+2*Math.random()*(pBestPosicao[0] - posicao[0])+","+2*Math.random()*(pBestPosicao[1] - posicao[1])
                            +") termo social:" +2*Math.random()*(lBestPosicao[0] - posicao[0])+","+2*Math.random()*(lBestPosicao[1] - posicao[1]));
        */
        deslocamento[0] = velocidadeCartesiana[0]*w +2*Math.random()*(pBestPosicao[0] - posicao[0])+2*Math.random()*(lBestPosicao[0] - posicao[0]);
        deslocamento[1] = velocidadeCartesiana[1]*w +2*Math.random()*(pBestPosicao[1] - posicao[1])+2*Math.random()*(lBestPosicao[1] - posicao[1]);

        posicaoDestino[0] = posicao[0] + deslocamento[0];
        posicaoDestino[1] = posicao[1] + deslocamento[1];

        //posicaoDestino[0] = memoriaDeSolucoes.getSolucaoLbest().getMelhorPosicao()[0];
        //posicaoDestino[1] = memoriaDeSolucoes.getSolucaoLbest().getMelhorPosicao()[1];
        /*
        //se o agente não é o mais indicado para ir até a posição, por conta da distância ou por outro agente já ter
        if(!seAgenteMaisProximoDoLbest()){
            System.out.println("Agente "+idAgente+" não é o mais indicado para ir à  posição: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]);
            posicaoDestino = memoriaDeSolucoes.getSolucaoLbest().gerarPosicaoAproximada();
        }

        System.out.println("Agente "+idAgente+" posicaoDestino: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]);
        if(posicaoDestino[0]==posicao[0]&& posicaoDestino[1]==posicao[1]){
            System.out.println("Agente "+idAgente+" não deve permanecer na  posição: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]);
            posicaoDestino = memoriaDeSolucoes.getSolucaoLbest().gerarPosicaoAproximada();
            System.out.println("Agente "+idAgente+" posicaoDestino: "+ posicaoDestino[0]+ ","+ posicaoDestino[1]);
           //posicaoDestino[0] +=20*Math.random()-10;//numero aleatorio entre -10 e 10
           //posicaoDestino[1] +=20*Math.random()-10;
        }
        */
        //Verifica se a podicaoDestino não encontra-se dentro do espaço de busca, espera 1 ciclo caso sim
        if(!sePosicaoDestinoPertenceAoEspacoBusca(limiteMaxCoordX, limiteMaxCoordY)){
            System.out.println("Agente "+idAgente+" ("+posicaoDestino[0]+","+posicaoDestino[1]+") Posicao Destino Fora do Espaco!");
            posicaoDestino[0] = posicao[0];
            posicaoDestino[1] = posicao[1];
            estadoDePausa = true;//marca para evitar que ele fique tirando foto do mesmo lugar
        }
        //Se a podicaoDestino pertence a área de repulsao de um obstáculo
        if(sePosicaoDestinoPertenceAreaDeRepulsaoDeObstaculo()){
            System.out.println("Agente "+idAgente+" ("+posicaoDestino[0]+","+posicaoDestino[1]+") Posicao Destino Pertence a Campo de Repulsao!");
            posicaoDestino[0] = posicao[0];
            posicaoDestino[1] = posicao[1];
            estadoDePausa = true;
        }
        alvoEncontrado = false;
    }

    public void comunicacao(){

        ConjuntoDeAgentes idsVizinhos = sensorDeVizinhanca.sensorResponse(posicao);

        Solucao solucaoLbestVizinho;
        ConjuntoDeSolucoes memoriaDeSolucoesDoVizinho;
        for (int v = 0; v < idsVizinhos.getNumAgentes(); v++) {

            //Se o vizinho encontrou objetivo
            if( this.isEstadoPSO() && idsVizinhos.get(v).isEstadoPSO()){
                memoriaDeSolucoesDoVizinho = idsVizinhos.get(v).getMemoriaDeSolucoes();
                solucaoLbestVizinho = memoriaDeSolucoesDoVizinho.getSolucaoLbest();//pega o valor do lbest(gbest) que o vizinho encontrou
                if(solucaoLbestVizinho.getFitness() > memoriaDeSolucoes.getSolucaoLbest().getFitness()){
                    //se a solução do vizinho for melhor que a minha atual, atualiza a minha
                    memoriaDeSolucoes.setSolucaoLbest(solucaoLbestVizinho);
                    System.out.println("comunicacao - Agente "+ idAgente+" diz: Vizinho "+ idsVizinhos.get(v).getIdAgente()+ " tem melhor Lbest!");
                }
                else if(solucaoLbestVizinho.getFitness() < memoriaDeSolucoes.getSolucaoLbest().getFitness()){
                    //se a solução do vizinho for pior que a minha atual, o vizinho atualiza a sua
                    memoriaDeSolucoesDoVizinho.setSolucaoLbest(memoriaDeSolucoes.getSolucaoLbest());
                    System.out.println("comunicacao - Agente "+ idAgente+" diz: Vizinho "+ idsVizinhos.get(v).getIdAgente()+ " não tem melhor Lbest!");
                }
                else{
                    System.out.println("comunicacao - Agente "+ idAgente+" diz: Vizinho "+ idsVizinhos.get(v).getIdAgente()+ " tem igual Lbest!");
                }
            }
            //else if( !this.isEstadoPSO()){
                memoriaDeSolucoesDoVizinho = idsVizinhos.get(v).getMemoriaDeSolucoes(); //.getUltimosLocaisAvaliados();
                atualizarMemoriaDeSolucoesRecebidas( memoriaDeSolucoesDoVizinho );
            //}
        }
        
    }

    public void comunicacao2(){

        ConjuntoDeAgentes idsVizinhos = sensorDeVizinhanca.sensorResponse(posicao);

        Solucao solucaoLbestVizinho;
        ConjuntoDeSolucoes memoriaDeSolucoesDoVizinho;
        Agente vizinho;
        for (int v = 0; v < idsVizinhos.getNumAgentes(); v++) {
            vizinho = idsVizinhos.get(v);
            //Se o vizinho encontrou objetivo
            if( this.isEstadoPSO() && vizinho.isEstadoPSO()){
                memoriaDeSolucoesDoVizinho = vizinho.getMemoriaDeSolucoes();
                solucaoLbestVizinho = memoriaDeSolucoesDoVizinho.getSolucaoLbest();//pega o valor do lbest(gbest) que o vizinho encontrou
                if(solucaoLbestVizinho.getFitness() > memoriaDeSolucoes.getSolucaoLbest().getFitness()){
                    //se a solução do vizinho for melhor que a minha atual e nao ta na minha lista de descartadas, atualiza a minha
                    if(!memoriaDeSolucoes.solucoesDescartadasContem(solucaoLbestVizinho)){
                        memoriaDeSolucoes.setSolucaoLbest(solucaoLbestVizinho);
                        System.out.println("comunicacao2 - Agente "+ idAgente+" diz: Vizinho "+ vizinho.getIdAgente()+ " tem melhor Lbest!");
                    }
                }
                else if(solucaoLbestVizinho.getFitness() < memoriaDeSolucoes.getSolucaoLbest().getFitness()){
                    //se a solução do vizinho for pior que a minha atual, o vizinho atualiza a sua
                    if(! memoriaDeSolucoesDoVizinho.solucoesDescartadasContem(memoriaDeSolucoes.getSolucaoLbest())){
                        memoriaDeSolucoesDoVizinho.setSolucaoLbest(memoriaDeSolucoes.getSolucaoLbest());
                        System.out.println("comunicacao2 - Agente "+ idAgente+" diz: Vizinho "+ vizinho.getIdAgente()+ " não tem melhor Lbest!");
                    }
                }
                else{
                    System.out.println("comunicacao2 - Agente "+ idAgente+" diz: Vizinho "+ vizinho.getIdAgente()+
                                        " tem Lbest igual");
                }
                memoriaDeSolucoesDoVizinho = idsVizinhos.get(v).getMemoriaDeSolucoes(); //.getUltimosLocaisAvaliados();
                atualizarMemoriaDeSolucoesRecebidas( memoriaDeSolucoesDoVizinho );
            }
            else if( this.isEstadoPSO() && !vizinho.isEstadoPSO()){
                vizinho.setEstadoPSO(true);
                vizinho.getMemoriaDeSolucoes().removerTodos();
                vizinho.getMemoriaDeSolucoesRecebidas().removerTodos();
                System.out.println("comunicacao2 - Agente "+ idAgente+" diz: Fiz Vizinho "+ vizinho.getIdAgente()+ " entrar no modo MI!");
            }
        }
    }

    public boolean seAgenteMaisAdequadoParaPosicaoDestino(){// se existe outro agente com esse destino

        ConjuntoDeAgentes idsVizinhos = sensorDeVizinhanca.sensorResponse(posicao);
        Agente vizinho;

        double distVizinhoParaLbest=0;
        double distAgenteParaLbest = Math.sqrt( Math.pow( Math.abs(posicao[0] - posicaoDestino[0]) , 2)
                                                +
                                                Math.pow( Math.abs(posicao[1] - posicaoDestino[1]) , 2) );
        boolean adequado =true;
        for (int v = 0; v < idsVizinhos.getNumAgentes(); v++) {
            vizinho = idsVizinhos.get(v);
            
            //Se o vizinho encontrou objetivo
            if( this.isEstadoPSO() && vizinho.isEstadoPSO()){
                if(vizinho.getPosicaoDestino()[0]==posicaoDestino[0] && vizinho.getPosicaoDestino()[1]==posicaoDestino[1]){
                    System.out.println("Agente "+ idAgente+" testa seAgenteMaisAdequado: false, outro ja possui o mesmo lbest --> true");
                    return false;// agente não adequado, pois a posicao de destino ja tem "dono"
                }
                /*sempre vai haver um agente mais proximo... (o agente q gerou o lbest)
                else{//se a posicao destino de outro agente estiver proxima da nova posicao destino testada, ele(vizinho) será escolhido para ir ate ela...
                    distVizinhoParaLbest = Math.sqrt(   Math.pow( Math.abs(vizinho.getPosicao()[0] - posicaoDestino[0]) , 2)
                                                        +
                                                        Math.pow( Math.abs(vizinho.getPosicao()[1] - posicaoDestino[1]) , 2) );
                    System.out.println("Agente "+ idAgente+" diz: distVizinhoParaLbest: "+distVizinhoParaLbest+" distAgenteParaLbest: "+distAgenteParaLbest);
                    if(distVizinhoParaLbest<distAgenteParaLbest)// e não esta setado pra um quadrante destino da mesma solucao(evitar q seja o cara q gerou a solução)
                       return false;// agente não adequado, pois a posicao de destino esta mais proxima de outro agente

                }*/
            }

        }
        if(idsVizinhos.getNumAgentes()!=0){
            System.out.println("Agente "+ idAgente+" testa seAgenteMaisAdequado: true (outro ja possui o mesmo lbest-->false");
        }
        else{
            System.out.println("Agente "+ idAgente+" não testa seAgenteMaisAdequado, pois nao detecta vizinho");
        }
        return true;
    }

    private boolean sePosicaoDestinoPertenceAoEspacoBusca(int limiteMaxCoordX, int limiteMaxCoordY){
        //Verifica se encontra-se dentro do espaço de busca
        if (posicaoDestino[0] >= 0 && posicaoDestino[0] <= limiteMaxCoordX &&
               posicaoDestino[1] >= 0 && posicaoDestino[1] <= limiteMaxCoordY) {
            return true;
        }
        else {
            return false;
        }
    }


    private boolean seJaAvaliouPosicao(){
        double[] localJaVisitado;
        double diametroAlcance;
        double distanciaEntreOsPontos;
        boolean posicaoJaAvaliada = false;
        ConjuntoDeSolucoes memoriaDeSolucoesTemp;

        //Testa se a posicao de destino ja foi avaliada
        //memoriaDeSolucoesTemp = this.memoriaDeSolucoes;//.getUltimosLocaisAvaliados();
        diametroAlcance = getSensorFitness().getSensorDiametro(); //Rd = 2*Rsensor
        if(estadoPSO){// se MI
            distanciaEntreOsPontos = diametroAlcance/2;//raio
        }else{distanciaEntreOsPontos = diametroAlcance;}

        posicaoJaAvaliada = posicaoJaAvaliada ||memoriaDeSolucoes.testarDistanciaMinimaPermitidaEntrePontos(posicaoDestino, distanciaEntreOsPontos, false);

        /*
        for (int idSolucao = 0;  idSolucao < memoriaDeSolucoesTemp.getNumSolucoes();  idSolucao++) {
                localJaVisitado = memoriaDeSolucoesTemp.get(idSolucao).getPosicao();
                distanciaEntreOsPontos = Math.sqrt(
                                                Math.pow( Math.abs(localJaVisitado[0] - posicaoDestino[0]) , 2)
                                                +
                                                Math.pow( Math.abs(localJaVisitado[1] - posicaoDestino[1]) , 2) );
                if ( distanciaEntreOsPontos < diametroAlcance){
                   return true; //posicao ja avaliada
                }
        }*/
        //Testa se a posicao de destino ja foi avaliada pelos vizinhos
       
      

       /*double raioDaSolucao;
       double distaciaMinimaPermitida;
       for (int idSolucao = 0;  idSolucao < memoriaDeSolucoesTemp.getNumSolucoes();  idSolucao++) {
            localJaVisitado = memoriaDeSolucoesTemp.get(idSolucao).getPosicao();
            raioDaSolucao =   memoriaDeSolucoesTemp.get(idSolucao).getRaioDaAreaDaImagemEmMetros();
            //Se a podicaoDestino pertence a uma área que algum vizinho já avaliou, espera 1 ciclo
            distanciaEntreOsPontos = Math.sqrt(
                                            Math.pow( Math.abs(localJaVisitado[0] - posicaoDestino[0]) , 2)
                                            +
                                            Math.pow( Math.abs(localJaVisitado[1] - posicaoDestino[1]) , 2) );
            distaciaMinimaPermitida = raioSensorDoAgente +raioDaSolucao;
            if ( distanciaEntreOsPontos < distaciaMinimaPermitida){//lembrando que diametroAlcance==2*R...
                return true; //posicao ja avaliada
            }
        }*/
        return posicaoJaAvaliada;
    }

    public boolean sePosicaoDestinoJaFoiAvaliadaPorVizinho(){
        boolean posicaoJaAvaliada = false;
        double raioSensorDoAgente = getSensorFitness().getSensorDiametro()/2;
        double distanciaEntreOsPontos = raioSensorDoAgente;
        posicaoJaAvaliada = posicaoJaAvaliada || memoriaDeSolucoesRecebidas.testarDistanciaMinimaPermitidaEntrePontos(posicaoDestino, distanciaEntreOsPontos, false);
        return posicaoJaAvaliada;
    }
    private boolean sePosicaoDestinoPertenceAreaDeRepulsaoDeObstaculo(){
        EspacoDeBusca espacoDeBusca = EspacoDeBusca.getInstance();
        ConjuntoDeObstaculos conjuntoDeObstaculos = espacoDeBusca.getObstaculos();
        double distanciaEntreOsPontos;
        double distanciaMinimaEsperada;
        Obstaculo obstaculo;
        for(int i=0; i< conjuntoDeObstaculos.getNumObstaculos();i++){
                obstaculo =  conjuntoDeObstaculos.get(i);
                distanciaMinimaEsperada = obstaculo.getRaioAreaRepulsao() + sensorAntiColisao.getSensorDiametro();//(sensorAntiColisao.getSensorDiametro()/2);
                 //Se a podicaoDestino pertence a uma área de repulsao de obstaculo
                distanciaEntreOsPontos = Math.sqrt(
                                                Math.pow( Math.abs(obstaculo.getPosicao()[0] - posicaoDestino[0]) , 2)
                                                +
                                                Math.pow( Math.abs(obstaculo.getPosicao()[1] - posicaoDestino[1]) , 2) );
                if ( distanciaEntreOsPontos < distanciaMinimaEsperada){
                    return true; //posicao pertence a um campo de repulsao
                }
         }
         return false;
    }

    public void estabelecerAlvo(Alvo alvo){
        //////////////////////////////////////////////////////////////////////////////////////////
        ((SensorFotografico) sensorFitness).setCorAlvo(alvo.getCorTextura().getRed(),
                                                       alvo.getCorTextura().getGreen(),
                                                       alvo.getCorTextura().getBlue(),
                                                       alvo.getCorTextura().getRed(),
                                                       alvo.getCorTextura().getGreen(),
                                                       alvo.getCorTextura().getBlue());
    }

    public void atualizarMemoriaDeSolucoesRecebidas(ConjuntoDeSolucoes memoriaDeSolucoesDoVizinho){
        Solucao solucao;
        double[] localJaVisitado;
        for (int idSolucao = 0; idSolucao < memoriaDeSolucoesDoVizinho.getNumSolucoes(); idSolucao++) {
            solucao = memoriaDeSolucoesDoVizinho.get(idSolucao);
            localJaVisitado = solucao.getPosicao();
            if(!this.memoriaDeSolucoesRecebidas.existeSolucao(localJaVisitado))
                this.memoriaDeSolucoesRecebidas.inserirSolucao(solucao);
        }
        
    }
    public boolean sePosicaoAgenteIgualPosiaoDestino(){
        if ((int) Math.round(posicao[0]) == posicaoDestino[0] && (int) Math.round(posicao[1]) == posicaoDestino[1]) {
            return true;
        }
        else
            return false;
    }

    public String toString() {
        String retorno = "Agente: " + idAgente + " - Posicao: (" + posicao[0] + "," + posicao[1] + ") - alvoEncontrado: " + alvoEncontrado;
        return retorno;
    }
    public String showPosicao() {
        String retorno = "Posicao: (" + posicao[0] + "," + posicao[1] + ")";
        return retorno;
    }
    public String showPosicaoDestino() {
        String retorno = "Posicao Destino: (" + posicao[0] + "," + posicao[1] + ")";
        return retorno;
    }

    //----------------------MAIN------------------------//
    public static void main(String[] args) {
        // TODO Auto-generated method stub
    }

    //----------------getters & setters-----------------//
    public int getIdAgente() {
        return idAgente;
    }

    public void setIdAgente(int idAgente) {
        this.idAgente = idAgente;
    }

    public double getFitness() {
        return fitness;
    }

    public void setFitness(double fitness) {
        this.fitness = fitness;
    }

    public SensorDeVizinhanca getSensorDeVizinhanca() {
        return sensorDeVizinhanca;
    }

    public void setSensorDeVizinhanca(SensorDeVizinhanca sensorDeVizinhanca) {
        this.sensorDeVizinhanca = sensorDeVizinhanca;
    }

    public SensorFitness getSensorFitness() {
        return sensorFitness;
    }

    public void setSensorFitness(SensorFitness sensorFitness) {
        this.sensorFitness = sensorFitness;
    }

    public SensorAntiColisao getSensorAntiColisao() {
        return sensorAntiColisao;
    }

    public void setSensorAntiColisao(SensorAntiColisao sensorAntiColisao) {
        this.sensorAntiColisao = sensorAntiColisao;
    }

    public double[] getVelocidadePolar() {
        return velocidadePolar;
    }

    public void setVelocidadePolar(double[] velocidadePolar) {
        this.velocidadePolar = velocidadePolar;
    }

    public boolean isEstadoDePausa() {
        return  estadoDePausa;
    }

    public void setEstadoDePausa(boolean estadoDePausa) {
        this.estadoDePausa = estadoDePausa;
    }

    public double[] getPosicao() {
        return posicao;
    }

    public void setPosicao(double[] posicao) {
        this.posicao = posicao;
    }

    public double[] getPosicaoDestino() {
        return posicaoDestino;
    }

    public void setPosicaoDestino(double[] posicaoDestino) {
        this.posicaoDestino = posicaoDestino;
    }

    public boolean getAlvoEncontrado() {
        return alvoEncontrado;
    }

    public void setAlvoEncontrado(boolean alvoEncontrado) {
        this.alvoEncontrado = alvoEncontrado;
    }

    public double getComprimentoHorizontal() {
        return comprimentoHorizontal;
    }

    public void setComprimentoHorizontal(double comprimentoHorizontal) {
        this.comprimentoHorizontal = comprimentoHorizontal;
    }

    public double getComprimentoVertical() {
        return comprimentoVertical;
    }

    public void setComprimentoVertical(double comprimentoVertical) {
        this.comprimentoVertical = comprimentoVertical;
    }

    public boolean isMostrarTrajetoria() {
        return mostrarTrajetoria;
    }

    public void setMostrarTrajetoria(boolean mostrarRastro) {
        this.mostrarTrajetoria = mostrarRastro;
    }

    public int getNumeroMaxDeSolucoesParaTransmitir(){
        return memoriaDeSolucoes.getNumeroMaxDeSolucoesParaTransmitir();
    }
    
    public void setNumeroMaxDeSolucoesParaTransmitir(int numeroMaxDeSolucoesParaTransmitir){
        memoriaDeSolucoes.setNumeroMaxDeSolucoesParaTransmitir(numeroMaxDeSolucoesParaTransmitir);
    }

    public ConjuntoDeSolucoes getMemoriaDeSolucoes() {
        return memoriaDeSolucoes;
    }

    public ConjuntoDeSolucoes getMemoriaDeSolucoesRecebidas() {
        return memoriaDeSolucoesRecebidas;
    }

    public boolean isEstadoPSO() {
        return estadoPSO;
    }

    public void setEstadoPSO(boolean estadoPSO) {
        this.estadoPSO = estadoPSO;
    }

    public boolean isEscolheuPosicaoAleatoria() {
        return escolheuPosicaoAleatoria;
    }

    public void setEscolheuPosicaoAleatoria(boolean escolheuPosicaoAleatoria) {
        this.escolheuPosicaoAleatoria = escolheuPosicaoAleatoria;
    }


}
