Application: gvSIG desktop » gvSIG Linear Reference System

/* gvSIG. Desktop Geographic Information System.

 *

 * Copyright ? 2007-2015 gvSIG Association

 *

 * This program is free software; you can redistribute it and/or

 * modify it under the terms of the GNU General Public License

 * as published by the Free Software Foundation; either version 2

 * of the License, or (at your option) any later version.

 *

 * This program 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 General Public License

 * along with this program; if not, write to the Free Software

 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,

 * MA  02110-1301, USA.

 *

 * For any additional information, do not hesitate to contact us

 * at info AT gvsig.com, or visit our website www.gvsig.com.

 */

package org.gvsig.lrs.lib.impl;

import java.util.ArrayList;

import java.util.HashMap;

import java.util.List;

import java.util.Map;

import java.util.Map.Entry;

import org.cresques.cts.IProjection;

import org.gvsig.fmap.dal.feature.EditableFeature;

import org.gvsig.fmap.dal.feature.Feature;

import org.gvsig.fmap.dal.feature.FeatureAttributeDescriptor;

import org.gvsig.fmap.dal.feature.FeatureSet;

import org.gvsig.fmap.dal.feature.FeatureStore;

import org.gvsig.fmap.dal.feature.NewFeatureStoreParameters;

import org.gvsig.fmap.geom.Geometry;

import org.gvsig.fmap.geom.GeometryLocator;

import org.gvsig.fmap.geom.GeometryManager;

import org.gvsig.fmap.geom.exception.CreateGeometryException;

import org.gvsig.fmap.geom.operation.GeometryOperationException;

import org.gvsig.fmap.geom.operation.GeometryOperationNotSupportedException;

import org.gvsig.fmap.geom.primitive.Line;

import org.gvsig.fmap.geom.primitive.Point;

import org.gvsig.lrs.lib.api.DistanceUnits;

import org.gvsig.lrs.lib.api.LrsAlgorithm;

import org.gvsig.lrs.lib.api.LrsAlgorithmParams;

import org.gvsig.lrs.lib.api.LrsCalibrateRouteAlgorithmParams;

import org.gvsig.lrs.lib.api.LrsCreateRouteAlgorithmParams;

import org.gvsig.lrs.lib.api.LrsMeasureCalculationMethods;

import org.gvsig.lrs.lib.api.exceptions.LrsCalibrateRouteException;

import org.gvsig.lrs.lib.api.exceptions.LrsException;

import org.gvsig.tools.ToolsLocator;

import org.gvsig.tools.exception.BaseException;

import org.gvsig.tools.i18n.I18nManager;

import org.gvsig.tools.service.Manager;

import org.gvsig.tools.task.SimpleTaskStatus;

import org.gvsig.tools.visitor.VisitCanceledException;

import org.gvsig.tools.visitor.Visitor;

import org.slf4j.Logger;

import org.slf4j.LoggerFactory;

/**

 * @author dmartinez

 *

 */

public class LrsCalibrateRouteAlgorithm implements LrsAlgorithm {

    private static final Logger logger = LoggerFactory.getLogger(LrsCalibrateRouteAlgorithm.class);

    private LrsCalibrateRouteAlgorithmParams parameters;

    /**

     *

     */

    public LrsCalibrateRouteAlgorithm(LrsCalibrateRouteAlgorithmParams parameters) {

        this.parameters = parameters;

    }

    /*

     * (non-Javadoc)

     *

     * @see org.gvsig.tools.service.Service#getManager()

     */

    public Manager getManager() {

        return null;

    }

    /*

     * (non-Javadoc)

     *

     * @see org.gvsig.lrs.lib.api.LrsAlgorithm#getParams()

     */

    public  LrsAlgorithmParams getParams() {

        return this.parameters;

    }

    /* (non-Javadoc)

     * @see org.gvsig.lrs.lib.api.LrsAlgorithm#setParams(org.gvsig.lrs.lib.api.LrsAlgorithmParams)

     */

