package com.graphhopper.util.shapes;

import java.security.InvalidParameterException;

import org.geotools.referencing.CRS;
import org.opengis.geometry.MismatchedDimensionException;
import org.opengis.referencing.FactoryException;
import org.opengis.referencing.crs.CoordinateReferenceSystem;
import org.opengis.referencing.operation.TransformException;

import uk.co.ordnancesurvey.api.srs.LatLong;
import uk.co.ordnancesurvey.api.srs.OpenCoordConverter;

import com.graphhopper.GHResponse;
import com.graphhopper.util.PointList;

public class GHResponseCoordinateTransformer
{

	private static final String BNG = "BNG";
	private CoordinateReferenceSystem targetCRS = OpenCoordConverter.wgs84CoordRefSystem;

	public GHResponseCoordinateTransformer( String crsCode ) throws InvalidParameterException
	{
		if(!crsCode.equalsIgnoreCase("WGS84"))
	        try
            {
	            targetCRS = crsCode.equalsIgnoreCase(BNG) ? OpenCoordConverter.bngCoordRefSystem : CRS
	                .decode(crsCode);
            } catch (FactoryException e)
            {
	           throw new IllegalArgumentException("Srs " + crsCode + " is not a valid srs for output.");
            }
	}

	public void transformCoordinates( GHResponse response )
	{
		if(targetCRS.equals(OpenCoordConverter.wgs84CoordRefSystem))
			return;
		
		PointList points = response.getPoints();
		PointList transformedPoints = new PointList(points.getSize(), points.is3D());
		try
		{
			GHPoint transformedPoint;
			for (GHPoint3D ghPoint3D : points)
			{
				LatLong transformedCoordinate = OpenCoordConverter
				        .transformFromSourceCRSToTargetCRS(OpenCoordConverter.wgs84CoordRefSystem,
				                targetCRS, ghPoint3D.getLat(), ghPoint3D.getLon(), true);
				if (points.is3D())
					transformedPoint = new GHPoint3D(transformedCoordinate.getLatAngle(),
					        transformedCoordinate.getLongAngle(), ghPoint3D.getElevation());
				else
					transformedPoint = new GHPoint(transformedCoordinate.getLatAngle(),
					        transformedCoordinate.getLongAngle());

				transformedPoints.add(transformedPoint);
			}

			response.setPoints(transformedPoints);
		} catch (FactoryException | MismatchedDimensionException | TransformException e)
		{
			e.printStackTrace();
		}

	}

}

@@ -1493,18 +1493,16 @@
 oint
-3D
  ghPoint
 3D :
@@ -1497,18 +1497,16 @@
  ghPoint
-3D
  : point
@@ -1689,26 +1689,24 @@
 CRS, ghPoint
-3D
 .getLat(), g
@@ -1707,26 +1707,24 @@
 t(), ghPoint
-3D
 .getLon(), t
@@ -1751,16 +1751,68 @@
 .is3D())
+ %7B%0A%09%09%09%09%09GHPoint3D threedPoint = (GHPoint3D) ghPoint;
 %0A%09%09%09%09%09tr
@@ -1934,17 +1934,19 @@
 (), 
-gh
+threed
 Point
-3D
 .get
@@ -1959,16 +1959,22 @@
 ion());%0A
+%09%09%09%09%7D%0A
 %09%09%09%09else

Support BNG projection