A*

import robocode.Robot;
import robocode.ScannedRobotEvent;
import java.awt.*;
import java.lang.*;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Iterator;
import java.util.Map;
import java.util.TreeMap;
import java.awt.event.MouseEvent;
import java.util.Collections;

public class MujRobotKlasa extends Robot
{ //lub AdvancedRobot

    public class Node implements Comparable<Node>
    {
        Pozycja pos;
        Double gCost = 10000000.0;
        Double hCost = 10000000.0;
        Double fCost = 10000000.0;
        Pozycja parent;
        Boolean isStart = Boolean.FALSE;
        Boolean isEnd = Boolean.FALSE;

        public Node(Pozycja pos, Boolean start, Boolean end)
        {
            this.pos = pos;
            isStart = start;
            isEnd = end;
            if(start) gCost = 0.0;
        }

        public void setHCost(Pozycja end)
        {
            hCost = travelCost(pos, end);
        }

        public void setGcost(double cost)
        {
            gCost = cost;
        }

        public void setParent(Pozycja parPos)
        {
            parent = parPos;
        }

         @Override
        public int compareTo(Node other)
        {
            return this.fCost.compareTo(other.fCost);
        }
    }

    public class Pozycja implements Comparable<Pozycja>
    {
        Integer x;
        Integer xid;
        Integer y;
        Integer yid;

        public Pozycja(int xPos, int yPos)
        {
            x = xPos;
            xid = x/20;
            y = yPos;
            yid = y/20;
        }

        public Integer getId()
        {
            return x + y*2000;
        }

        @Override
        public int compareTo(Pozycja pos)
        {
            return this.getId().compareTo(pos.getId());
        }
    }

    ArrayList<Pozycja> Wrugowie = new ArrayList<>();
    Map<Pozycja, Boolean> mapTiles = new TreeMap<>();       // bool = czy wolna
    boolean notMappedYet = true;
    int bfHeight = 0;
    int bfWidth = 0;
    Map<Pozycja, Node> graph = new TreeMap<>();
    ArrayList<Node> droga = new ArrayList<>();

    double destX = -1;
    double destY = -1;
    double destDir = -1;
    double destDist = -1;

    Boolean testowyBool = Boolean.FALSE;


    public void run()
    {
        bfHeight = (int)getBattleFieldHeight();
        bfWidth = (int)getBattleFieldWidth();

        // ruch robota
        while (true)
        {
            turnRadarRight(360);

            //goToXY(destX, destY);
            if(droga.size() > 0)
            {
                Collections.reverse(droga);
                Iterator<Node> iterDroga = droga.iterator();
                Node nodeD = iterDroga.next();
                destX = nodeD.pos.x;
                destY = nodeD.pos.y;
                while(iterDroga.hasNext())
                {
                    if(Math.abs(getX()-destX) < 1 && Math.abs(getY()-destY) < 1)
                    {
                        nodeD = iterDroga.next();
                        destX = nodeD.pos.x;
                        destY = nodeD.pos.y;
                    }


                    goToXY(destX, destY);
                }
            }


        }
    }

    public void goToXY(double destX, double destY)
    {
        if(Math.abs(getX() - destX) > 0.1 || Math.abs(getY() - destY) > 0.1)
        {
            destDir = atan2deg(Math.toDegrees(Math.atan2(destX-getX(), destY-getY())));
            if(Math.abs(destDir - getHeading()) >= 0.001)
            {
                if(destDir-getHeading()>180)
                {
                    turnLeft(360-(destDir - getHeading()));
                }
                else turnRight(destDir - getHeading());
            }
            else ahead(Math.hypot(destX-getX(), destY-getY()));
        }
    }

