content: /**
  *
  * CoreSLAM.java abstract Java class for CoreSLAM algorithm in BreezySLAM
  *
  * Copyright (C) 2014 Simon D. Levy
  
  * This code is free software: you can redistribute it and/or modify
  * it under the terms of the GNU Lesser General Public License as 
  * published by the Free Software Foundation, either version 3 of the 
  * License, or (at your option) any later version.
  * 
  * This code is distributed in the hope that it will be useful,     
  * but WITHOUT ANY WARRANTY without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  * 
  * You should have received a copy of the GNU Lesser General Public License 
  * along with this code.  If not, see <http://www.gnu.org/licenses/>.
  */
  
  
  package edu.wlu.cs.levy.breezyslam.algorithms;
  
  import edu.wlu.cs.levy.breezyslam.components.Laser;
  import edu.wlu.cs.levy.breezyslam.components.Velocities;
  import edu.wlu.cs.levy.breezyslam.components.Map;
  import edu.wlu.cs.levy.breezyslam.components.Scan;
  
  /**
  *    CoreSLAM is an abstract class that uses the classes Position, Map, Scan, and Laser
  *    to run variants of the simple CoreSLAM (tinySLAM) algorithm described in 
  *    <pre>
  *     @inproceedings{coreslam-2010,
  *       author    = {Bruno Steux and Oussama El Hamzaoui}, 
  *       title     = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code},  
  *       booktitle = {11th International Conference on Control, Automation,   
  *                    Robotics and Vision, ICARCV 2010, Singapore, 7-10  
  *                    December 2010, Proceedings},  
  *       pages     = {1975-1979},  
  *       publisher = {IEEE},  
  *       year      = {2010}
  *     }
  *     </pre>
  *    Implementing classes should provide the method
  *
  *      void updateMapAndPointcloud(int * scan_mm, Velocities & velocities)
  *
  *    to update the map and point-cloud (particle cloud).
  *
  */
  public abstract class CoreSLAM {
  
      /**
      * The quality of the map (0 through 255)
      */
      public int map_quality = 50; 
  
      /**
      * The width in millimeters of each "hole" in the map (essentially, wall width)
      */
      public double hole_width_mm = 600;
  
      protected Laser laser;
  
      protected Velocities velocities;
  
      protected Map map;
  
      protected Scan scan_for_mapbuild;
      protected Scan scan_for_distance;
  
      public CoreSLAM(Laser laser, int map_size_pixels, double map_size_meters)
      {
          // Set default params
          this.laser = new Laser(laser);
          
          // Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry
          this.velocities = new Velocities();
  
          // Initialize a scan for computing distance to map, and one for updating map
          this.scan_for_mapbuild = this.scan_create(3);
          this.scan_for_distance = this.scan_create(1);
          
          // Initialize the map 
          this.map = new Map(map_size_pixels, map_size_meters);
      }
  
      private Scan scan_create(int span)
      {
          return new Scan(this.laser, span);
      }
  
      private void scan_update(Scan scan, int [] scan_mm)
      {
          scan.update(scan_mm, this.hole_width_mm, this.velocities);
      }
  
  
      public void update(int [] scan_mm, Velocities velocities)
      {             
          // Build a scan for computing distance to map, and one for updating map
          this.scan_update(this.scan_for_mapbuild, scan_mm);
          this.scan_update(this.scan_for_distance, scan_mm);
          
          // Update velocities
          this.velocities.update(velocities.getDxyMm(), velocities.getDthetaDegrees(),  velocities.getDtSeconds());
                                   
          // Implementing class updates map and pointcloud
          this.updateMapAndPointcloud(velocities);
      }   
  
      /**
      * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities
      * (no odometry).
      * @param scan_mm Lidar scan values, whose count is specified in the <tt>scan_size</tt> 
      * attribute of the Laser object passed to the CoreSlam constructor
      */
      public void update(int [] scan_mm)
      {
          Velocities zero_velocities = new Velocities();
  
          this.update(scan_mm, zero_velocities);
      }
  
  
      protected abstract void updateMapAndPointcloud(Velocities velocities);
  
      public void getmap(byte [] mapbytes)
      {
          this.map.get(mapbytes);
      }
  
  }
  