    public void setParams(LrsAlgorithmParams params) throws IllegalArgumentException {

        if(!(params instanceof LrsCreateRouteAlgorithmParams)){

            throw new IllegalArgumentException("params should be LrsCalibrateRouteAlgorithmParams type.");

        }

        // TODO Auto-generated method stub

    }

    /*

     * (non-Javadoc)

     * @see org.gvsig.lrs.lib.api.LrsAlgorithm#execute(org.gvsig.tools.task.SimpleTaskStatus)

     */

    public void execute(final SimpleTaskStatus taskStatus) throws LrsException {

        NewFeatureStoreParameters newFeatureStoreParameters =

            parameters.getNewFeatureStoreParameters();

        FeatureStore sourceFeatureStore = parameters.getSourceFeatureStore();

        FeatureAttributeDescriptor idRouteField = parameters.getIdRouteField();

        FeatureStore calibratePointFeatureStore= parameters.getCalibratePointFeatureStore();

        FeatureAttributeDescriptor idRouteCalibratePointField =parameters.getCalibratePointIdRouteField();

        FeatureAttributeDescriptor fromMeasureField = parameters.getFromMeasureField();

        final boolean includeAll=parameters.includeAll();

        logger.info(parameters.toString());

        taskStatus.setTitle(parameters.getName());

        I18nManager i18nManager = ToolsLocator.getI18nManager();

        taskStatus.message(i18nManager.getTranslation("grouping_features"));

        try {

            final String routeFieldName = idRouteField.getName();

            final String routeCalibratePointFieldName=idRouteCalibratePointField.getName();

            final String measureFieldName=fromMeasureField.getName();

            final Double convertedSearchRadius=calculateConvertedSearchRadius();

            FeatureStore newFeatureStore = LrsAlgorithmUtils

                .createNewDataStore(newFeatureStoreParameters, idRouteField);

            FeatureSet sourceFeatures;

            if (sourceFeatureStore.getFeatureSelection().getSize() > 0) {

                sourceFeatures = sourceFeatureStore.getFeatureSelection();

            } else {

                sourceFeatures = sourceFeatureStore.getFeatureSet();

            }

            final Map<String, Object[]> featuresMap =

                new HashMap<String, Object[]>();

            sourceFeatures.accept(new Visitor() {

                public void visit(Object obj)

                    throws VisitCanceledException, BaseException {

                    Feature feature = (Feature) obj;

                    String routeName = (String) feature.get(routeFieldName);

                    List<Point> points = new ArrayList<Point>();

                    Object[] valueObject=new Object[2];

                    valueObject[0]=feature.getDefaultGeometry();

                    valueObject[1]=points;

                    featuresMap.put(routeName, valueObject);

                }

            });

            FeatureSet pointFeatures=calibratePointFeatureStore.getFeatureSet();

            pointFeatures.accept(new Visitor() {

                public void visit(Object obj) throws VisitCanceledException, BaseException {

                    Feature feature = (Feature) obj;

                    String routeName = (String) feature.get(routeCalibratePointFieldName);

                    Double measure= (Double) feature.get(measureFieldName);

                    Geometry geomPoint=(Point)feature.getDefaultGeometry();

                    if (geomPoint instanceof Point){

                        Point point=(Point) geomPoint;

                        Object valueObject=featuresMap.get(routeName);

                        if (valueObject!=null && valueObject instanceof Object[]){

                            Object[] objArray=(Object[])valueObject;

                            if (objArray[1] instanceof List){

                                List<Point> points=(List<Point>)objArray[1];

                                if (feature.getDefaultGeometry() instanceof Point){

                                    Geometry geometry=(Geometry)objArray[0];

                                    if (isValidPoint(point,geometry, convertedSearchRadius)){

                                        try {

                                            Point mPoint= pointToMPoint(geometry,point, measure);

                                            points.add(mPoint);

                                        } catch (Exception e) {

                                            logger.debug("Error adding point",e);

                                        }

                                    }

                                }

                            }

                        }

                    }

                }

            });

            taskStatus.setRangeOfValues(0, featuresMap.size());

            int taskCount = 0;

            newFeatureStore.edit(FeatureStore.MODE_FULLEDIT);

            for (Entry<String, Object[]> entry : featuresMap

                .entrySet()) {

                String routeName = entry.getKey();

                Object valueObject = entry.getValue();

                List<Point> points=null;

                Geometry geometry=null;

                if (valueObject!=null && valueObject instanceof Object[]){

                    Object[] objArray=(Object[])valueObject;

                    if (objArray[0] instanceof Geometry){

                        geometry=(Geometry)objArray[0];

                    }

                    if (objArray[1] instanceof List){

                        points=(List<Point>)objArray[1];

                    }

                }

                if (!points.isEmpty()){

                    EditableFeature newFeature =

                        newFeatureStore.createNewFeature(true);

                    newFeature.set(routeFieldName, routeName);

                    Geometry calibratedRoute = calibrateRoute(geometry,points);

                    newFeature.setDefaultGeometry(calibratedRoute);

                    newFeatureStore.update(newFeature);

                }else{

                    if (includeAll){

                        EditableFeature newFeature =

                            newFeatureStore.createNewFeature(true);

                        newFeature.set(routeFieldName, routeName);

                        newFeature.setDefaultGeometry(geometry);

                        newFeatureStore.update(newFeature);

                    }

                }

                taskCount++;

                taskStatus.setCurValue(taskCount);

            }

            newFeatureStore.finishEditing();

        } catch (Exception e1) {

            taskStatus.abort();

            throw new LrsCalibrateRouteException("Error calibrating routes", e1);

        }

        taskStatus.terminate();

    }

