import java.io.*;
import javax.swing.*;
import java.awt.*;
import java.util.ArrayList;

/**
 * A graphical way to view the map and draw/view the particles.
 * @author Matt Hoffman
 * @version 2.0
 ***/
public class GridMap extends JPanel
{
    private static PrintStream err   = System.err;
    private static PrintStream debug = System.err;
    
    public int realRobotX, realRobotY;
    public float realRobotR;
    public boolean hasRealData;

    public int robotX, robotY;
    public int[] robotS;
    public float robotR;
    public int mMinX, mMinY;
    public int mMaxX, mMaxY;

    private double mResolution;
    private int mxSize, mySize;
    private boolean[] mOccupied;

    // ADDED FOR SECOND PART OF PROJECT
    sensorLookupTable lookupTable;
    // END ADDED FOR SECOND PART OF PROJECT

    private class Particle 
    {
        public Particle(int x, int y) { X=x; Y=y; }
        int X = 0;
        int Y = 0;
    }
    private ArrayList<Particle> mParticles;

    /**
     * Construct the grid map given a map-file (which will be
     * provided). 
     * @param file The file specifying the map.
     ***/
    public GridMap(String file)
    { 
        mParticles = new ArrayList<Particle>();

        try {
            InputStream input = new FileInputStream(file);
            handleVers2(input);
            setPreferredSize(new Dimension(mxSize, mySize));

	        // ADDED FOR SECOND PART OF PROJECT
	        initSensorLookupTable();
	        // END ADDED FOR SECOND PART OF PROJECT
        }
        catch (Exception e) {
            err.println("" + e);
            System.exit(-1);
        }
    }

    // ---------------------------------------------------------------
    //  Handle version 2 stuff
    // ---------------------------------------------------------------
    private void handleVers2(InputStream input)
        throws IOException
    {
        mResolution = 100.0;

        // find the map's bounding box
        mMinX = readInteger(input);
        mMinY = readInteger(input);
        mMaxX = mMinX + (int)(readInteger(input) * mResolution + .5);
        mMaxY = mMinY + (int)(readInteger(input) * mResolution + .5);

        // find the width/height
        mxSize = (int) ((mMaxX-mMinX)/ mResolution + .5);
        mySize = (int) ((mMaxY-mMinY)/ mResolution + .5);

        if ((mxSize & 0x7) != 0) // multiple of 8
            mxSize = 0x8 + (mxSize & ~0x7); 

        if ((mySize & 0x1) != 0) // multiple of 2
            mySize = 0x1 + (mySize & ~0x1); 

        mOccupied = new boolean[mxSize * mySize];
        for (int y=0; y < mySize; ++y)
            for (int x=0; x < mxSize; ++x)
            {
                boolean occupied = !(input.read()==0);
                mOccupied[y*mxSize+x] = occupied;
            }
    }

    private static int readInteger(InputStream input)
        throws IOException
    {
        byte[] integer = new byte[4];
        input.read(integer);
        return byteArrayToInt(integer);
    }

    // NOTE: java is big-endian no matter what. I hate this language
    // (although I guess this's not necessarily a bad thing).
    private static int byteArrayToInt(byte[] a)  
    {
        return ( ( (int)a[3] << 24 ) & 0xff000000 ) |
               ( ( (int)a[2] << 16 ) & 0x00ff0000 ) |
               ( ( (int)a[1] << 8  ) & 0x0000ff00 ) |
               (   (int)a[0]         & 0x000000ff );
    }

    // ---------------------------------------------------------------
    public int getX (double x) {
        return (int) ((x - mMinX) / mResolution + .5);
    }

    public int getY (double y) {
        return mySize - (int) ((y - mMinY) / mResolution + .5);
    }

    public void paintComponent(Graphics g)
    {
        super.paintComponent(g);

        for (int y=0; y < mySize; y++)
            for (int x=0; x < mxSize; x++)
            {
                if (mOccupied[y*mxSize+x]) g.setColor(Color.gray);
                else g.setColor(Color.white);

                // draw the pixel
                g.fillRect(x, (int)(mySize) - y, 1, 1);
            }

        g.setColor(Color.green);
        for (int i=0; i < mParticles.size(); ++i)
        {
            Particle p = mParticles.get(i);
            int x = getX(p.X);
            int y = getY(p.Y);
            int size = 3;

            g.drawOval(x, y, size, size);
            g.fillOval(x, y, size, size);
        }

        int size = 4;
        int x = getX(robotX);
        int y = getY(robotY);

        g.setColor(Color.red);
        g.drawOval(x, y, size, size);
        g.fillOval(x, y, size, size);

        if (hasRealData)
        {
            x = getX(realRobotX);
            y = getY(realRobotY);
            g.setColor(Color.blue);
            g.drawOval(x, y, size, size);
            g.fillOval(x, y, size, size);

            for (int i=0; i < robotS.length; ++i)
            {
                float angle = realRobotR + 
                    (float)(i*Math.PI/(Status.NUM_SCANS-1) - Math.PI/2);
                int x1 = getX(realRobotX);
                int y1 = getY(realRobotY);
                int x2 = getX(realRobotX + Math.cos(angle)*expectedSensorReading(realRobotX,realRobotY,angle));
                int y2 = getY(realRobotY + Math.sin(angle)*expectedSensorReading(realRobotX,realRobotY,angle));
                g.drawLine(x1,y1,x2,y2);
            }
        }
    }

