gwiazdalak

import java.awt.Color;
import java.awt.Font;
import java.awt.Graphics2D;
import java.util.ArrayList;
import robocode.AdvancedRobot;
import robocode.*;
import robocode.util.Utils;
import static robocode.util.Utils.normalAbsoluteAngle;
import static robocode.util.Utils.normalRelativeAngle;

/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

/**
 *
 * @author Gruby
 */
public class gwiazdkalak extends AdvancedRobot{
    int szerokosc_kratki = 40;
    ArrayList<Wierzchol> wierzcholki = new ArrayList<>();
    ArrayList<Przeszkody> przeszkody = new ArrayList<>();
    ArrayList<Mapa> closed = new ArrayList<>();
    ArrayList<ArrayList<Mapa>> mapa = new ArrayList<>();
    boolean rysuj_trase = false;
    int liczba_robotow = 0;
    int celY = 24;
    int celX = 4;
    int kosztg = 20;

    @Override
    public void run(){

        stworz_plansze();
        //astar();
        setTurnRight(90);
        waitFor(new TurnCompleteCondition(this));
        setTurnLeft(180);
        waitFor(new TurnCompleteCondition(this));
        setTurnRight(180);
        waitFor(new TurnCompleteCondition(this));
        a();
        rysuj_trase = true;

        while (true) {
            setTurnRight(90);
            waitFor(new TurnCompleteCondition(this));
            setTurnLeft(180);
            waitFor(new TurnCompleteCondition(this));
            setTurnRight(180);
            waitFor(new TurnCompleteCondition(this));
//
        }
    }

    public void a(){

        // aktualna pozycja
        int akt_x;
        int akt_y;
        double najmniejsza;
        Wierzchol najmniejsza_mapa;
        ArrayList<Wierzchol> otwarta = new ArrayList<>();
        wierzcholki.add(new Wierzchol(3, 3));
        mapa.get(3).get(3).zajety = true;

//        while(wierzcholki.size() != 50){

        while(wierzcholki.get(wierzcholki.size()-1).y != celY){
            otwarta.clear();
            najmniejsza = 100000;
            najmniejsza_mapa = new Wierzchol(0, 0);

            // badanie dostepnych wierzcholkow
            akt_x = wierzcholki.get(wierzcholki.size()-1).x;
            akt_y = wierzcholki.get(wierzcholki.size()-1).y;
            dodaj_do_otwartej(otwarta, akt_x, akt_y);

            // wycofaj jezeli slepa uliczka
            while (otwarta.isEmpty()){
                wierzcholki.remove(wierzcholki.size()-1);
                akt_x = wierzcholki.get(wierzcholki.size()-1).x;
                akt_y = wierzcholki.get(wierzcholki.size()-1).y;
                dodaj_do_otwartej(otwarta, akt_x, akt_y);
            }

            // najmniejszy koszt podrozy
            for (int i = 0; i < otwarta.size(); i++){
                if (najmniejsza
                        > mapa.get(otwarta.get(i).y).get(otwarta.get(i).x).f)
                {
                    najmniejsza =
                            mapa.get(otwarta.get(i).y).get(otwarta.get(i).x).f;
                    najmniejsza_mapa = otwarta.get(i);
                }
            }

            // dodanie wierzcholka z najmniejszym f
            wierzcholki.add(new Wierzchol(najmniejsza_mapa.x,
                    najmniejsza_mapa.y));
            mapa.get(najmniejsza_mapa.y).get(najmniejsza_mapa.x).zajety = true;
        }
    }

    public void dodaj_do_otwartej(ArrayList<Wierzchol> otwarta, int x, int y) {
        if (x >0 && mapa.get(y).get(x-1).zajety != true){
            otwarta.add(new Wierzchol(x-1, y));
            aktualizuj_mape(x, y, x-1, y);
        }
        if (x < 24 && mapa.get(y).get(x+1).zajety != true){
            otwarta.add(new Wierzchol(x+1, y));
            aktualizuj_mape(x, y, x+1, y);
        }
        if (y > 0 && mapa.get(y-1).get(x).zajety != true){
            otwarta.add(new Wierzchol(x, y-1));
            aktualizuj_mape(x, y, x, y-1);
        }
        if (y < 24 && mapa.get(y+1).get(x).zajety != true){
            otwarta.add(new Wierzchol(x, y+1));
            aktualizuj_mape(x, y, x, y+1);
        }
    }

    public void aktualizuj_mape(int aktx, int akty, int otwx, int otwy){
        mapa.get(otwy).get(otwx).h = pitagorasalak(celX, celY, otwx, otwy);
        mapa.get(otwy).get(otwx).g = mapa.get(akty).get(aktx).g + kosztg;
        mapa.get(otwy).get(otwx).aktualizuj();
    }

    public double pitagorasalak(int x1, int y1, int x2, int y2){
        double loc_x1 = mapa.get(y1).get(x1).x;
        double loc_y1 = mapa.get(y1).get(x1).y;
        double loc_x2 = mapa.get(y2).get(x2).x;
        double loc_y2 = mapa.get(y2).get(x2).y;

        return Math.sqrt(Math.pow(loc_y2 - loc_y1, 2)
                + Math.pow(loc_x2 - loc_x1, 2));
    }