    private Point pointToMPoint(Geometry geometry,Point point, Double measure) throws CreateGeometryException, GeometryOperationNotSupportedException, GeometryOperationException{

        GeometryManager geomanager = GeometryLocator.getGeometryManager();

        Point[] pointsInLine=(Point[])geometry.closestPoints(point);

        Point pointInLine=pointsInLine[0];

        Point mPoint = (Point) geomanager

            .create(Geometry.TYPES.POINT, Geometry.SUBTYPES.GEOM2DM);

        mPoint.setX(pointInLine.getX());

        mPoint.setY(pointInLine.getY());

        int mDimension=mPoint.getDimension()-1;

        mPoint.setCoordinateAt(mDimension, measure);

        return mPoint;

    }

    private Double calculateConvertedSearchRadius(){

        Double searchRadius=parameters.getSearchRadius();

        DistanceUnits measureUnits = parameters.getMeasureUnits();

        FeatureStore sourceFeatureStore = parameters.getSourceFeatureStore();

        IProjection projection;

        try {

            projection = sourceFeatureStore.getDefaultFeatureType().getDefaultSRS();

            Double searchRadiusInMeters=searchRadius*measureUnits.getTransToMeter();

            //TODO

            //DistanceUnits projectionDistanceUnits=getProjectionDistanceUnits(projection);

            //return searchRadiusInMeters/projectionDistanceUnits.getTransToMeter();

            return searchRadiusInMeters;

        } catch (Exception e) {

            return new Double(0);

        }

    }

    private boolean isValidPoint(Point point,Geometry geometry, Double convertedSearchRadius){

        try {

            return geometry.isWithinDistance(point, convertedSearchRadius);

        } catch (Exception e) {

            return false;

        }

    }

    private Geometry calibrateRoute(Geometry geometry, List<Point> points) {

        LrsMeasureCalculationMethods calculationMethod= parameters.getMeasureCalculationMethods();

        boolean ignoreSpatialGaps = parameters.ignoreSpatialGaps();

        boolean interpolateBetweenCalibrationPoints=parameters.interpolateBetweenCalibrationPoints();

        boolean extrapolateBeforeCalibrationPoints=parameters.extrapolateBeforeCalibrationPoints();

        boolean extrapolateAfterCalibrationPoints=parameters.extrapolateAfterCalibrationPoints();

        if (points.isEmpty()){

            return geometry;

        }

        ExpandedRoute expandedRoute=new ExpandedRoute(geometry, ignoreSpatialGaps);

        switch (calculationMethod) {

        case DISTANCE:

            if (extrapolateBeforeCalibrationPoints||interpolateBetweenCalibrationPoints||extrapolateAfterCalibrationPoints){

                expandedRoute.resetMeasuresToDistances();

            }

        case MEASURES:

        default:

            expandedRoute.addFixedPoints(points);

            expandedRoute.calculateCalibrationPoints(extrapolateBeforeCalibrationPoints,interpolateBetweenCalibrationPoints,extrapolateAfterCalibrationPoints);

            return expandedRoute.getRoute();

        }

    }

