/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/

#include <base_local_planner/point_grid.h>
#include <ros/console.h>
#ifdef HAVE_SYS_TIME_H
#include <sys/time.h>
#endif

#include <math.h>
#include <cstdio>
#include <sensor_msgs/point_cloud2_iterator.h>

using namespace std;
using namespace costmap_2d;

void printPoint(const geometry_msgs::Point& pt){
  printf("(%.2f, %.2f, %.2f)", pt.x, pt.y, pt.z);
}

void printPSHeader(){
  printf("%%!PS\n");
  printf("%%%%Creator: Eitan Marder-Eppstein (Willow Garage)\n");
  printf("%%%%EndComments\n");
}

void printPSFooter(){
  printf("showpage\n%%%%EOF\n");
}

void printPolygonPS(const std::vector<geometry_msgs::Point32>& poly, double line_width){
  if(poly.size() < 2)
    return;

  printf("%.2f setlinewidth\n", line_width);
  printf("newpath\n");
  printf("%.4f\t%.4f\tmoveto\n", poly[0].x * 10, poly[0].y * 10);
  for(unsigned int i = 1; i < poly.size(); ++i)
    printf("%.4f\t%.4f\tlineto\n", poly[i].x * 10, poly[i].y * 10);
  printf("%.4f\t%.4f\tlineto\n", poly[0].x * 10, poly[0].y * 10);
  printf("closepath stroke\n");

}

using namespace base_local_planner;

int main(int argc, char** argv){
  geometry_msgs::Point origin;
  origin.x = 0.0;
  origin.y = 0.0;
  PointGrid pg(50.0, 50.0, 0.2, origin, 2.0, 3.0, 0.0);
  /*
     double x = 10.0;
     double y = 10.0;
     for(int i = 0; i < 100; ++i){
     for(int j = 0; j < 100; ++j){
     pcl::PointXYZ pt;
     pt.x = x;
     pt.y = y;
     pt.z = 1.0;
     pg.insert(pt);
     x += .03;
     }
     y += .03;
     x = 10.0;
     }
     */
  std::vector<geometry_msgs::Point> footprint, footprint2, footprint3;
  geometry_msgs::Point pt;

  pt.x = 1.0;
  pt.y = 1.0;
  footprint.push_back(pt);

  pt.x = 1.0;
  pt.y = 1.65;
  footprint.push_back(pt);

  pt.x = 1.325;
  pt.y = 1.75;
  footprint.push_back(pt);

  pt.x = 1.65;
  pt.y = 1.65;
  footprint.push_back(pt);

  pt.x = 1.65;
  pt.y = 1.0;
  footprint.push_back(pt);

  pt.x = 1.325;
  pt.y = 1.00;
  footprint2.push_back(pt);

  pt.x = 1.325;
  pt.y = 1.75;
  footprint2.push_back(pt);

  pt.x = 1.65;
  pt.y = 1.75;
  footprint2.push_back(pt);

  pt.x = 1.65;
  pt.y = 1.00;
  footprint2.push_back(pt);

  pt.x = 0.99;
  pt.y = 0.99;
  footprint3.push_back(pt);

  pt.x = 0.99;
  pt.y = 1.66;
  footprint3.push_back(pt);

  pt.x = 1.3255;
  pt.y = 1.85;
  footprint3.push_back(pt);

  pt.x = 1.66;
  pt.y = 1.66;
  footprint3.push_back(pt);

  pt.x = 1.66;
  pt.y = 0.99;
  footprint3.push_back(pt);

  pt.x = 1.325;
  pt.y = 1.325;

  geometry_msgs::Point32 point;
  point.x = 1.2;
  point.y = 1.2;
  point.z = 1.0;

#ifdef HAVE_SYS_TIME_H
  struct timeval start, end;
  double start_t, end_t, t_diff;
#endif

  printPSHeader();

#ifdef HAVE_SYS_TIME_H
  gettimeofday(&start, NULL);
#endif

  for(unsigned int i = 0; i < 2000; ++i){
    pg.insert(point);
  }

#ifdef HAVE_SYS_TIME_H
  gettimeofday(&end, NULL);
  start_t = start.tv_sec + double(start.tv_usec) / 1e6;
  end_t = end.tv_sec + double(end.tv_usec) / 1e6;
  t_diff = end_t - start_t;
  printf("%%Insertion Time: %.9f \n", t_diff);
#endif

  vector<Observation> obs;
  vector<PlanarLaserScan> scan;

#ifdef HAVE_SYS_TIME_H
  gettimeofday(&start, NULL);
#endif
  pg.updateWorld(footprint, obs, scan);

  double legal = pg.footprintCost(pt, footprint, 0.0, .95);
  pg.updateWorld(footprint, obs, scan);
  double legal2 = pg.footprintCost(pt, footprint, 0.0, .95);

#ifdef HAVE_SYS_TIME_H
  gettimeofday(&end, NULL);
  start_t = start.tv_sec + double(start.tv_usec) / 1e6;
  end_t = end.tv_sec + double(end.tv_usec) / 1e6;
  t_diff = end_t - start_t;

  printf("%%Footprint calc: %.9f \n", t_diff);
#endif

  if(legal >= 0.0)
    printf("%%Legal footprint %.4f, %.4f\n", legal, legal2);
  else
    printf("%%Illegal footprint\n");

  printPSFooter();

  return 0;
}