    public void onScannedRobot(ScannedRobotEvent e)
    {
        // wyniki z radaru

        double myX = getX();
        double myY = getY();

        double kontWrogaWzgl = e.getBearing();
        double mojKont = this.getHeading();
        double kontWrogaBezWzgl = Math.toRadians((mojKont+kontWrogaWzgl) % 360);
        double odleglosc = e.getDistance();

        Integer nowyWrugX = (int)Math.round(Math.sin(kontWrogaBezWzgl)*odleglosc + myX);
        Integer nowyWrugY = (int)Math.round(Math.cos(kontWrogaBezWzgl)*odleglosc + myY);

        boolean jestJusz = false;
        Iterator<Pozycja> iteratorWrug = Wrugowie.iterator();
        while(iteratorWrug.hasNext())
        {
            Pozycja next = iteratorWrug.next();
            if(Math.abs(next.x-nowyWrugX)<5 && Math.abs(next.y-nowyWrugY)<5)
            {
                jestJusz = true;
                break;
            }
        }
        if(!jestJusz) Wrugowie.add(new Pozycja(nowyWrugX, nowyWrugY));
    }

    public void onPaint(Graphics2D g)
    {

        // Set the paint color to red
        double myX = getX();
        double myY = getY();

        Pozycja mojeTajle = xyToTile(myX, myY);

        //if(Wrugowie.size() == 123 && notMappedYet)mapToTiles();
        if(Wrugowie.size() == 123 && notMappedYet)
        {
            mapToTiles();
            notMappedYet = false;
            //aGwiazdka(mojeTajle, xyToTile(900,100));
            //System.out.println("rozmiar drogi: " + Integer.toString(droga.size()));
        }

        g.setColor(java.awt.Color.WHITE);
        if(droga.size() > 0)
        {
            System.out.println("rysujedroge");
            Iterator<Node> iterDroga = droga.iterator();
            while(iterDroga.hasNext())
            {
                Node nodeD = iterDroga.next();
                System.out.println(" B:" + Boolean.toString(stojable((double)nodeD.pos.x, (double)nodeD.pos.y)));
                g.fillOval(nodeD.pos.x-5, nodeD.pos.y-5, 10, 10);
            }
        }
        //mapToTiles();



        g.drawString("Muj  X: " + Double.toString(myX), 10, 990);
        g.drawString("Tile  X: " + Integer.toString(mojeTajle.x), 200, 990);
        g.drawString("Heading: " + Double.toString(getHeading()), 280, 990);
        g.drawString("Muj  Y: " + Double.toString(myY), 10, 978);
        g.drawString("Tile  Y: " + Integer.toString(mojeTajle.y), 200, 978);
        g.drawString("Dest H: " + Double.toString(destDir), 280, 978);
        g.drawString("Wroguf: " + Wrugowie.size(), 10, 966);
        g.drawString("Tile: " + mapTiles.size(), 10, 954);
        g.drawString("Zmapowane: " + Boolean.toString(!notMappedYet), 100, 954);
        g.drawString("Stojable: " + Boolean.toString(testowyBool), 10, 942);
        g.drawString("Droga: " + Integer.toString(droga.size()), 100, 942);

//        g.setColor(java.awt.Color.BLUE);
//        g.fillRect(mojeTajle.x*20, mojeTajle.y*20, 20, 20);

        g.setColor(java.awt.Color.RED);
        /*Iterator<Pozycja> iteratorWrug = Wrugowie.iterator();
        while(iteratorWrug.hasNext())
        {
            Pozycja next = iteratorWrug.next();
//            g.drawRect(next.x-20, next.y-20, 40, 40);
            g.fillRect(next.x-20, next.y-20, 40, 40);
        }*/
        g.setColor(java.awt.Color.GREEN);
        for(int cntH = 0; cntH<=bfHeight; cntH+=20)
        {
            g.drawLine(cntH, 0, cntH, bfWidth);
        }
        for(int cntW = 0; cntW<=bfWidth; cntW+=20)
        {
            g.drawLine(0, cntW, bfHeight, cntW);
        }

        g.setColor(java.awt.Color.BLUE);
        for(Map.Entry<Pozycja,Boolean> entry : mapTiles.entrySet())
        {
            if(entry.getValue() == false)
            {
                g.fillRect(entry.getKey().x, entry.getKey().y, 20, 20);
            }
        }
    }

    public void onMouseClicked(MouseEvent e)
    {
        //testowyBool = stojable(e.getX(), e.getY());
        //destX = e.getX();
        //destY = e.getY();
        destDist = Math.hypot(destX-getX(), destY-getY());
        Pozycja mojeTajle = xyToTile(getX(), getY());
        mapToTiles();
        if(!notMappedYet)aGwiazdka(mojeTajle, xyToTile(e.getX(),e.getY()));
        System.out.println("jest droga");
    }