    class ExpandedRoute{

        private Geometry route;

        private List<Boolean> isFixedPoint;

        private List<Double> distances;

        private List<Double>oldMValues;

        private List<Point> vertices;

        private List<Integer> lineIndexes;

        private int MDIMENSION;

        private final Double PRECISION=new Double(1.0e-5);

        public Geometry getRoute(){

            return route;

        }

        public ExpandedRoute(Geometry geometry, boolean ignoreSpatialGaps){

            isFixedPoint=new ArrayList<Boolean>();

            distances=new ArrayList<Double>();

            oldMValues=new ArrayList<Double>();

            vertices=new ArrayList<Point>();

            lineIndexes=new ArrayList<Integer>();

            route=geometry.cloneGeometry();

            double distance = 0;

            List<Line> lines=LrsAlgorithmUtils.extractLines(geometry);

            Point previousVertex=null;

            for (int i=0; i<lines.size();i++){

                Line line=lines.get(i);

                for (int j=0; j<line.getNumVertices();j++){

                    Point vertex=line.getVertex(j);

                    if(j==0){

                       if (previousVertex==null){

                           //First point in geometry

                           MDIMENSION=vertex.getDimension()-1;

                           distance=0;

                       }

                       Integer visitedIndex = getIndexVisitedVertex(vertex);

                       if (visitedIndex!=null){

                           previousVertex=vertices.get(visitedIndex);

                           distance=distances.get(visitedIndex);

                       }else{

                           if (previousVertex!=null){

                               try {

                                   if(!LrsAlgorithmUtils.equalPoints(vertex, previousVertex)){

                                       Integer nearestVertexPos=getIndexNearestVisitedVertex(vertex);

                                       if (ignoreSpatialGaps){

                                           distance=distances.get(nearestVertexPos);

                                       }else{

                                           Point nearestVertex=vertices.get(nearestVertexPos);

                                           distance=distances.get(nearestVertexPos);

                                           distance+=vertex.distance(nearestVertex);

                                       }

                                   }

                               } catch (Exception e) {

                                   distance=Double.NaN;

                               }

                           }

                       }

                    }else{

                        try {

                            distance+=vertex.distance(previousVertex);

                        } catch (Exception e) {

                            distance=Double.NaN;

                        }

                    }

                    Double mValue=vertex.getCoordinateAt(MDIMENSION);

                    isFixedPoint.add(false);

                    vertices.add(vertex);

                    distances.add(distance);

                    oldMValues.add(mValue);

                    lineIndexes.add(i);

                    previousVertex = vertex;

                }

            }

        }

        private Integer getIndexNearestVisitedVertex(Point vertex) throws GeometryOperationNotSupportedException, GeometryOperationException{

            double nearestDistance = Double.POSITIVE_INFINITY;

            Integer nearestPointPos=null;

            for (int i=0;i<vertices.size();i++){

                Double distance=vertex.distance(vertices.get(i));

                if(nearestDistance>distance && distance>Double.valueOf(0)){

                    nearestDistance=distance;

                    nearestPointPos=i;

                }

            }

            return nearestPointPos;

        }

        private Integer getIndexVisitedVertex(Point point){

            double distance = Double.NEGATIVE_INFINITY;

            Integer index = null;

            for(int i =0; i<vertices.size(); i++){

                if (LrsAlgorithmUtils.equalPoints(point, vertices.get(i))){

                    if(this.distances.get(i)>distance){

                        distance = this.distances.get(i);

                        index = i;

                    }

                }

            }

            return index;

        }