    public void jedz(int nrx, int nry) {
        double celx = mapa.get(nry).get(nrx).x + szerokosc_kratki/2;
        double cely = mapa.get(nry).get(nrx).y + szerokosc_kratki/2;
        setTurnRightRadians(angleToTurnInRadians(celx, cely));
        waitFor(new TurnCompleteCondition(this));
            while(dystans(celx, cely) > 5){
                setAhead(50);
                execute();

            }
//        }
        setAhead(0);
    }

    public double dystans(double x, double y){
        return Math.sqrt(Math.pow(y - getY(), 2) + Math.pow(x - getX(), 2));
    }

    public double angleToTurnInRadians(double x, double y){
        double angle = normalAbsoluteAngle(Math.atan2(x - getX(), y - getY()));
        return normalRelativeAngle(angle - getHeadingRadians());
    }

    public void stworz_plansze(){
        int x_m = 0;
        int y_m = 0;
        int liczba_kratek = 1000/szerokosc_kratki;

        for(int j=0; j<liczba_kratek; j++){
            ArrayList<Mapa> mapalista = new ArrayList<>();
            for(int i=0; i<liczba_kratek; i++){
                mapalista.add(new Mapa(x_m, y_m, false, 10000, 10000));
                x_m=x_m+40;
            }
            mapa.add(mapalista);
            y_m=y_m+40;
            x_m=0;
        }
    }

    @Override
    public void onScannedRobot(ScannedRobotEvent e) {
//        double angle = Math.toRadians((getHeading() + e.getBearing()) % 360);
        if(liczba_robotow < 300){
        double absoluteBearing = getHeadingRadians() + e.getBearingRadians();
        double enemyX = getX() + e.getDistance() * Math.sin(absoluteBearing);
        double enemyY = getY() + e.getDistance() * Math.cos(absoluteBearing);

        for (int i = 0; i < mapa.size(); i++) {
            for (int j = 0; j < mapa.size(); j++) {
                if(((mapa.get(i).get(j).x >= enemyX-szerokosc_kratki
                        && mapa.get(i).get(j).x<=enemyX)
                        && (mapa.get(i).get(j).y >= enemyY-szerokosc_kratki
                        && mapa.get(i).get(j).y<=enemyY))
                        && mapa.get(i).get(j).zajety != true){
                    mapa.get(i).get(j).zajety=true;
                    liczba_robotow += 1;
                }
           }
        }
        }
    }

    @Override
    public void onStatus( StatusEvent e) {
    // you can also put all setAdjustXX(..) methods here
        setTurnRadarRightRadians(Double.POSITIVE_INFINITY);
    }

    @Override
    public void onPaint(Graphics2D g){
        g.setColor(Color.blue);
        /// rysowalak siatki
        for (int i = 0; i < 25; i++) {
            for (int j = 0; j < 25; j++) {
                g.setColor(Color.blue);
                g.drawRect(mapa.get(i).get(j).x, mapa.get(i).get(j).y,
                        szerokosc_kratki, szerokosc_kratki);
                if(mapa.get(i).get(j).zajety==true){
                    g.setColor(Color.red);
                    g.fillRect(mapa.get(i).get(j).x, mapa.get(i).get(j).y,
                            szerokosc_kratki, szerokosc_kratki);
                }
            }
        }
        if(rysuj_trase == true){
            g.setColor(Color.green);
            for(int i=0; i<wierzcholki.size(); i++){
                int iks = wierzcholki.get(i).x;
                int igr = wierzcholki.get(i).y;
                g.fillRect(mapa.get(igr).get(iks).x, mapa.get(igr).get(iks).y,
                            szerokosc_kratki, szerokosc_kratki);
            }
        }
    }


    public class Mapa
    {
        public int x=0;
        public int y=0;
        public boolean zajety = false;
        public double h=0;
        public double g=0;
        public double f=0;

        public void aktualizuj(){
            this.f = this.g + this.h;
        }

        public Wierzchol zwroc_wierzchol(){
            int x_indeks = (int) this.x / 40;
            int y_indeks = (int) this.y / 40;

            return new Wierzchol(x_indeks, y_indeks);
        }

        public Mapa(int x_m,int y_m, boolean zajete,int h_m,int g_m){
                this.x=x_m ;
                this.y=y_m;
                this.zajety=zajete;
                this.h=h_m;
                this.g=g_m;
                this.f=this.h+this.g;


           }
    }
    public class Wierzchol
    {
        public int x=0;
        public int y=0;

        public Wierzchol(int x_m,int y_m){
                x=x_m ;
                y=y_m;
        }
    }

    public class Przeszkody
    {
        public int x=0;
        public int y=0;

        public Przeszkody(int x_m,int y_m){
                x=x_m ;
                y=y_m;
        }
    }
}