    public double atan2deg(double atan)
    {
        if(atan >= 0) return atan;
        else return 360.0+atan;
    }

    public void mapToTiles()
    {
        if(mapTiles.isEmpty())
        {
            for(int cntH = 0; cntH<bfHeight; cntH+=20)
            {
                for(int cntW = 0; cntW<bfWidth; cntW+=20)
                {
                    //g.drawRect(cntW, cntH, 20, 20);
                    mapTiles.put(new Pozycja(cntW, cntH), Boolean.TRUE);
                }
            }

        }
        Iterator<Pozycja> iteratorWrug = Wrugowie.iterator();
        while(iteratorWrug.hasNext())
        {
            Pozycja next = iteratorWrug.next();

            int rog1X = next.x - 19;
            int rog1Y = next.y - 19;
            int rog2X = next.x + 19;
            int rog2Y = next.y - 19;
            int rog3X = next.x - 19;
            int rog3Y = next.y + 19;
            int rog4X = next.x + 19;
            int rog4Y = next.y + 19;

            mapTiles.put(xyToTile(rog1X, rog1Y), Boolean.FALSE);
            mapTiles.put(xyToTile(rog2X, rog2Y), Boolean.FALSE);
            mapTiles.put(xyToTile(rog3X, rog3Y), Boolean.FALSE);
            mapTiles.put(xyToTile(rog4X, rog4Y), Boolean.FALSE);
        }

        for(Map.Entry<Pozycja,Boolean> entry : mapTiles.entrySet())
        {
            if(entry.getValue() == Boolean.FALSE)
            {
                Pozycja pos = entry.getKey();
                if(!mapTiles.get(new Pozycja(pos.x+40, pos.y)))
                {
                    mapTiles.put(new Pozycja(pos.x+20, pos.y), Boolean.FALSE);
                }
                if(!mapTiles.get(new Pozycja(pos.x, pos.y+40)))
                {
                    mapTiles.put(new Pozycja(pos.x, pos.y+20), Boolean.FALSE);
                }
            }
        }
        notMappedYet = false;
    }

    Pozycja xyToTile(double x, double y)
    {
        int xTile = (int)Math.floor(x/20)*20;
        int yTile = (int)Math.floor(y/20)*20;

        return new Pozycja(xTile, yTile);
    }

    Boolean stojable(double x, double y)
    {
        if(x<19.9 || y<19.9 || x>bfWidth-19.9 || y>bfHeight-19.9) return Boolean.FALSE;
        if(!mapTiles.get(xyToTile(x, y))) return Boolean.FALSE;
        if(!mapTiles.get(xyToTile(x-19.9, y+19.9)) ||
           !mapTiles.get(xyToTile(x, y+19.9)) ||
           !mapTiles.get(xyToTile(x+19.9, y+19.9)) ||
           !mapTiles.get(xyToTile(x-19.9, y)) ||
           !mapTiles.get(xyToTile(x+19.9, y)) ||
           !mapTiles.get(xyToTile(x-19.9, y-19.9)) ||
           !mapTiles.get(xyToTile(x, y-19.9)) ||
           !mapTiles.get(xyToTile(x+19.9, y-19.9))) return Boolean.FALSE;

        return Boolean.TRUE;
    }