        private void addFixedPoints(List<Point> fixedPoints){

            Double minMValue=null;

            Double maxMValue=null;

            for (Point fixedPoint:fixedPoints){

                Double length=Double.valueOf(0);

                int index = 0;

                List<Line> lines=LrsAlgorithmUtils.extractLines(route);

                List<Integer>insertAtList=new ArrayList<Integer>();

                List<Double>lengths=new ArrayList<Double>();

                List<Double>oldValues=new ArrayList<Double>();

                List<Integer>lineReference=new ArrayList<Integer>();

                for (int i=0; i<lines.size();i++){

                    Line line=lines.get(i);

                    try{

                        //FIXME

                        //if (line.intersects(fixedPoint)){

                        if (line.isWithinDistance(fixedPoint, PRECISION)){

                            length=distances.get(index);

                            for (int j=0;j<line.getNumVertices();j++){

                                Point vertex=line.getVertex(j);

                                Point nextVertex;

                                if (j+1<line.getNumVertices()){

                                    nextVertex=line.getVertex(j+1);

                                }else{

                                    nextVertex=null;

                                }

                                if(LrsAlgorithmUtils.equalPoints(vertex, fixedPoint)||vertex.distance(fixedPoint)<=PRECISION){

                                    oldMValues.set(index, vertex.getCoordinateAt(MDIMENSION));

                                    Double mValue=fixedPoint.getCoordinateAt(MDIMENSION);

                                    vertex.setCoordinateAt(MDIMENSION, mValue);

                                    isFixedPoint.set(index, true);

                                    vertices.set(index, vertex);

                                }else{

                                    if (nextVertex!=null

                                        && !LrsAlgorithmUtils.equalPoints(nextVertex, fixedPoint)

                                        && vertex.distance(fixedPoint)>PRECISION){

                                        GeometryManager geomanager = GeometryLocator.getGeometryManager();

                                        Line segment= (Line) geomanager

                                            .create(Geometry.TYPES.LINE, Geometry.SUBTYPES.GEOM2DM);

                                        segment.addVertex(vertex);

                                        segment.addVertex(nextVertex);

                                        //FIXME

                                        //if (line.intersects(fixedPoint)){

                                        if (segment.isWithinDistance(fixedPoint, PRECISION)){

                                            length+=vertex.distance(fixedPoint);

                                            Double oldMValue=LrsAlgorithmUtils.straightLineThroughTwoPointsEquation(

                                                vertex.getCoordinateAt(MDIMENSION),

                                                nextVertex.getCoordinateAt(MDIMENSION),

                                                distances.get(index),

                                                distances.get(index+1),

                                                length);

                                            insertAtList.add(index+1);

                                            lengths.add(length);

                                            oldValues.add(oldMValue);

                                            lineReference.add(i);

                                        }

                                    }

                                }

                                index++;

                            }

                        }else{

                            index+=line.getNumVertices();

                        }

                    }catch(Exception e){

                        logger.warn("Error inserting point "+fixedPoint.toString(),e);

                    }

                }

                int pos=0;

                for (Integer insertAt:insertAtList){

                    Double distance=lengths.get(pos);

                    Double oldMValue=oldValues.get(pos);

                    Integer linePos=lineReference.get(pos);

                    int correctedPosition=insertAt+pos;

                    route=LrsAlgorithmUtils.insertVertex(route, fixedPoint, correctedPosition);

                    isFixedPoint.add(correctedPosition, true);

                    distances.add(correctedPosition, distance);

                    oldMValues.add(correctedPosition, oldMValue);

                    lineIndexes.add(correctedPosition, linePos);

                    vertices.add(correctedPosition, fixedPoint);

                    pos++;

                }

            }

        }

