/*!
  \file
  \brief マップ画像からのデータ抽出

  \author Satofumi KAMIMURA

  $Id: map_parser.cpp 1568 2009-12-06 13:49:56Z satofumi $
*/

#include "grid_width.h"
#include "Point.h"
#include "PointUtils.h"
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <fstream>
#include <iostream>
#include <vector>

using namespace qrk;
using namespace std;


namespace
{
  void pixelColor(unsigned char* b, unsigned char* g, unsigned char* r,
                  IplImage* image, int x, int y)
  {
    int width_step = image->widthStep;
    int index = width_step * y + x * 3;

    *b = image->imageData[index + 0];
    *g = image->imageData[index + 1];
    *r = image->imageData[index + 2];
  }


  Point<long> nextPoint(bool& path_point,
                        const Point<long>& current,
                        const Point<long>& previous,
                        const Point<long>& prepre,
                        IplImage* image)
  {
    path_point = false;
    Point<long> detected_point(current);
    bool detected = false;
    long detected_length;

    for (int y = current.y - 1; y <= current.y + 1; ++y) {
      for (int x = current.x - 1; x <= current.x + 1; ++x) {

        if (((x == prepre.x) && (y == prepre.y)) ||
            ((x == previous.x) && (y == previous.y)) ||
            ((x == current.x) && (y == current.y))) {
          continue;
        }
        unsigned char b, g, r;
        pixelColor(&b, &g, &r, image, x, y);

        if ((r == 0xff) && (g == 0xff) && (b == 0x00)) {
          // 黄色。経路の中点
          path_point = true;
          return Point<long>(x, y);

        } else if ((r == 0xff) || (g == 0xff) || (b == 0xff)) {
          // 黒以外の 0xff を含む色
          int current_length = length<long>(current, Point<long>(x, y));
          if ((! detected) || (current_length < detected_length)) {
            detected = true;
            detected_point = Point<long>(x, y);
            detected_length = current_length;
          }
        }
      }
    }
    return detected_point;
  }


  void createPath(vector<Point<long> >& path,
                  unsigned char* b, unsigned char* g, unsigned char* r,
                  IplImage* image, int x, int y)
  {
    Point<long> current(x, y);
    Point<long> previous = current;
    Point<long> prepre = current;

    while (true) {
      bool path_point = false;
      Point<long> point =
        nextPoint(path_point, current, previous, prepre, image);

      if (path_point) {
        path.push_back(point);
      }

      if ((point.x == current.x) && (point.y == current.y)) {
        path.push_back(point);
        break;
      }

      prepre = previous;
      previous = current;
      current = point;
    }

    // 線の色を返す
    pixelColor(b, g, r, image, current.x, current.y);
  }


  void saveRunPath(const vector<Point<long> >& run_path,
                   const Point<long>& start)
  {
    ofstream fout("run_path.txt");
    if (! fout.is_open()) {
      perror("run_path.txt");
      exit(1);
    }

    for (vector<Point<long> >::const_iterator it = run_path.begin();
         it != run_path.end(); ++it) {
      long x = (it->x - start.x) * GridWidth;
      long y = -(it->y - start.y) * GridWidth;

      fout << x << '\t' << y << endl;
    }
  }


  void saveLines(const vector<vector<Point<long> > >& lines,
                 const Point<long>& start)
  {
    ofstream fout("lines.txt");
    if (! fout.is_open()) {
      perror("lines.txt");
      exit(1);
    }

    for (vector<vector<Point<long> > >::const_iterator line_it = lines.begin();
         line_it != lines.end(); ++line_it) {
      vector<Point<long> >::const_iterator first_it = line_it->begin();
      for (vector<Point<long> >::const_iterator last_it = line_it->begin() + 1;
           last_it != line_it->end(); ++last_it) {

        long x0 = (first_it->x - start.x) * GridWidth;
        long y0 = -(first_it->y - start.y) * GridWidth;
        long x1 = (last_it->x - start.x) * GridWidth;
        long y1 = -(last_it->y - start.y) * GridWidth;
        fout << x0 << '\t' << y0 << '\t' << x1 << '\t' << y1 << endl;

        first_it = last_it;
      }
    }
  }


  void savePoles(const vector<Point<long> >& poles,
                  const Point<long>& start)
  {
    ofstream fout("poles.txt");
    if (! fout.is_open()) {
      perror("poles.txt");
      exit(1);
    }

    for (vector<Point<long> >::const_iterator it = poles.begin();
         it != poles.end(); ++it) {
      long x = (it->x - start.x) * GridWidth;
      long y = -(it->y - start.y) * GridWidth;

      fout << x << '\t' << y << endl;
    }
  }


  void parseMapFile(IplImage* image)
  {
    int width = image->width;
    int height = image->height;

    vector<Point<long> > run_path;
    vector<vector<Point<long> > > lines;
    vector<Point<long> > poles;

    for (int y = 0; y < height; ++y) {
      for (int x = 0; x < width; ++x) {

        unsigned char b, g, r;
        pixelColor(&b, &g, &r, image, x, y);

        // 紫を始点として扱う
        if ((b == 0xff) && (g == 0x00) && (r == 0xff)) {
          vector<Point<long> > path;
          path.push_back(Point<long>(x, y));
          createPath(path, &b, &g, &r, image, x, y);

          // 色で出力情報を分ける
          if ((b == 0xff) && (g == 0xff) && (r == 0xff)) {
            // 白色は経路として扱う
            run_path = path;

          } else if ((b == 0x00) && (g == 0xff) && (r == 0x00)) {
            // 緑は壁として扱う
            lines.push_back(path);
          }
        } else if ((b == 0xff) && (g == 0x00) && (r == 0x00)) {
          // 青色は、柱の位置として扱う
          poles.push_back(Point<long>(x, y));
        }
      }
    }

    // マップ情報の出力
    if (run_path.empty()) {
      cout << "running path is not detected." << endl;
      return;
    }

    Point<long> start(run_path.front());
    saveRunPath(run_path, start);
    saveLines(lines, start);
    savePoles(poles, start);
  }
}


int main(int argc, char *argv[])
{
  const char* map_file = "trial_2009.png";
  for (int i = 1; i < argc; ++i) {
    map_file = argv[i];
  }

  IplImage* image = cvLoadImage(map_file, CV_LOAD_IMAGE_COLOR);
  if (! image) {
    cout << "cvLoadImage: " << map_file << endl;
    return 1;
  }

  // マップから走行用のファイルを出力
  parseMapFile(image);

  cvReleaseImage(&image);

  return 0;
}
