/*
 * map_saver
 * 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 <ORGANIZATION> 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.
 */

#include "map_saver.h"

#include <sys/types.h>
#include <sys/stat.h>

#include <tf/tf.h>

using namespace std;
 

    MapGenerator::MapGenerator(const std::string mapname)
    {
        m_Mapname = mapname;
    }

    void MapGenerator::saveMapLayer(const nav_msgs::OccupancyGridConstPtr &map, std::string fileName)
    {
        ROS_INFO("Writing map occupancy data to %s", fileName.c_str());
        FILE* out = fopen(fileName.c_str(), "w");
        if (!out)
        {
          ROS_ERROR("Couldn't save map file to %s", fileName.c_str());
          return;
        }

        fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
                map->info.resolution, map->info.width, map->info.height);
        for(unsigned int y = 0; y < map->info.height; y++) {
          for(unsigned int x = 0; x < map->info.width; x++) {
            unsigned int i = x + (map->info.height - y - 1) * map->info.width;
            if (map->data[i] == -1) {
                fputc(205, out);
            } else if (map->data[i] < 20) { //occ [0,0.2)
                fputc(254, out);
            } else if (map->data[i] > 65) { //occ (0.65,1]
                fputc(000, out);
            } else { //occ [0.2,0.65]
                fputc(205, out);
            }
          }
        }
        fclose(out);
    }

    void MapGenerator::save(const nav_msgs::OccupancyGridConstPtr& SLAMMap,
                            const nav_msgs::OccupancyGridConstPtr& maskingMap,
                            std::vector<homer_mapnav_msgs::PointOfInterest> poiList)
    {
      uint a = m_Mapname.rfind('/');
      std::string filename = m_Mapname.substr(a+1, m_Mapname.size()-1);
      int status;
      status = mkdir(m_Mapname.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
      std::string SLAMMapdatafile = filename + "_SLAM.pgm";
      std::string maskingMapdatafile = "";
      saveMapLayer(SLAMMap, m_Mapname + "/" + SLAMMapdatafile);
      if(maskingMap != NULL)
      {
        maskingMapdatafile = filename + "_mask.pgm";
        saveMapLayer(maskingMap, m_Mapname + "/" + maskingMapdatafile);
      }
      std::string mapmetadatafile = m_Mapname + "/" + filename + ".yaml";
      ROS_INFO("Writing map metadata to %s", mapmetadatafile.c_str());
      FILE* yaml = fopen(mapmetadatafile.c_str(), "w");


      /*
resolution: 0.100000
origin: [0.000000, 0.000000, 0.000000]
#
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

       */

      geometry_msgs::Quaternion orientation = SLAMMap->info.origin.orientation;
      //btMatrix3x3 mat(btQuaternion(orientation.x, orientation.y, orientation.z, orientation.w));
      tf::Quaternion quat_tf;
      tf::quaternionMsgToTF(orientation, quat_tf);
      double yaw = tf::getYaw(quat_tf);
      //mat.getEulerYPR(yaw, pitch, roll);

      stringstream pois;
      if(!poiList.empty())
      {
        pois << "pois:\n";
        std::vector< homer_mapnav_msgs::PointOfInterest >::iterator it;
        for ( it = poiList.begin(); it != poiList.end(); it++ ) {
            pois << " - name: " << it->name << "\n";
            pois << "   type: " << it->type << "\n";
            pois << "   x: " << it->pose.position.x << "\n";
            pois << "   y: " << it->pose.position.y << "\n";
            pois << "   theta: " << tf::getYaw(it->pose.orientation) << "\n";
            pois << "   remarks: " << it->remarks << "\n";
        }
      }
      string poiStr = pois.str();

      string maskImage = "";
      if(maskingMapdatafile != "")
        maskImage = "\nmask_image: ";
      fprintf(yaml, "image: %s%s%s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.195\n\n%s",
              SLAMMapdatafile.c_str(), maskImage.c_str(), maskingMapdatafile.c_str(), SLAMMap->info.resolution, SLAMMap->info.origin.position.x, SLAMMap->info.origin.position.y, yaw, poiStr.c_str());
      fclose(yaml);

      ROS_INFO("Done\n");
    }