        public void resetMeasuresToDistances(){

             List<Point> points=LrsAlgorithmUtils.extractPoints(route);

             for (int i=0;i<points.size();i++){

                 if (!isFixedPoint.get(i)){

                     points.get(i).setCoordinateAt(MDIMENSION, distances.get(i));

                     vertices.get(i).setCoordinateAt(MDIMENSION, distances.get(i));

                     oldMValues.set(i, distances.get(i));

                 }

             }

        }

//        public void interpolateBetweenCalibrationPoints(){

//            Double newMValueBeforePoint=Double.NaN;

//            Double oldMValueBeforePoint=Double.NaN;

//            Double calibrationRatio=Double.NaN;

//            for (int i=firstFixedPointPos; i<=lastFixedPointPos-1;i++){

//                if (isFixedPoint.get(i)){

//                    int pointBeforePos=i;

//                    int pointAfterPos=getNextPosFixedPoint(i);

//

//                    newMValueBeforePoint=vertices.get(pointBeforePos).getCoordinateAt(MDIMENSION);

//                    Double newMValueAfterPoint=vertices.get(pointAfterPos).getCoordinateAt(MDIMENSION);

//

//                    oldMValueBeforePoint=oldMValues.get(pointBeforePos);

//                    Double oldMValueAfterPoint=oldMValues.get(pointAfterPos);

//

//                    Double distanceBetweenFixedPoints=newMValueAfterPoint-newMValueBeforePoint;

//                    Double distanceBetweenOldFixedPoints=oldMValueAfterPoint-oldMValueBeforePoint;

//

//                    calibrationRatio=distanceBetweenOldFixedPoints/distanceBetweenFixedPoints;

//                }else{

//                    Double valueToRecalculate=vertices.get(i).getCoordinateAt(MDIMENSION);

//                    Double distanceBetween=valueToRecalculate-oldMValueBeforePoint;

//                    Double mValue=newMValueBeforePoint+(distanceBetween/calibrationRatio);

//                    refreshMValueGeometryVertex(i,mValue);

//                }

//            }

//        }

        public void calculateCalibrationPoints(boolean before, boolean between, boolean after){

            for (int i=0; i<vertices.size();i++){

                if(!isFixedPoint.get(i)){

                    Integer prevFixedPointPos=getPreviousPosFixedPoint(i);

                    Integer nextFixedPointPos=getNextPosFixedPoint(i);

                    if (prevFixedPointPos==null && nextFixedPointPos!=null  && before){

                        //TODO

                        //extrapolateBeforeCalibrationPoints(nextFixedPointPos,i);

                    }

                    if (prevFixedPointPos!=null && nextFixedPointPos!=null && between){

                        calculateMValue(prevFixedPointPos,nextFixedPointPos,i);

                    }

                    if (prevFixedPointPos!=null && nextFixedPointPos==null && after){

                        //TODO

                        //extrapolateAfterCalibrationPoints(prevFixedPointPos,i);

                    }

                }

            }

        }

