/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package algoritmo;


import auxiliar.Conversor;
import java.awt.geom.Rectangle2D;

/**
 *
 * @author Marília
 */
public class SensorAntiColisao extends Sensor{

    private ConjuntoDeObstaculos obstaculos;
    private ConjuntoDeAgentes agentes;
    private double[] vetorRepulsaoResultante;
    int anguloVetorRepulsaoResultante;

    public SensorAntiColisao(){
        super();
        vetorRepulsaoResultante = new double[2];
		obstaculos = ConjuntoDeObstaculos.getInstance();
        agentes = EspacoDeBusca.getInstance().getAgentes();
        setSensorDiametro(50);//em metros (Rcomunicacao = 4*Rsensor)
        setCor(new java.awt.Color(50, 255, 50,150));//cor que representa o sensor(verde)
	}

    //retorna true se houver algum obstaculo dentro da área de alcance do sensor
    public boolean sensorResponse(double[] posicaoDoAgente){
        //double[] C = {9,5};
        //double Rad = 3;
        //Rectangle R = new Rectangle(3,4,4,3);
        //checarIntersecao(R, C, Rad);

        Obstaculo obstaculo;
        boolean detectouAlgumObstaculo = false;
        boolean detectouObstaculo = false;
        vetorRepulsaoResultante =new double[2];
        
        //Testa os obstáculos estáticos
        for(int i=0; i<obstaculos.getNumObstaculos() ;i++){
            obstaculo = obstaculos.get(i);
            detectouObstaculo = checarIntersecao(obstaculo.getPosicao(), obstaculo.getRaioAreaRepulsao(), posicaoDoAgente, (getSensorDiametro()/2)  );
            if(detectouObstaculo){
                atribuirRepulsaoAoVetorResultante(obstaculo.getPosicao(),posicaoDoAgente);
                
                double[] vetorRepulsaoReultanteCoordPolar = Conversor.coordCartesianaParaPolar(vetorRepulsaoResultante[0], vetorRepulsaoResultante[1]);
                anguloVetorRepulsaoResultante = (int) Math.round(vetorRepulsaoReultanteCoordPolar[1]);
                detectouAlgumObstaculo = detectouAlgumObstaculo || detectouObstaculo;
            }
        }
        //Testa os obstáculos dinâmicos (outros agentes)
        Agente agente;
        double raioAreaRepulsaoAgente_i;
        for(int i=0; i<agentes.getNumAgentes() ;i++){

            agente = agentes.get(i);
             //Exceto quando i é o proprio agente dono do sensor, testa se há colisão
             if((agente.getPosicao()[0] != posicaoDoAgente[0]) || (agente.getPosicao()[1] != posicaoDoAgente[1])){
                raioAreaRepulsaoAgente_i = agente.getSensorAntiColisao().getSensorDiametro()/2;//raio da área de repulsão é igual ao da área de detecção de colisão

                detectouObstaculo = checarIntersecao(agente.getPosicao(),raioAreaRepulsaoAgente_i , posicaoDoAgente, (getSensorDiametro()/2)  );
                if(detectouObstaculo){
                     atribuirRepulsaoAoVetorResultante(agente.getPosicao(),posicaoDoAgente);

                     double[] vetorRepulsaoReultanteCoordPolar = Conversor.coordCartesianaParaPolar(vetorRepulsaoResultante[0], vetorRepulsaoResultante[1]);
                     anguloVetorRepulsaoResultante = (int) Math.round(vetorRepulsaoReultanteCoordPolar[1]);
                     detectouAlgumObstaculo = detectouAlgumObstaculo || detectouObstaculo;
                }
             }
        }
        return detectouAlgumObstaculo;
      
    }
    //Checa interseção de retângulo com círculo
    public boolean checarIntersecao(Rectangle2D R, double[] C, double Rad){

         /* Returna TRUE se a area retangular R intersecta o circulo 
          de centro C (posicaoDoAgente do agente) e raio Rad (raio detecção de colisão do agente). */

        double Rad2;

        Rad2 = Rad * Rad;

        double Rmax_x = R.getMaxX();
        double Rmax_y = R.getMaxY();
        double Rmin_x = R.getMinX();
        double Rmin_y = R.getMinY();
        /* Translate coordinates, placing C at the origin. */
        Rmax_x -= C[0];//cx
        Rmax_y -= C[1];//cy
        Rmin_x -= C[0];//cx
        Rmin_y -= C[1];//cy

        if (Rmax_x < 0) /* R to left of circle center */ {
            if (Rmax_y < 0) /* R in lower left corner */ {
                return ((Rmax_x * Rmax_x + Rmax_y * Rmax_y) < Rad2);
            } else if (Rmin_y > 0) /* R in upper left corner */ {
                return ((Rmax_x * Rmax_x + Rmin_y * Rmin_y) < Rad2);
            } else /* R due West of circle */ {
                return (Math.abs(Rmax_x) < Rad);
            }
        } else if (Rmin_x > 0) /* R to right of circle center */ {
            if (Rmax_y < 0) /* R in lower right corner */ {
                return ((Rmin_x * Rmin_x + Rmax_y * Rmax_y) < Rad2);
            } else if (Rmin_y > 0) /* R in upper right corner */ {
                return ((Rmin_x * Rmin_x + Rmin_y * Rmin_y) < Rad2);
            } else /* R due East of circle */ {
                return (Rmin_x < Rad);
            }
        } else /* R on circle vertical centerline */ if (Rmax_y < 0) /* R due South of circle */ {
            return (Math.abs(Rmax_y) < Rad);
        } else if (Rmin_y > 0) /* R due North of circle */ {
            return (Rmin_y < Rad);
        } else /* R contains circle centerpoint */ {
            return (true);
        }
    }

    //Checa interseção de cículo com círculo
    public boolean checarIntersecao(double[] posicaoObstaculo, double raioAreaRespulsaoDoObstaculo,
                                            double[] posicaoAgente, double raioAreaDeteccaoDoAgente){

         /* Returna TRUE se a area retangular R intersecta o circulo
          de centro C (posicaoDoAgente do agente) e raio Rad (raio detecção de colisão do agente). */
         double distanciaDosCentros = Math.sqrt(
                                            Math.pow(posicaoObstaculo[0]-posicaoAgente[0],2)+
                                            Math.pow(posicaoObstaculo[1]-posicaoAgente[1],2));

         double distanciaMinimaEsperada = raioAreaRespulsaoDoObstaculo + raioAreaDeteccaoDoAgente;
        if(distanciaDosCentros < distanciaMinimaEsperada) //houve detecção de colisão
            return true;
        else
            return false;
    }

    public void atribuirRepulsaoAoVetorResultante(double[] posicaoObstaculo,double[] posicaoAgente){
          double[] vetorRepulsao =  new double[2];
          //vetor com origem no centro do obstáculo e fim no centro do agente
          vetorRepulsao[0] = posicaoAgente[0]-posicaoObstaculo[0];
          vetorRepulsao[1] = posicaoAgente[1]-posicaoObstaculo[1];
          vetorRepulsaoResultante[0]+=vetorRepulsao[0];
          vetorRepulsaoResultante[1]+=vetorRepulsao[1];

    }

    public int getAnguloVetorRepulsaoResultante() {
        return anguloVetorRepulsaoResultante;
    }

    public double[] getVetorRepulsaoResultante() {
        return vetorRepulsaoResultante;
    }

     //----------------------MAIN------------------------//
    @SuppressWarnings("empty-statement")
    public static void main(String[] args) {
        
    }

}
