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

import jp.hasc.hasctool.core.data.VectorSignalMessage;
import jp.hasc.hasctool.core.data.VectorSignalMessages;
import jp.hasc.hasctool.core.runtime.RuntimeContext;

/**
 * センサの傾きを返すフィルタです
 * ベクトルの内積を利用しています
 * @author hiro
 */

public class AngleVariationFilter extends AbstractFilter {

	private long[] time_;//入力時間
	private double[][] sampleData_;//入力データ
	private int length_;//各chにおけるsampleDataの個数
	private int next_;//現在の位置
	private boolean fill_;

	@Override
	public void setup(RuntimeContext context) {
		super.setup(context);
		time_=new long[length_];
		sampleData_=new double[1][length_];//初期設定は1ch
		next_ = 0;
		fill_ = false;
	}

	public void setSampleNum(int samplenum) {
		length_=samplenum;
	}

	private void createData(double[][] valAry, int index){
		sampleData_=new double[index][length_];
		if(index > valAry.length){
			for(int i=0; i<valAry.length; i++){
				for(int j=0; j<length_; j++){
					sampleData_[i][j] = valAry[i][j];
				}
			}
		} else {
			for(int i=0; i<index; i++){
				for(int j=0; j<length_; j++){
					sampleData_[i][j] = valAry[i][j];
				}
			}
		}
	}

	private void setTime(long time){
		time_[next_] = time;	
	}

	private void setData(double val, int index){
		sampleData_[index][next_]=val;	
	}

	private double getAngle(int index ,int indexLen){
		int past = (next_+length_-1)%length_;
		int index2 = (index+1) % indexLen;
		double a1 = sampleData_[index][next_];
		double a2 = sampleData_[index2][next_];
		double b1 = sampleData_[index][past];
		double b2 = sampleData_[index2][past];
		double sp = a1*b1 + a2*b2;
		double aAbs = Math.sqrt(a1*a1 + a2*a2);
		double bAbs = Math.sqrt(b1*b1 + b2*b2);
		return Math.acos(sp/(aAbs*bAbs));
	}

	@Override
	public void processMessage(Object message) throws InterruptedException {
		if (message instanceof VectorSignalMessage) {
			// VectorSignalMessageの場合
			VectorSignalMessage vsig=(VectorSignalMessage)message;
			if(vsig.getVectorSize() != sampleData_.length){
				createData(sampleData_, vsig.getVectorSize());
			}
			int len = sampleData_.length;
			for(int i=0; i<len; ++i){
				setTime(vsig.getTime());
				setData(vsig.getVectorElement(i),i);
			}
			next_ = (next_+1)%length_;
			if(next_ == 0 && !(fill_)){
				fill_ = true;
			}// 結果を出力
			if(fill_){
				long stime = time_[next_];
				double[] newValue=new double[len];
				for(int i=0; i<len; i++){
					newValue[i] = getAngle(i, len);
				}
				outputMessage(VectorSignalMessages.create(stime, newValue));
			}
		}else{
			// その他のメッセージ（BEGIN, ENDなど）はそのまま出力
			outputMessage(message);
		}
	}
}