        public void calculateMValue(int prevFixedPointPos,int nextFixedPointPos,int pos){

            Double newMValueBeforePoint=vertices.get(prevFixedPointPos).getCoordinateAt(MDIMENSION);

            Double newMValueAfterPoint=vertices.get(nextFixedPointPos).getCoordinateAt(MDIMENSION);

            Double oldMValueBeforePoint=oldMValues.get(prevFixedPointPos);

            Double oldMValueAfterPoint=oldMValues.get(nextFixedPointPos);

            Double distanceBetweenFixedPoints=newMValueAfterPoint-newMValueBeforePoint;

            Double distanceBetweenOldFixedPoints=oldMValueAfterPoint-oldMValueBeforePoint;

            Double calibrationRatio=distanceBetweenOldFixedPoints/distanceBetweenFixedPoints;

            Double valueToRecalculate=vertices.get(pos).getCoordinateAt(MDIMENSION);

            Double distanceBetween=valueToRecalculate-oldMValueBeforePoint;

            Double mValue=newMValueBeforePoint+(distanceBetween/calibrationRatio);

            refreshMValueGeometryVertex(pos,mValue);

        }

//        public void extrapolateBeforeCalibrationPoints(){

//            Integer nextFixedPointPos=getNextPosFixedPoint(firstFixedPointPos);

//            if (nextFixedPointPos!=null){

//                Double newMValueFirstPoint=vertices.get(firstFixedPointPos).getCoordinateAt(MDIMENSION);

//                Double newMValueSecondPoint=vertices.get(nextFixedPointPos).getCoordinateAt(MDIMENSION);

//

//                Double oldMValueFirstPoint=oldMValues.get(firstFixedPointPos);

//                Double oldMValueSecondPoint=oldMValues.get(nextFixedPointPos);

//

//                Double distanceBetweenFixedPoints=newMValueSecondPoint-newMValueFirstPoint;

//                Double distanceBetweenOldFixedPoints=oldMValueSecondPoint-oldMValueFirstPoint;

//

//                Double calibrationRatio=distanceBetweenOldFixedPoints/distanceBetweenFixedPoints;

//                for (int i=0; i<firstFixedPointPos;i++){

//                    Double valueToRecalculate=vertices.get(i).getCoordinateAt(MDIMENSION);

//                    Double distanceBetween=oldMValueFirstPoint-valueToRecalculate;

//                    Double mValue=newMValueFirstPoint-(distanceBetween/calibrationRatio);

//                    refreshMValueGeometryVertex(i,mValue);

//                }

//            }

//        }

//

//        public void extrapolateAfterCalibrationPoints(){

//            Integer prevFixedPointPos=getPreviousPosFixedPoint(lastFixedPointPos);

//            if (prevFixedPointPos!=null){

//                Double newMValueLastPoint=vertices.get(lastFixedPointPos).getCoordinateAt(MDIMENSION);

//                Double newMValuePrevPoint=vertices.get(prevFixedPointPos).getCoordinateAt(MDIMENSION);

//

//                Double oldMValueLastPoint=oldMValues.get(lastFixedPointPos);

//                Double oldMValuePrevPoint=oldMValues.get(prevFixedPointPos);

//

//                Double distanceBetweenFixedPoints=newMValueLastPoint-newMValuePrevPoint;

//                Double distanceBetweenOldFixedPoints=oldMValueLastPoint-oldMValuePrevPoint;

//

//                Double calibrationRatio=distanceBetweenOldFixedPoints/distanceBetweenFixedPoints;

//                for (int i=lastFixedPointPos+1; i<vertices.size();i++){

//                    Double valueToRecalculate=vertices.get(i).getCoordinateAt(MDIMENSION);

//                    Double distanceBetween=valueToRecalculate-oldMValueLastPoint;

//                    Double mValue=newMValueLastPoint+(distanceBetween/calibrationRatio);

//                    refreshMValueGeometryVertex(i,mValue);

//                }

//            }

//        }

//

//        private Integer getNextPosFixedPoint(int pos){

//            for (int i=pos+1;i<isFixedPoint.size();i++){

//                if (isFixedPoint.get(i)){

//                    if( !distances.get(pos).equals(distances.get(i)) ){

//                        return i;

//                    }

//                }

//            }

//            return null;

//        }

        private Integer getNextPosFixedPoint(int pos){

            Integer nextFixedPointPos= findNextFixedPointInLine(pos);

            if (nextFixedPointPos!=null){

                return nextFixedPointPos;

            }else{

                int lastPositionInLine=getLastPositionInLine(pos);

                nextFixedPointPos=findNextFixedPointInNextLines(lastPositionInLine,new ArrayList<Integer>());

                if (nextFixedPointPos!=null){

                    return nextFixedPointPos;

                }

                try{

                    Integer nearestVertex=findNearestNextPoint(lastPositionInLine);

                    return getNextPosFixedPoint(nearestVertex);

                }catch (Exception e){

                    return null;

                }

            }

        }

        private Integer getLastPositionInLine (int pos){

            Integer lineVisited=lineIndexes.get(pos);

            for (int i=pos+1;i<vertices.size();i++){

                if (!lineVisited.equals(lineIndexes.get(i))){

                    return i-1;

                }

            }

            return vertices.size()-1;

        }

