Program: 175224
Project: 175224
Buggy source:
public void orbitInterpolation(List<S1Metadata.OrbitStatePosVelox> vpList,double zeroDopplerTimeFirstLineSeconds,double zeroDopplerTimeLastLineSeconds,double samplingf,double iSafetyBufferAz) throws MathException {
  this.iSafetyBufferAz=iSafetyBufferAz;
  double deltaT=1 / samplingf;
  double deltaTinv=1 / deltaT;
  int nPoints=vpList.size();
  double reftime=0;
  double firstTime=vpList.get(0).time;
  double lastTime=vpList.get(vpList.size() - 1).time;
  if (firstTime < (lastTime)) {
    reftime=firstTime;
  }
 else {
    reftime=lastTime;
  }
  int idx=0;
  secondsDiffFromRefTime=new double[vpList.size()];
  for (  S1Metadata.OrbitStatePosVelox vp : vpList) {
    secondsDiffFromRefTime[idx]=(vp.time - reftime);
    idx++;
  }
  zeroDopplerTimeFirstRef=(zeroDopplerTimeFirstLineSeconds - reftime);
  zeroDopplerTimeLastRef=(zeroDopplerTimeLastLineSeconds - reftime);
  double minT=0;
  double maxT=0;
  double zeroDopplerTimeFirstLineWSafetyBufSecondsRef=0;
  double zeroDopplerTimeLastLineWSafetyBufSecondsRef=0;
  if (zeroDopplerTimeFirstRef < zeroDopplerTimeLastRef) {
    zeroDopplerTimeFirstLineWSafetyBufSecondsRef=zeroDopplerTimeFirstRef - iSafetyBufferAz * deltaT;
    zeroDopplerTimeLastLineWSafetyBufSecondsRef=zeroDopplerTimeLastRef + iSafetyBufferAz * deltaT;
    minT=FastMath.min(0,zeroDopplerTimeFirstLineWSafetyBufSecondsRef);
    maxT=FastMath.max(secondsDiffFromRefTime[secondsDiffFromRefTime.length - 1],zeroDopplerTimeLastLineWSafetyBufSecondsRef);
  }
 else {
    zeroDopplerTimeFirstLineWSafetyBufSecondsRef=zeroDopplerTimeFirstRef + iSafetyBufferAz * deltaT;
    zeroDopplerTimeLastLineWSafetyBufSecondsRef=zeroDopplerTimeLastRef - iSafetyBufferAz * deltaT;
    if (firstTime < (lastTime)) {
      minT=FastMath.min(secondsDiffFromRefTime[0],zeroDopplerTimeFirstLineWSafetyBufSecondsRef);
      maxT=FastMath.max(secondsDiffFromRefTime[secondsDiffFromRefTime.length - 1],zeroDopplerTimeFirstLineWSafetyBufSecondsRef);
    }
 else {
      minT=FastMath.min(secondsDiffFromRefTime[secondsDiffFromRefTime.length - 1],zeroDopplerTimeFirstLineWSafetyBufSecondsRef);
      maxT=FastMath.max(secondsDiffFromRefTime[0],zeroDopplerTimeFirstLineWSafetyBufSecondsRef);
    }
  }
  int limit=((Double)((maxT - minT) / deltaT)).intValue();
  Double[] timeStampInterpSecondsRef=new Double[limit + 1];
  double nextVal=0;
  for (int j=0; j <= limit; j++) {
    timeStampInterpSecondsRef[j]=nextVal;
    nextVal=deltaT + nextVal;
  }
  double zeroDopplerTimeInitRef=zeroDopplerTimeFirstRef;
  double zeroDopplerTimeEndRef=zeroDopplerTimeLastRef;
  if (zeroDopplerTimeFirstRef > zeroDopplerTimeLastRef) {
    zeroDopplerTimeInitRef=zeroDopplerTimeLastRef;
    zeroDopplerTimeEndRef=zeroDopplerTimeFirstRef;
  }
  if (zeroDopplerTimeInitRef < timeStampInterpSecondsRef[1]) {
    int size=Math.min(nPoints,N_ORB_POINT_INTERP);
    double[] subTimesDiffRef=Arrays.copyOfRange(secondsDiffFromRefTime,0,nPoints);
    List<Double> timeStampInitSecondsRefPointsInterp=new ArrayList<Double>();
    for (int i=0; i < size; i++) {
      double valMax=timeStampInterpSecondsRef[size - 1];
      if (timeStampInterpSecondsRef[i] < valMax) {
        timeStampInitSecondsRefPointsInterp.add(timeStampInterpSecondsRef[i]);
      }
    }
    List<IMetadata.OrbitStatePosVelox> subList=vpList.subList(0,size);
    double initTime=zeroDopplerTimeInitRef;
    int idxInitTime=-1;
    boolean finded=false;
    double endTime=secondsDiffFromRefTime[0];
    int idxEndTime=-1;
    for (int i=0; i < timeStampInterpSecondsRef.length; i++) {
      if (timeStampInterpSecondsRef[i] >= initTime && !finded) {
        idxInitTime=i;
        finded=true;
      }
      if (timeStampInterpSecondsRef[i] < endTime) {
        idxEndTime=i;
      }
    }
    logger.debug("String_Node_Str" + idxInitTime + "String_Node_Str"+ idxEndTime);
    Double[] t0=(Double[])timeStampInitSecondsRefPointsInterp.toArray(new Double[0]);
    if (idxInitTime != -1 && idxEndTime != -1 && initTime < endTime) {
      List<double[]> vPoints=new ArrayList<double[]>();
      List<double[]> pPoints=new ArrayList<double[]>();
      List<Double> timeStampOutput=new ArrayList<Double>();
      HermiteInterpolation.interpolation(subTimesDiffRef,subList,t0,idxInitTime,idxEndTime,deltaTinv,vPoints,pPoints,timeStampOutput);
      addPointsToResult(vPoints,pPoints);
      if (timeStampInterp == null)       timeStampInterp=new ArrayList<>(timeStampOutput);
 else       timeStampInterp.addAll(timeStampOutput);
    }
  }
  for (int orbPoint=1; orbPoint < nPoints; orbPoint++) {
    if (secondsDiffFromRefTime[orbPoint - 1] < zeroDopplerTimeEndRef || secondsDiffFromRefTime[orbPoint] > zeroDopplerTimeEndRef) {
      int idxStart=0;
      int idxEnd=0;
      if (N_ORB_POINT_INTERP > nPoints) {
        idxEnd=nPoints;
      }
 else {
        idxStart=orbPoint - new Double(Math.floor(N_ORB_POINT_INTERP / 2)).intValue();
        idxEnd=idxStart + N_ORB_POINT_INTERP - 1;
        if (idxStart < 1) {
          idxStart=0;
          idxEnd=N_ORB_POINT_INTERP - 1;
        }
        if (idxEnd >= nPoints) {
          idxStart=nPoints - N_ORB_POINT_INTERP;
          idxEnd=nPoints - 1;
        }
      }
      double[] timeStampInitSecondsRefPoints=Arrays.copyOfRange(secondsDiffFromRefTime,idxStart,idxEnd + 1);
      List<Double> timeStampInitSecondsRefPointsInterp=new ArrayList<Double>();
      for (int i=0; i < timeStampInterpSecondsRef.length; i++) {
        if (timeStampInterpSecondsRef[i] >= secondsDiffFromRefTime[idxStart] && timeStampInterpSecondsRef[i] < secondsDiffFromRefTime[idxEnd]) {
          timeStampInitSecondsRefPointsInterp.add(timeStampInterpSecondsRef[i]);
        }
      }
      List<S1Metadata.OrbitStatePosVelox> stateVecPoints=vpList.subList(idxStart,idxEnd + 1);
      double initTime=Math.max(secondsDiffFromRefTime[orbPoint - 1],zeroDopplerTimeInitRef);
      int idxInitTime=0;
      boolean findIdxInitTime=false;
      for (int i=0; i < timeStampInitSecondsRefPointsInterp.size() && !findIdxInitTime; i++) {
        if (timeStampInitSecondsRefPointsInterp.get(i) >= initTime) {
          idxInitTime=i;
          findIdxInitTime=true;
        }
      }
      double endTime=Math.min(secondsDiffFromRefTime[orbPoint],zeroDopplerTimeEndRef);
      int idxEndTime=0;
      boolean findIdxEndTime=false;
      for (int i=timeStampInitSecondsRefPointsInterp.size() - 1; i >= 0 && !findIdxEndTime; i--) {
        if (timeStampInitSecondsRefPointsInterp.get(i) < endTime) {
          idxEndTime=i;
          findIdxEndTime=true;
        }
      }
      double timeRef=timeStampInitSecondsRefPointsInterp.get(0);
      Double[] timeStampInitSecondsRefPointsInterp0=new Double[timeStampInitSecondsRefPointsInterp.size()];
      for (int i=0; i < timeStampInitSecondsRefPointsInterp.size(); i++) {
        timeStampInitSecondsRefPointsInterp0[i]=timeStampInitSecondsRefPointsInterp.get(i) - timeRef;
      }
      if (findIdxEndTime && findIdxInitTime && initTime < endTime && idxInitTime < idxEndTime) {
        List<double[]> vPoints=new ArrayList<double[]>();
        List<double[]> pPoints=new ArrayList<double[]>();
        List<Double> timeStampOutput=new ArrayList<Double>();
        HermiteInterpolation.interpolation(timeStampInitSecondsRefPoints,stateVecPoints,timeStampInitSecondsRefPointsInterp0,idxInitTime,idxEndTime,deltaTinv,pPoints,vPoints,timeStampOutput);
        addPointsToResult(vPoints,pPoints);
        List<Double> timeStampInterpSecondsRefOutput=new ArrayList<Double>();
        for (int i=0; i < timeStampOutput.size(); i++) {
          timeStampInterpSecondsRefOutput.add(timeStampOutput.get(i) + timeRef);
        }
        if (timeStampInterp == null)         timeStampInterp=new ArrayList<>(timeStampInterpSecondsRefOutput);
 else         timeStampInterp.addAll(timeStampInterpSecondsRefOutput);
      }
    }
  }
  if (zeroDopplerTimeEndRef > secondsDiffFromRefTime[secondsDiffFromRefTime.length - 1]) {
    int vOrbPointsInit=(nPoints - Math.min(nPoints,N_ORB_POINT_INTERP));
    int vOrbPointsEnd=nPoints - 1;
    double[] timeStampInitSecondsRefPoints=Arrays.copyOfRange(secondsDiffFromRefTime,vOrbPointsInit,vOrbPointsEnd + 1);
    List<Double> timeStampInitSecondsRefPointsInterp=new ArrayList<Double>();
    for (int i=0; i < timeStampInterpSecondsRef.length; i++) {
      if (timeStampInterpSecondsRef[i] >= secondsDiffFromRefTime[vOrbPointsInit]) {
        timeStampInitSecondsRefPointsInterp.add(timeStampInterpSecondsRef[i]);
      }
    }
    List<S1Metadata.OrbitStatePosVelox> stateVecPoints=vpList.subList(vOrbPointsInit,vOrbPointsEnd + 1);
    double initTime=secondsDiffFromRefTime[vOrbPointsEnd];
    int idxInitTime=0;
    boolean findIdxInitTime=false;
    for (int i=0; i < timeStampInitSecondsRefPointsInterp.size() && !findIdxInitTime; i++) {
      if (timeStampInitSecondsRefPointsInterp.get(i) >= initTime) {
        idxInitTime=i;
        findIdxInitTime=true;
      }
    }
    double endTime=zeroDopplerTimeEndRef;
    int idxEndTime=0;
    boolean findIdxEndTime=false;
    for (int i=timeStampInitSecondsRefPointsInterp.size() - 1; i >= 0 && !findIdxEndTime; i--) {
      if (timeStampInitSecondsRefPointsInterp.get(i) < endTime) {
        idxEndTime=i;
        findIdxEndTime=true;
      }
    }
    double timeRef=timeStampInitSecondsRefPointsInterp.get(0);
    Double[] timeStampInitSecondsRefPointsInterp0=new Double[timeStampInitSecondsRefPointsInterp.size()];
    for (int i=0; i < timeStampInitSecondsRefPointsInterp.size(); i++) {
      timeStampInitSecondsRefPointsInterp0[i]=timeStampInitSecondsRefPointsInterp.get(i) - timeRef;
    }
    if (findIdxEndTime && findIdxInitTime && initTime < endTime) {
      List<double[]> vPoints=new ArrayList<double[]>();
      List<double[]> pPoints=new ArrayList<double[]>();
      List<Double> timeStampOutput=new ArrayList<Double>();
      HermiteInterpolation.interpolation(timeStampInitSecondsRefPoints,stateVecPoints,timeStampInitSecondsRefPointsInterp0,idxInitTime,idxEndTime,deltaTinv,pPoints,vPoints,timeStampOutput);
      addPointsToResult(vPoints,pPoints);
      List<Double> timeStampInterpSecondsRefOutput=new ArrayList<Double>();
      for (int i=0; i < timeStampOutput.size(); i++) {
        timeStampInterpSecondsRefOutput.add(timeStampOutput.get(i) + timeRef);
      }
      if (timeStampInterp == null)       timeStampInterp=new ArrayList<>(timeStampInterpSecondsRefOutput);
 else       timeStampInterp.addAll(timeStampInterpSecondsRefOutput);
    }
  }
}

