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

import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;

import jp.hasc.hasctool.core.runtime.RuntimeContext;
import jp.hasc.hasctool.core.runtime.filter.AbstractFilter;
import jp.hasc.hasctool.core.util.CoreUtil;
import jp.hasc.hasctool.core.data.EnumCommand;
import jp.hasc.hasctool.core.data.ScalarSignalMessage;
import jp.hasc.hasctool.core.data.VectorSignalMessage;
import jp.hasc.hasctool.core.data.VectorSignalMessages;

public class SimpleDeadReconing extends AbstractFilter {
	
	private long[] stepTimes_;
	private double error_;
	private String sorce_;
	
	private long before = 0;
	private long now = 0;
	double x,y;
	double radian;
	double sum;
	int count;
	int step;
	
	
	String metaFilePath_;
	String imgPath_;
	double stride_ = 1;
	double initialDirection_ = 0.0;
	double initialX_ = 0.0;
	double initialY_ = 0.0;
	
	public void setStepTimes (String stepTimesString) {
		if (stepTimesString.indexOf(",") > 0) {
			String[] splitedStepTimesString = stepTimesString.split(",");
			stepTimes_ = new long[splitedStepTimesString.length];
			for(int i = 0; i < stepTimes_.length; i++) {
				stepTimes_[i] = Long.parseLong(splitedStepTimesString[i]);
			}		
		}
		
	}
	
	public void setMetaFilePath (String filePath) {
		metaFilePath_ = filePath;
	}
	
	public void setError (double error) {
		error_ = error;
	}
	
	public void setInitialDirection (double direction) {
		initialDirection_ = direction;
	}
	
	public void setSorce (String sorce) {
		sorce_ = sorce;
	}
	
	public void setStride (double stride) {
		stride_ = stride; 
	}

	public void setInitialX (double initial_x) {
		initialX_ = initial_x; 
	}
	
	public void setInitialY (double initial_y) {
		initialY_ = initial_y; 
	}
	
	public void setTargetImgPath (String imgPath) {
		imgPath_ = imgPath; 
	}
	
	@Override
	public void processMessage(Object message) throws InterruptedException {
		if (message instanceof VectorSignalMessage) {
			if (step < stepTimes_.length) {
				VectorSignalMessage vsm = (VectorSignalMessage)message;
				before = now;
				now = vsm.getTime();
				if (sorce_.equals("gyro")) {
					double dt = (double)((now - before)*(1e-6));
					radian += vsm.getVectorElement(1) * dt;
					sum += radian;
					count++;
				} else {
					double dx = vsm.getVectorElement(2);
					double dy = vsm.getVectorElement(0);
					radian = Math.atan2(dy, dx);
					sum += radian;
					count++;
				}

				if(now > stepTimes_[step]) {
					double angle;
					angle = sum / count;
					x += Math.cos(angle - error_)*stride_;
					y -= Math.sin(angle - error_)*stride_;
					
					double[] vec = {x, y};
					
					outputMessage(VectorSignalMessages.create(stepTimes_[step], vec));
//					System.out.println(x + "," + y);
					if (step < stepTimes_.length) {
						step++;
					}
					count = 0;
					sum = 0;
				}
			}
		} else if (message == EnumCommand.BEGIN){
			if (metaFilePath_ != null) {
				readMetaFile();
			}
			x = initialX_;
			y = initialY_;
			before = 0;
			now = 0;
			count = 0;
			radian = initialDirection_;
			step = 0;
			sum = 0;
			outputMessage(message);
			double[] vec = {x, y};
			outputMessage("#targetImg:" + imgPath_);
			outputMessage(VectorSignalMessages.create(0, vec));
		} else {
			outputMessage(message);
		}
	}
	
	public void readMetaFile() {
		try {
			InputStream inps = getRuntimeContext().getFileStreamProvider().openInputStream(metaFilePath_);
			BufferedReader bufferedReader = new BufferedReader(new InputStreamReader(inps, RuntimeContext.DEFAULT_CHARSET));
			while(true){
				String line = bufferedReader.readLine();
				if (line == null) {
					break;
				} else {
					String[] args = line.split(":");
					if (args[0].indexOf("InitialPoint") >= 0) {
						String[] coordinate = args[1].split(",");
						setInitialX(Double.parseDouble(coordinate[0].trim()));
						setInitialY(Double.parseDouble(coordinate[1].trim()));
					} else if (args[0].indexOf("TargetImg") >= 0) {
						if(args[1].length() > 0) setTargetImgPath(args[1].trim());
					} else if (args[0].indexOf("Stride") >= 0) {
						setStride(Double.parseDouble(args[1].trim()));
					} else if (args[0].indexOf("InitialDirection") >= 0) {
						setInitialDirection(Double.parseDouble(args[1].trim()));
					} else if (args[0].indexOf("Error") >= 0) {
						setError(Double.parseDouble(args[1].trim()));
					} 
				}
			}
			bufferedReader.close();
			inps.close();
		} catch (IOException e) {
			CoreUtil.throwAsRuntimeException(e);
		}
		
	}

}