        private Integer findNextFixedPointInNextLines(int pos,List<Integer> visitedLines){

            Integer nextFixedPointPos= findNextFixedPointInLine(pos);

            if (nextFixedPointPos!=null){

                return nextFixedPointPos;

            }else{

                Integer lineIndex=lineIndexes.get(pos);

                visitedLines.add(lineIndex);

                int lastPositionInLine=getLastPositionInLine(pos);

                Point lastVertexInLine=vertices.get(lastPositionInLine);

                for (int i=pos+1;i<vertices.size();i++){

                    if (LrsAlgorithmUtils.equalPoints(lastVertexInLine, vertices.get(i))){

                        if (!visitedLines.contains(lineIndexes.get(i))){

                            findNextFixedPointInNextLines(i,visitedLines);

                        }

                    }

                }

            }

            return null;

        }

        private Integer findNextFixedPointInLine(int vertexPos){

            int lineIndex=lineIndexes.get(vertexPos);

            for (int i=vertexPos;i<vertices.size();i++){

                if(!lineIndexes.get(i).equals(lineIndex)){

                    return null;

                }

                if (isFixedPoint.get(i)){

                    return i;

                }

            }

            return null;

        }

        private Integer findNearestNextPoint(int vertexPos) throws GeometryOperationNotSupportedException, GeometryOperationException{

            Point vertex=vertices.get(vertexPos);

            double nearestDistance = Double.POSITIVE_INFINITY;

            Integer nearestPointPos=null;

            for (int i=vertexPos+1;i<vertices.size();i++){

                Double distance=vertex.distance(vertices.get(i));

                if(nearestDistance>distance && distance>Double.valueOf(0)){

                    nearestDistance=distance;

                    nearestPointPos=i;

                }

            }

            return nearestPointPos;

        }

        private Integer getPreviousPosFixedPoint(int pos){

            Integer prevFixedPointPos= findPrevFixedPointInLine(pos);

            if (prevFixedPointPos!=null){

                return prevFixedPointPos;

            }else{

                Integer lineVisited=lineIndexes.get(pos);

                for (int i=pos-1;i>=0;i--){

                    if (!lineVisited.equals(lineIndexes.get(i))){//Line has changed

                        int lastPositionInLine=i+1;

                        for (int j=lastPositionInLine;j>=0;j--){

                            if (LrsAlgorithmUtils.equalPoints(vertices.get(lastPositionInLine), vertices.get(j))){

                                if (isFixedPoint.get(j)){

                                    return j;

                                }

                            }

                        }

                        try{

                            return findNearestPrevFixedPoint(lastPositionInLine);

                        }catch (Exception e){

                            return null;

                        }

                    }

                }

            }

            return null;

        }

        private Integer findPrevFixedPointInLine(int vertexPos){

            int lineIndex=lineIndexes.get(vertexPos);

            for (int i=vertexPos;i>=0;i--){

                if(!lineIndexes.get(i).equals(lineIndex)){

                    return null;

                }

                if (isFixedPoint.get(i)){

                    return i;

                }

            }

            return null;

        }

        private Integer findNearestPrevFixedPoint(int vertexPos) throws GeometryOperationNotSupportedException, GeometryOperationException{

            Point vertex=vertices.get(vertexPos);

            double nearestDistance = Double.POSITIVE_INFINITY;

            Integer nearestPointPos=null;

            for (int i=vertexPos-1;i>=0;i--){

                if (isFixedPoint.get(i)){

                    Double distance=vertex.distance(vertices.get(i));

                    if(nearestDistance>distance){

                        nearestDistance=distance;

                        nearestPointPos=i;

                    }

                }

            }

            return nearestPointPos;

        }

        private void refreshMValueGeometryVertex(int i, Double mValue){

            List<Point> points=LrsAlgorithmUtils.extractPoints(route);

            vertices.get(i).setCoordinateAt(MDIMENSION, mValue);

            points.get(i).setCoordinateAt(MDIMENSION,mValue);

            isFixedPoint.set(i, true);

        }

    }

}