    /** Whether or not a position is occupied. */
    public boolean occupied(int X, int Y)
    {
        int x = (int)((X - mMinX) / mResolution);
        int y = (int)((Y - mMinY) / mResolution);

        if (y>=0 && x>=0 && y<mySize && x<mxSize)
            return mOccupied[y*mxSize+x];
        else
            return true;
    }

    /** 
     * Draw a point for some particle at the specific position.
     * @param x The x position.
     * @param y The y position.
     ***/
    public void addParticle(int x, int y) 
    { 
        Particle p = new Particle(x,y);
        mParticles.add(p);
    }

    /** Clear the set of particles that are drawn. */
    public void clearParticles() { mParticles.clear(); }

    //************************************************//
    // BEGIN ADDITIONS FOR PART II                    //
    //************************************************//

    private void initSensorLookupTable() {
	lookupTable = new sensorLookupTable(this);
    }

    public double expectedSensorReading(int globalXPos, 
				     int globalYPos,
				     double angleInRadians) {
	return lookupTable.distanceOfFirstHit(globalXPos, globalYPos, angleInRadians);
    }


    /* SensorLookupTable
     *******************
     * Each grid cell has one of these, whose values are precomputed.
     * 
     */
    private class sensorLookupTable {

	public int numAngles;
	public double angleThinFactor = 5.0; //use every nth angle
	public int maxDistance = 5000; // sensors can see this many meters.
	public double distanceResolution;
	// distance to nearest object, organized by [x][y][angle] 
	// [0][0] is bottom-left.
	public char[][][] nearestDistances; 
	public double angleStep;
	public double mapResolution;
	public GridMap map;

	public sensorLookupTable(GridMap map) {

	    this.map = map;
	    numAngles = (int)(360.0/angleThinFactor);
	    mapResolution = map.mResolution;
	    this.distanceResolution = 20;
	    double numSteps = (int)Math.ceil(maxDistance / distanceResolution);
	    angleStep = 360.0 / numAngles;
	    nearestDistances = new char[map.mxSize][map.mySize][numAngles];

   
	    for(int cellX = 0; cellX<map.mxSize; cellX++) {
		
		//System.out.println("x: "+cellX+" of "+map.mxSize);
		for(int cellY =0; cellY<map.mySize; cellY++) {
		    
		    // if this cell is occupied, then we just 
		    // leave all of its entries blank
		    int globalX = (int)(((cellX+.5)*mapResolution)+map.mMinX);
		    int globalY = (int)(((cellY+.5)*mapResolution)+map.mMinY);
		    if (map.occupied(globalX, globalY)) {
			continue;
		    } 


		    for(double z=0; z<360; z += angleStep) {
			
			int angleIndex = angleToIndex(z);
			double xStep = Math.cos(Math.toRadians(z)) * distanceResolution;
			double yStep = Math.sin(Math.toRadians(z)) * distanceResolution;
			
			
			// This part stores the indices of the map cells that 
			// are hit by the sensor beam
			int prevX = 0;
			int prevY = 0;
			boolean foundObject = false;

			
			for(int step = 0; step < numSteps; step++) {

			    int nextX = (int) ((.5 + step * xStep) / mapResolution);
			    int nextY = (int) ((.5 + step * yStep) / mapResolution);
			    
			    // check to see whether this new distance from our
			    // starting cell falls into a new map cell
			    if ( (nextX != prevX) ||
				 (nextY != prevY)) {

				prevX = nextX;
				prevY = nextY;

 				
				// convert to global coordinates
				int mapX = (int)(((nextX + cellX)*mapResolution)+map.mMinX);
				int mapY = (int)(((nextY + cellY)*mapResolution)+map.mMinY);
				
				// if the cell is in bounds, see if it's occupied
				// if so, store the distance and stop looking
				// along this beam
				if (map.occupied(mapX, mapY)) {
				    nearestDistances[cellX][cellY][angleIndex] = (char)step;
				    foundObject = true;
				    break; 
				} 
			    }
			}
			// if we've checked out to the max distance without
			// hitting an object, set the distance to -1.
			if (!foundObject) {
			    nearestDistances[cellX][cellY][angleIndex] = (char)(maxDistance/distanceResolution);//-1;
			} 


		    } // end loop over z (angle)
		} // end loop over y (cell)
	    } // end loop over x (cell)

	} // end ctor

	private int angleToIndex(double angleInDegrees) {

	    while(angleInDegrees >= 360.0) angleInDegrees -= 360.0;
	    while(angleInDegrees < 0.0) angleInDegrees += 360.0;
	    int index = (int)Math.floor(angleInDegrees / angleStep);
	    return index;
	}

	public double distanceOfFirstHit(int globalXCoord,
					 int globalYCoord,
					 double angleInRadians) {

	    // convert the global coords to relative cells
	    int cellX =   (int)((globalXCoord - map.mMinX) / mapResolution);
	    int cellY = (int)((globalYCoord - map.mMinY) / mapResolution);

	    // convert the angle to the appropriate angle lookup index
	    double angleInDegrees = Math.toDegrees(angleInRadians);

	    // DEBUGGING
	    //	    angleInDegrees += 180;

	    while(angleInDegrees >= 360.0) angleInDegrees -= 360.0;
	    while(angleInDegrees < 0.0) angleInDegrees += 360.0;
	    int lookupIndex = angleToIndex(angleInDegrees);
	    char charNumSteps = nearestDistances[cellX][cellY][lookupIndex];
	    double numSteps = (double)charNumSteps;
	    double dist = numSteps*distanceResolution;
	    return dist;
	}

    } // end sensorLookupTable class
}