package jp.hasc.hasctool.core.runtime.filter.interpolator;

import jp.hasc.hasctool.core.data.SignalMessage;
import jp.hasc.hasctool.core.data.Spline;
import jp.hasc.hasctool.core.data.VectorSignalMessage;
import jp.hasc.hasctool.core.data.VectorSignalMessages;

/**
 * ベクトルをspline補間して返すSignalInterpolatorです
 * @yang
 */
public class SplineVectorInterporator implements SignalInterpolator {

	private VectorSignalMessage v0 = null,v1 = null, v2 = null, v3 = null;
	private int vsize;


	@Override
	public boolean addSample(SignalMessage message) {
		if (message instanceof VectorSignalMessage) {
			v0 = v1;
			v1 = v2;
			v2 = v3;
			v3 = (VectorSignalMessage) message;	
//			System.out.println("AddSample:"+v3.getTime()+","+v3.getVectorElement(0));
			return true;
		}else{
			return false;
		}
	}

	@Override
	public SignalMessage getAtTime(long time) {
		
		if(v3 == null) return null;
//		if(v2 == null) return null;
//		if(v1 == null) return null;
//		if(v0 == null) return null;
		
		if(v2 == null){
			System.out.println("V2null");
				return null;
		}
		if(v1 == null){
			long vd = v3.getTime()-v2.getTime();
			v1= (VectorSignalMessage)v2.createDelayedElement(-vd);
		}
		if(v0 == null){
			long vd = v2.getTime()-v1.getTime();
			v0= (VectorSignalMessage)v1.createDelayedElement(-vd);
		}
		
		
	
		
		
		vsize = Math.min(v0.getVectorSize(), v1.getVectorSize());
		vsize = Math.min(vsize, v2.getVectorSize());
		vsize = Math.min(vsize, v3.getVectorSize());
		
		
		double[] vectorTime = new double[]{v0.getTime(),v1.getTime(), v2.getTime(), v3.getTime()};
		
		if (vsize==1) {			
			double[] vector = new double[]{v0.getVectorElement(0),v1.getVectorElement(0), v2.getVectorElement(0), v3.getVectorElement(0)};
//			System.out.print("Vec[");
//			for (int i = 0; i < vector.length;i++){ System.out.print("("+vectorTime[i]+","+vector[i]+") "); }
//			System.out.println("]");
			Spline sp = new Spline(vectorTime, vector); 
			return VectorSignalMessages.createScalar(time, sp.map(time));
		}else{
			double[] vals = new double[vsize];
			for(int i = 0;i < vsize;i++){
				double[] vector = new double[]{v0.getVectorElement(i),v1.getVectorElement(i), v2.getVectorElement(i), v3.getVectorElement(i)};			
				Spline sp = new Spline(vectorTime, vector); 
				vals[i] = sp.map(time);
			}

			return VectorSignalMessages.create(time, vals, false);
		}
	}
}
