package GlobalNavigation;

import java.util.*;
import java.awt.geom.*;

/**
 * <p>Simple configuration space.</p>
 *
 * <p>Each real obstacle is expanded to a CS obstacle by computing the convex
 * hull of the Minkowski sum of the real obstacle and a square circumscribed on
 * the robot bounding disc.</p>
 *
 * @author vona
 **/
public class CSpace {

  /**
   * <p>The CS obstacles.</p>
   **/
  protected LinkedList<PolygonObstacle> obstacles =
    new LinkedList<PolygonObstacle>();
 
  /**
   * <p>Compute a new CSpace.</p>
   *
   * @param realObstacles the set of real obstacles
   * @param robotRadius the robot disc radius
   **/
  public CSpace(List<PolygonObstacle> realObstacles, double robotRadius) {
    for (PolygonObstacle realObstacle : realObstacles)
      obstacles.add(makeCSObstacle(realObstacle, robotRadius));
  }

  /**
   * <p>Get {@link #obstacles}.</p>
   *
   * @return a reference to <code>obstacles</code>
   **/
  public List<PolygonObstacle> getObstacles() {
    return obstacles;
  }

  /**
   * <p>Make a CS obstacle.</p>
   *
   * @param realObstacle the corresp real obstacle
   * @param robotRadius the robot bounding disc radius (m)
   **/
  protected PolygonObstacle makeCSObstacle(PolygonObstacle realObstacle,
                                           double robotRadius) {

    List<Point2D.Double> csoPoints = new LinkedList<Point2D.Double>();

    List<Point2D.Double> roVertices = realObstacle.getVertices();

    for (Point2D.Double roVertex : roVertices) {
      csoPoints.add(new Point2D.Double(roVertex.x + robotRadius,
                                       roVertex.y + robotRadius));
      csoPoints.add(new Point2D.Double(roVertex.x - robotRadius,
                                       roVertex.y + robotRadius));
      csoPoints.add(new Point2D.Double(roVertex.x - robotRadius,
                                       roVertex.y - robotRadius));
      csoPoints.add(new Point2D.Double(roVertex.x + robotRadius,
                                       roVertex.y - robotRadius));
    }

    PolygonObstacle ret = GeomUtils.convexHull(csoPoints);
    ret.color = realObstacle.color;
    return ret;
  }
}