Interpolasi

public double[] dataInterpolasi(List<DataCSV> listJarak, List<DataCSV> listPercepatan){
        float jarakMax = pref.getFloat("runDistance", 0f);
        double t = 0.0;
        double[] result = new double[5];
        int i = 0;
        double d1=0.0, d2, t1 = 0.0,t2;
        for(i = 0; i <listJarak.size(); i++){
            d2 = Math.abs(listJarak.get(i).getZ());
            t2 = listJarak.get(i).getTimestamp();
            if (i >0){
                d1 = Math.abs(listJarak.get(i-1).getZ());
                t1 = listJarak.get(i-1).getTimestamp();
            }
            if ( d2 > jarakMax){
                if (i==0){
                    t = 0+((jarakMax-0)/(d2-0))*(t2-0);//(listdata.get(i).getTimestamp()-0))*set_point;
                }else {
                    t = t1+ (jarakMax-d1)/(d2- d1)* (t2-t1);
                }
                break;
            }else if (d2 == jarakMax){
                t = t2;
                break;
            }else if (d2 < jarakMax && i == listJarak.size()-1){
                t = t2 + (jarakMax-d1)/(d2- d1)* (t2-t1);
                break;
            }
        }
        //mencari percepatannya pada data setpoint
        double setPointWaktu = t, a = 0.0;
        if (listPercepatan.get(i).getTimestamp() > setPointWaktu){
            if (i==0){
                a = 0+((setPointWaktu-0)/(listPercepatan.get(i).getTimestamp()-0))*
                        (listPercepatan.get(i).getZ()-0);
            }else {
                a = listPercepatan.get(i-1).getZ()+
                        (setPointWaktu-listPercepatan.get(i-1).getTimestamp())/(listPercepatan.get(i).getTimestamp() - listPercepatan.get(i-1).getTimestamp())*
                                (listPercepatan.get(i).getZ()-listPercepatan.get(i-1).getZ());
            }
        }else if (listPercepatan.get(i).getZ() == setPointWaktu){
            a = listPercepatan.get(i).getZ();
        }else if (listPercepatan.get(i).getZ() < setPointWaktu && i== listPercepatan.size()-1){
            a = listPercepatan.get(i).getZ()+
                    (setPointWaktu-listPercepatan.get(i-1).getTimestamp())/(listPercepatan.get(i).getTimestamp() - listPercepatan.get(i-1).getTimestamp())*
                            (listPercepatan.get(i).getZ()-listPercepatan.get(i-1).getZ());
        }
//        Log.i("interpolasi", i +" "+t+" "+listPercepatan.get(i).getTimestamp()+" "+a+" "+dataAsli.size());
        result[0] = jarakMax;//jarak
        result[1] = t;//waktu pada jarak
        result[2] = a;//percepatan pada waktu dan jarak
        result[3] = a*t;//kecepatan
        result[4] = i;//posisi index
        dataAsli.get(i).setTimestamp(t);
        dataAsli.get(i).setZ(a);
        this.listjarak.get(i).setTimestamp(t);
        this.listjarak.get(i).setZ(Double.valueOf(jarakMax));
        listkecepatan.get(i).setTimestamp(t);
        listkecepatan.get(i).setZ(result[3]);
        for (int c = i+1; c < dataAsli.size();){
            dataAsli.remove(c);
            listjarak.remove(c);
            listkecepatan.remove(c);
        }
        Log.i("interpolasi", i +" "+t+" "+listPercepatan.get(i).getTimestamp()+" "+a+" "+dataAsli.size());
        return result;
    }