/**
 * <p>
 * An n-dimensional box, the simplest possible boundary environment for robots.
 * 
 * </p>
 * @author     Michael Schuresko
 * @version    %I%, %G%
 * @since      1.0
*/

public class BoxBoundary implements IBoundary
{

    /**
     * Indicates that box boundaries were set with mismatched
     * dimensionality.
     * @since 1.0
     */
    public class DimensionMismatchException extends RuntimeException
    {
	int m_nDim1, m_nDim2;
	boolean m_bDimSpecified;

	// for serializable
	private static final long
	    serialVersionUID = 1491756914486144643L;
	
	public DimensionMismatchException() {
	    m_bDimSpecified = false;
	}

	/**
	 * Constructor 2 (preferred) -- which dimensions didn't match
	 */
	public DimensionMismatchException(int nDim1, int nDim2) {
	    m_nDim1 = nDim1; m_nDim2 = nDim2;
	}

	public int getDim1() { return m_nDim1; }
	public int getDim2() { return m_nDim2; }
	public boolean areDimValidQ() { return m_bDimSpecified; }
	
    }



    int m_nDim;
    double m_arrLfMax[];
    double m_arrLfMin[];



    /**
     * Constructor -- box dimensions specified elsewhere
     */
    public BoxBoundary()
    {
	m_nDim = 0;
	m_arrLfMax = null; m_arrLfMin = null;
    }

    /**
     * Constructor -- specifies dimensions and extents of box
     */
    public BoxBoundary(double arrLfMin[], double arrLfMax[])
    {
	setFromMinMax(arrLfMin, arrLfMax);
    }
    

    /**
     * gets bounding box min corner
     * @return min corner of bounding box
     */
    public double [] bboxMinCorner()
    {
	return m_arrLfMin;
    }

    /** 
     * gets bounding box max corner
     * @return max corner of bounding box
     */
    public double [] bboxMaxCorner()
    {
	return m_arrLfMax;
    }

    /**
     * Mostly just a safety check, since the environment should have
     * this anyway
     */
    public int getNumDimensions() { return m_nDim; }

    /**
     * Inside / outside query.
     */
    public boolean isPointInside(double arrPoint[]) 
    {
	boolean bResult = true;
	if(arrPoint.length < m_nDim) {
	    throw new DimensionMismatchException(m_nDim, 
						 arrPoint.length);
	}
	for(int i = 0; bResult && i < m_nDim; ++i) {
	    bResult = bResult &&
		(arrPoint[i] > m_arrLfMin[i]) &&
		(arrPoint[i] < m_arrLfMax[i]);
	}
	return bResult;
    }

    /**
     * Finds distance to boundary along a ray from a start point, 
     * returns negative number if outside boundary
     * @param arrRayOrigin origin of ray
     * @param arrRayDir direction of ray
     * @return distance along arrRayDir from arrRayOrigin to boundary, 
     *  negative if arrRayOrigin is outside boundary
     */
    public double distToBoundary(double arrRayOrigin[], double arrRayDir[])
    {
	if(m_nDim < 1) {
	    throw new DimensionMismatchException(0, arrRayOrigin.length);
	}
	double lfMinDist = 
	    Math.min(Math.abs(arrRayDir[0]*(m_arrLfMax[0]-arrRayOrigin[0])),
		     Math.abs(arrRayDir[0]*(m_arrLfMax[0]-arrRayOrigin[0])));
	for(int i = 1; i < m_nDim; ++i) {
	    double lfMinDistCurrAxis = 
		Math.min(Math.abs(arrRayDir[i]*
				  (m_arrLfMax[i]-arrRayOrigin[i])),
			 Math.abs(arrRayDir[i]*
				  (m_arrLfMax[i]-arrRayOrigin[i])));
	    lfMinDist = Math.min(lfMinDist, lfMinDistCurrAxis);
	}
	if( ! isPointInside(arrRayOrigin)) {
	    return -lfMinDist;
	}
	return lfMinDist;
    }

    /**
     * Finds the closest point on the boundary to a given point,
     * or a minimally close point if multiple such points exist.
     * @param arrPoint point to find closest point to
     * @return closest point to arrPoint
     */
    public double [] closestBoundaryPoint(double arrPoint[])
    {
	if(m_nDim < 1) {
	    throw new DimensionMismatchException(0, arrPoint.length);
	}
	int nMinDistIdx = 0;
	double lfMinDist = 
	    Math.min(Math.abs(m_arrLfMax[0]-arrPoint[0]),
		     Math.abs(m_arrLfMax[0]-arrPoint[0]));
	for(int i = 1; i < m_nDim; ++i) {
	    double lfMinDistCurrAxis = 
		Math.min(Math.abs(m_arrLfMax[i]-arrPoint[i]),
			 Math.abs(m_arrLfMax[i]-arrPoint[i]));
	    if(lfMinDistCurrAxis < lfMinDist) {
		nMinDistIdx = i;
		lfMinDist = lfMinDistCurrAxis;
	    }
	}
	double arrLfResult[] = new double[m_nDim];
	System.arraycopy(arrPoint, 0, arrLfResult, 0, m_nDim);
	if(Math.abs(m_arrLfMax[nMinDistIdx]-arrPoint[nMinDistIdx])
	   <
	   Math.abs(m_arrLfMin[nMinDistIdx]-arrPoint[nMinDistIdx])) 
	{
	    arrLfResult[nMinDistIdx] = m_arrLfMax[nMinDistIdx];
	}
	else 
	{
	    arrLfResult[nMinDistIdx] = m_arrLfMin[nMinDistIdx];
	}
	return arrLfResult;
    }


    // the following functions are not part of the interface
    /**
     * Sets box boundaries from min and max coordinates
     */
    public void setFromMinMax(double arrLfMin[], double arrLfMax[])
    {
	m_nDim = arrLfMin.length;
	if(arrLfMax.length != m_nDim) {
	    // some sort of error happens here
	    throw new DimensionMismatchException(arrLfMin.length, 
						 arrLfMax.length);
	}
	m_arrLfMax = new double[m_nDim];
	m_arrLfMin = new double[m_nDim];
	for(int i = 0; i < m_nDim; ++i) {
	    m_arrLfMin[i] = arrLfMin[i];
	    m_arrLfMax[i] = arrLfMax[i];
	}
    }

    /**
     * Sets box boundaries from center and box extents.
     * Box extents are from center to corner
     */
    public void setFromCenterAndExtents(double arrLfCenter[], 
					double arrLfExtents[])
    {
	m_nDim = arrLfCenter.length;
	if(arrLfExtents.length != m_nDim) {
	    // some sort of error happens here
	    throw new DimensionMismatchException(arrLfCenter.length, 
						 arrLfExtents.length);
	}
	m_arrLfMax = new double[m_nDim];
	m_arrLfMin = new double[m_nDim];
	for(int i = 0; i < m_nDim; ++i) {
	    m_arrLfMin[i] = arrLfCenter[i] - 
		Math.abs(arrLfExtents[i]);
	    m_arrLfMax[i] = arrLfCenter[i] + 
		Math.abs(arrLfExtents[i]);
	}
    }
}