Fixed version (ground truth):
public void orbitInterpolation(List<S1Metadata.OrbitStatePosVelox> vpList,double zeroDopplerTimeFirstLineSeconds,double zeroDopplerTimeLastLineSeconds,double samplingf,double iSafetyBufferAz) throws MathException {
  this.iSafetyBufferAz=iSafetyBufferAz;
  double deltaT=1 / samplingf;
  double deltaTinv=1 / deltaT;
  int nPoints=vpList.size();
  double reftime=0;
  double firstTime=vpList.get(0).time;
  double lastTime=vpList.get(vpList.size() - 1).time;
  if (firstTime < (lastTime)) {
    reftime=firstTime;
  }
 else {
    reftime=lastTime;
  }
  int idx=0;
  secondsDiffFromRefTime=new double[vpList.size()];
  for (  S1Metadata.OrbitStatePosVelox vp : vpList) {
    secondsDiffFromRefTime[idx]=(vp.time - reftime);
    idx++;
  }
  zeroDopplerTimeFirstRef=(zeroDopplerTimeFirstLineSeconds - reftime);
  zeroDopplerTimeLastRef=(zeroDopplerTimeLastLineSeconds - reftime);
  double minT=0;
  double maxT=0;
  double zeroDopplerTimeFirstLineWSafetyBufSecondsRef=0;
  double zeroDopplerTimeLastLineWSafetyBufSecondsRef=0;
  if (zeroDopplerTimeFirstRef < zeroDopplerTimeLastRef) {
    zeroDopplerTimeFirstLineWSafetyBufSecondsRef=zeroDopplerTimeFirstRef - iSafetyBufferAz * deltaT;
    zeroDopplerTimeLastLineWSafetyBufSecondsRef=zeroDopplerTimeLastRef + iSafetyBufferAz * deltaT;
    minT=FastMath.min(0,zeroDopplerTimeFirstLineWSafetyBufSecondsRef);
    maxT=FastMath.max(secondsDiffFromRefTime[secondsDiffFromRefTime.length - 1],zeroDopplerTimeLastLineWSafetyBufSecondsRef);
  }
 else {
    zeroDopplerTimeFirstLineWSafetyBufSecondsRef=zeroDopplerTimeFirstRef + iSafetyBufferAz * deltaT;
    zeroDopplerTimeLastLineWSafetyBufSecondsRef=zeroDopplerTimeLastRef - iSafetyBufferAz * deltaT;
    if (firstTime < (lastTime)) {
      minT=FastMath.min(secondsDiffFromRefTime[0],zeroDopplerTimeLastLineWSafetyBufSecondsRef);
      maxT=FastMath.max(secondsDiffFromRefTime[secondsDiffFromRefTime.length - 1],zeroDopplerTimeFirstLineWSafetyBufSecondsRef);
    }
 else {
      minT=FastMath.min(secondsDiffFromRefTime[secondsDiffFromRefTime.length - 1],zeroDopplerTimeLastLineWSafetyBufSecondsRef);
      maxT=FastMath.max(secondsDiffFromRefTime[0],zeroDopplerTimeFirstLineWSafetyBufSecondsRef);
    }
  }
  int limit=((Double)((maxT - minT) / deltaT)).intValue();
  Double[] timeStampInterpSecondsRef=new Double[limit + 1];
  double nextVal=0;
  for (int j=0; j <= limit; j++) {
    timeStampInterpSecondsRef[j]=nextVal;
    nextVal=deltaT + nextVal;
  }
  double zeroDopplerTimeInitRef=zeroDopplerTimeFirstLineWSafetyBufSecondsRef;
  double zeroDopplerTimeEndRef=zeroDopplerTimeLastLineWSafetyBufSecondsRef;
  if (zeroDopplerTimeFirstLineWSafetyBufSecondsRef > zeroDopplerTimeLastLineWSafetyBufSecondsRef) {
    zeroDopplerTimeInitRef=zeroDopplerTimeLastLineWSafetyBufSecondsRef;
    zeroDopplerTimeEndRef=zeroDopplerTimeFirstLineWSafetyBufSecondsRef;
  }
  if (zeroDopplerTimeInitRef < timeStampInterpSecondsRef[1]) {
    int size=Math.min(nPoints,N_ORB_POINT_INTERP);
    double[] subTimesDiffRef=Arrays.copyOfRange(secondsDiffFromRefTime,0,nPoints);
    List<Double> timeStampInitSecondsRefPointsInterp=new ArrayList<Double>();
    for (int i=0; i < size; i++) {
      double valMax=timeStampInterpSecondsRef[size - 1];
      if (timeStampInterpSecondsRef[i] < valMax) {
        timeStampInitSecondsRefPointsInterp.add(timeStampInterpSecondsRef[i]);
      }
    }
    List<IMetadata.OrbitStatePosVelox> subList=vpList.subList(0,size);
    double initTime=zeroDopplerTimeInitRef;
    int idxInitTime=-1;
    boolean finded=false;
    double endTime=secondsDiffFromRefTime[0];
    int idxEndTime=-1;
    for (int i=0; i < timeStampInterpSecondsRef.length; i++) {
      if (timeStampInterpSecondsRef[i] >= initTime && !finded) {
        idxInitTime=i;
        finded=true;
      }
      if (timeStampInterpSecondsRef[i] < endTime) {
        idxEndTime=i;
      }
    }
    logger.debug("String_Node_Str" + idxInitTime + "String_Node_Str"+ idxEndTime);
    Double[] t0=(Double[])timeStampInitSecondsRefPointsInterp.toArray(new Double[0]);
    if (idxInitTime != -1 && idxEndTime != -1 && initTime < endTime) {
      List<double[]> vPoints=new ArrayList<double[]>();
      List<double[]> pPoints=new ArrayList<double[]>();
      List<Double> timeStampOutput=new ArrayList<Double>();
      HermiteInterpolation.interpolation(subTimesDiffRef,subList,t0,idxInitTime,idxEndTime,deltaTinv,vPoints,pPoints,timeStampOutput);
      addPointsToResult(vPoints,pPoints);
      if (timeStampInterp == null)       timeStampInterp=new ArrayList<>(timeStampOutput);
 else       timeStampInterp.addAll(timeStampOutput);
    }
  }
  for (int orbPoint=1; orbPoint < nPoints; orbPoint++) {
    if (secondsDiffFromRefTime[orbPoint - 1] < zeroDopplerTimeEndRef || secondsDiffFromRefTime[orbPoint] > zeroDopplerTimeEndRef) {
      int idxStart=0;
      int idxEnd=0;
      if (N_ORB_POINT_INTERP > nPoints) {
        idxEnd=nPoints;
      }
 else {
        idxStart=orbPoint - new Double(Math.floor(N_ORB_POINT_INTERP / 2)).intValue();
        idxEnd=idxStart + N_ORB_POINT_INTERP - 1;
        if (idxStart < 1) {
          idxStart=0;
          idxEnd=N_ORB_POINT_INTERP - 1;
        }
        if (idxEnd >= nPoints) {
          idxStart=nPoints - N_ORB_POINT_INTERP;
          idxEnd=nPoints - 1;
        }
      }
      double[] timeStampInitSecondsRefPoints=Arrays.copyOfRange(secondsDiffFromRefTime,idxStart,idxEnd + 1);
      List<Double> timeStampInitSecondsRefPointsInterp=new ArrayList<Double>();
      for (int i=0; i < timeStampInterpSecondsRef.length; i++) {
        if (timeStampInterpSecondsRef[i] >= secondsDiffFromRefTime[idxStart] && timeStampInterpSecondsRef[i] < secondsDiffFromRefTime[idxEnd]) {
          timeStampInitSecondsRefPointsInterp.add(timeStampInterpSecondsRef[i]);
        }
      }
      List<S1Metadata.OrbitStatePosVelox> stateVecPoints=vpList.subList(idxStart,idxEnd + 1);
      double initTime=Math.max(secondsDiffFromRefTime[orbPoint - 1],zeroDopplerTimeInitRef);
      int idxInitTime=0;
      boolean findIdxInitTime=false;
      for (int i=0; i < timeStampInitSecondsRefPointsInterp.size() && !findIdxInitTime; i++) {
        if (timeStampInitSecondsRefPointsInterp.get(i) >= initTime) {
          idxInitTime=i;
          findIdxInitTime=true;
        }
      }
      double endTime=Math.min(secondsDiffFromRefTime[orbPoint],zeroDopplerTimeEndRef);
      int idxEndTime=0;
      boolean findIdxEndTime=false;
      for (int i=timeStampInitSecondsRefPointsInterp.size() - 1; i >= 0 && !findIdxEndTime; i--) {
        if (timeStampInitSecondsRefPointsInterp.get(i) < endTime) {
          idxEndTime=i;
          findIdxEndTime=true;
        }
      }
      double timeRef=timeStampInitSecondsRefPointsInterp.get(0);
      Double[] timeStampInitSecondsRefPointsInterp0=new Double[timeStampInitSecondsRefPointsInterp.size()];
      for (int i=0; i < timeStampInitSecondsRefPointsInterp.size(); i++) {
        timeStampInitSecondsRefPointsInterp0[i]=timeStampInitSecondsRefPointsInterp.get(i) - timeRef;
      }
      if (findIdxEndTime && findIdxInitTime && initTime < endTime && idxInitTime < idxEndTime) {
        List<double[]> vPoints=new ArrayList<double[]>();
        List<double[]> pPoints=new ArrayList<double[]>();
        List<Double> timeStampOutput=new ArrayList<Double>();
        HermiteInterpolation.interpolation(timeStampInitSecondsRefPoints,stateVecPoints,timeStampInitSecondsRefPointsInterp0,idxInitTime,idxEndTime,deltaTinv,pPoints,vPoints,timeStampOutput);
        addPointsToResult(vPoints,pPoints);
        List<Double> timeStampInterpSecondsRefOutput=new ArrayList<Double>();
        for (int i=0; i < timeStampOutput.size(); i++) {
          timeStampInterpSecondsRefOutput.add(timeStampOutput.get(i) + timeRef);
        }
        if (timeStampInterp == null)         timeStampInterp=new ArrayList<>(timeStampInterpSecondsRefOutput);
 else         timeStampInterp.addAll(timeStampInterpSecondsRefOutput);
      }
    }
  }
  if (zeroDopplerTimeEndRef > secondsDiffFromRefTime[secondsDiffFromRefTime.length - 1]) {
    int vOrbPointsInit=(nPoints - Math.min(nPoints,N_ORB_POINT_INTERP));
    int vOrbPointsEnd=nPoints - 1;
    double[] timeStampInitSecondsRefPoints=Arrays.copyOfRange(secondsDiffFromRefTime,vOrbPointsInit,vOrbPointsEnd + 1);
    List<Double> timeStampInitSecondsRefPointsInterp=new ArrayList<Double>();
    for (int i=0; i < timeStampInterpSecondsRef.length; i++) {
      if (timeStampInterpSecondsRef[i] >= secondsDiffFromRefTime[vOrbPointsInit]) {
        timeStampInitSecondsRefPointsInterp.add(timeStampInterpSecondsRef[i]);
      }
    }
    List<S1Metadata.OrbitStatePosVelox> stateVecPoints=vpList.subList(vOrbPointsInit,vOrbPointsEnd + 1);
    double initTime=secondsDiffFromRefTime[vOrbPointsEnd];
    int idxInitTime=0;
    boolean findIdxInitTime=false;
    for (int i=0; i < timeStampInitSecondsRefPointsInterp.size() && !findIdxInitTime; i++) {
      if (timeStampInitSecondsRefPointsInterp.get(i) >= initTime) {
        idxInitTime=i;
        findIdxInitTime=true;
      }
    }
    double endTime=zeroDopplerTimeEndRef;
    int idxEndTime=0;
    boolean findIdxEndTime=false;
    for (int i=timeStampInitSecondsRefPointsInterp.size() - 1; i >= 0 && !findIdxEndTime; i--) {
      if (timeStampInitSecondsRefPointsInterp.get(i) < endTime) {
        idxEndTime=i;
        findIdxEndTime=true;
      }
    }
    double timeRef=timeStampInitSecondsRefPointsInterp.get(0);
    Double[] timeStampInitSecondsRefPointsInterp0=new Double[timeStampInitSecondsRefPointsInterp.size()];
    for (int i=0; i < timeStampInitSecondsRefPointsInterp.size(); i++) {
      timeStampInitSecondsRefPointsInterp0[i]=timeStampInitSecondsRefPointsInterp.get(i) - timeRef;
    }
    if (findIdxEndTime && findIdxInitTime && initTime < endTime) {
      List<double[]> vPoints=new ArrayList<double[]>();
      List<double[]> pPoints=new ArrayList<double[]>();
      List<Double> timeStampOutput=new ArrayList<Double>();
      HermiteInterpolation.interpolation(timeStampInitSecondsRefPoints,stateVecPoints,timeStampInitSecondsRefPointsInterp0,idxInitTime,idxEndTime,deltaTinv,pPoints,vPoints,timeStampOutput);
      addPointsToResult(vPoints,pPoints);
      List<Double> timeStampInterpSecondsRefOutput=new ArrayList<Double>();
      for (int i=0; i < timeStampOutput.size(); i++) {
        timeStampInterpSecondsRefOutput.add(timeStampOutput.get(i) + timeRef);
      }
      if (timeStampInterp == null)       timeStampInterp=new ArrayList<>(timeStampInterpSecondsRefOutput);
 else       timeStampInterp.addAll(timeStampInterpSecondsRefOutput);
    }
  }
}