    double travelCost(Pozycja start, Pozycja end)
    {
        //double kont = Math.toDegrees(Math.atan2(end.y-start.y, end.x-start.x));
        int xAkt = start.x;
        int yAkt = start.y;
        double kosztJedn = 20;
        double wspKosztuSkos = Math.sqrt(2)*2;

        double koszt = 0;
        while(xAkt!=end.x || yAkt!=end.y)
        {
            if(xAkt < end.x && yAkt < end.y)    // skos gura prawo
            {
                koszt += wspKosztuSkos*kosztJedn;
                xAkt += 20;
                yAkt += 20;
            }
            else if(xAkt > end.x && yAkt > end.y)    // skos dul lewo
            {
                koszt += wspKosztuSkos*kosztJedn;
                xAkt -= 20;
                yAkt -= 20;
            }
            else if(xAkt < end.x && yAkt > end.y)    //skos dul prawo
            {
                koszt += wspKosztuSkos*kosztJedn;
                xAkt += 20;
                yAkt -= 20;
            }
            else if(xAkt > end.x && yAkt < end.y)    //skos gura lewo
            {
                koszt += wspKosztuSkos*kosztJedn;
                xAkt -= 20;
                yAkt += 20;
            }
            else if(xAkt < end.x)
            {
                koszt += kosztJedn;
                xAkt += 20;
            }
            else if(xAkt > end.x)
            {
                koszt += kosztJedn;
                xAkt -= 20;
            }
            else if(yAkt < end.y)
            {
                koszt += kosztJedn;
                yAkt += 20;
            }
            else if(yAkt > end.y)
            {
                koszt += kosztJedn;
                yAkt -= 20;
            }
        }

        return koszt;
    }

    void createGraph(Pozycja start, Pozycja end)
    {
        for(Map.Entry<Pozycja,Boolean> entry : mapTiles.entrySet())
        {
            if(entry.getValue() == Boolean.TRUE)
            {
                if(stojable(entry.getKey().x, entry.getKey().y))
                {
                    if(entry.getKey().x - start.x == 0 && entry.getKey().y - start.y == 0)
                    {
                        graph.put(entry.getKey(), new Node(entry.getKey(), true, false));
                        System.out.println("Znaleziono start");
                    }
                    else if(entry.getKey().x - end.x == 0 && entry.getKey().y - end.y == 0)
                    {
                        graph.put(entry.getKey(), new Node(entry.getKey(), false, true));
                        System.out.println("Znaleziono koniec");
                    }
                    else graph.put(entry.getKey(), new Node(entry.getKey(), false, false));
                    //graph.get(entry.getKey()).setCost(start, end);
                }
            }
        }
    }

    void aGwiazdka (Pozycja start, Pozycja end)
    {
        createGraph(start, end);
        droga.clear();

        ArrayList<Node> openNodes = new ArrayList<Node>();
        ArrayList<Node> closedNodes = new ArrayList<Node>();
        Node curNode;
        ArrayList<Node> succesors = new ArrayList<Node>();

        openNodes.add(graph.get(start));
        curNode = openNodes.get(0);
        while(!openNodes.isEmpty())
        {
            openNodes.sort(null);
            curNode = openNodes.get(0);
            //System.out.print("Rozmiar: " + Boolean.toString(curNode.isStart));
            if(curNode.isEnd) break;
            succesors.clear();
            for(int i = -20;i<=20;i+=20)
            {
                for(int j = -20;j<=20;j+=20)
                {
                    if((i!=0 || j!=0) && stojable(curNode.pos.x + i, curNode.pos.y + j))
                        succesors.add(graph.get(new Pozycja(curNode.pos.x + i, curNode.pos.y + j)));
                }
            }
            System.out.print(" " + Integer.toString(succesors.size()));
            for(int i = 0; i<succesors.size();i++)
            {
                Node succesor = succesors.get(i);
                double currentSuccesorCost =  curNode.gCost + travelCost(curNode.pos, succesor.pos);
                if(openNodes.contains(succesor))
                {
                    if(succesor.gCost <= currentSuccesorCost) continue;
                }
                else if(closedNodes.contains(succesor))
                {
                    if(succesor.gCost <= currentSuccesorCost) continue;
                    closedNodes.remove(closedNodes.indexOf(succesor));
                    openNodes.add(succesor);
                }
                else
                {
                    openNodes.add(succesor);
                    succesor.setHCost(end);
                }
                succesor.setGcost(currentSuccesorCost);
                succesor.setParent(curNode.pos);
            }
            closedNodes.add(curNode);
            openNodes.remove(openNodes.indexOf(curNode));
        }
        Node tempNode = curNode;
        droga.add(curNode);
        while(tempNode.pos.x != start.x || tempNode.pos.y != start.y)
        {
            tempNode = graph.get(tempNode.parent);
            droga.add(tempNode);
        }
        //Collections.reverse(droga);

        if(curNode.pos != end)
        {
            System.out.print("pszypau");
        }
    }
}