#include <iostream>
#include <fstream>
#include <string>
#include "InputMap.h"

using namespace std;

#define MAXNAMELENGTH 30

// You can change this to use dynamic memory, if you like.
float gridMap[GRID_ROWS][GRID_COLS];
int height;
int width;
int maxVal;


/*************************************************************************
 *                                                                        *
 * Function inputMap converts the input map (in pnm format) into an       *
 * initial occupancy grid, which is stored in gridMap.                    *
 *                                                                        *
 * (Here, the "output" parameter says whether to print out the scaled     *
 *  map; output = 1 ==> yes, print;  output = 0 ==> no, don't print.)     *
 *                                                                        *
 * The input file name is passed as the second parameter.                 *
 * The created output file name is the same name, preceded by "scaled-".  *
 *                                                                        *
 * This function will only handle PNM files of type "P1", "P5", and "P6". *
 * (To see which you have, check the first line of your .pnm file).       *
 * If you have a "P4" file type, then use "pnmtoplainpnm to convert to    *
 * type "P1"  (i.e., linux>pnmtoplainpnm orig.pnm > orig-plain.pnm)       *
 *                                                                        *
 *************************************************************************/

void inputMap_v2(int output, char * fileName) 
{
    int i, j, m, n;
    char nextChar;
    string fileFormat;
    char outFileName[MAXNAMELENGTH];

    ifstream inFile(fileName);

    //ifstream inFile("hospital_section.pnm");
    //ifstream inFile("autolab-plain.pnm");

    /* Initialize map to 0's, meaning all free space */

    for (m=0; m<GRID_ROWS; m++)
        for (n=0; n<GRID_COLS; n++)   
	  gridMap[m][n] = 0.0;

    /* Get fileFormat */
    inFile >> fileFormat;

    cout << endl << "Input pnm file format = " << fileFormat << endl << endl;

    if (fileFormat == "P4")
      { cerr << "This program cannot handle pnm format type P4."  << endl
	     << "However, you can convert this file to a P1 file, which this program CAN handle." 
	     << endl 
	     << "To do this, run pnmtoplainpnm, then re-run this program with the resulting file." 
	     << endl << endl;
        exit(0);
      }

    if ((fileFormat != "P1") && (fileFormat != "P5") && (fileFormat != "P6"))
      { cerr << 
		  "This program is only set up to handle files of type P1, P5, and " 
			  << "P6." << endl;
        exit(0);
      }

    /* Get width, height */
    inFile >> width >> height;
    cout << "Width = " << width << ", Height = " << height << endl;

    if (fileFormat == "P5" || fileFormat == "P6")
      /* Read in maxVal */
      inFile >> maxVal;

    /* Read in map; */

    if (fileFormat == "P1") {
      for (i=0; i<height; i++)    
	for (j=0; j<width; j++) {
	  inFile >> nextChar;
	  if (nextChar == '1') 
	    gridMap[i/SCALE_MAP][j/SCALE_MAP] = 1.0;
	}
    }

    if (fileFormat == "P5") {
      for (i=0; i<height; i++)    
	for (j=0; j<width; j++) {
	  inFile >> nextChar;
	  if (!nextChar) 
	    gridMap[i/SCALE_MAP][j/SCALE_MAP] = 1.0;
	}
    }

	if( fileFormat == "P6") {
	    int val = 0;
		for(i = 0; i < height; i++)
		    for(j = 0; j < width; j++) {
		        for(int c = 0; c < 3; c++) {
				    inFile >> nextChar;
				    val += (int)nextChar; 
				}
                if(!val) {
				   gridMap[i/SCALE_MAP][j/SCALE_MAP] = 1.0;
				}
				else
					val = 0;
			}	
	}

    cout << "Map input complete.\n";

    if (output)  {
      strcpy(outFileName, "scaled-");
      strcat(outFileName, fileName);

	  if(fileFormat == "P6")
		  fileFormat = "P5";

      ofstream outFile(outFileName);
      outFile << fileFormat << endl;
      outFile << width/SCALE_MAP << " " << height/SCALE_MAP << endl;

      if (fileFormat == "P1") {
	for (i=0; i<height/SCALE_MAP; i++) {
	  for (j=0; j<width/SCALE_MAP; j++) {
	      if (gridMap[i][j] == 1.0)
	      outFile <<  1;
	    else
	      outFile <<  0;
	  }
	  outFile << endl;
	}
      }

      if (fileFormat == "P5") {
	outFile << maxVal << endl;
	for (i=0; i<height/SCALE_MAP; i++) {
	  for (j=0; j<width/SCALE_MAP; j++) {
	    if (gridMap[i][j] == 1.0)
	      outFile << (char) 0;
	    else
	      outFile << (char) -1;
	  }
	}
      }

       cout << "Scaled map output to file.\n";
    }
}