/*******************************************************************************
 * Copyright (c) 2014, 2015 IBM Corporation
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *******************************************************************************/

package hulo.localization.synthe;

import hulo.localization.Location;
import hulo.localization.Sample;
import hulo.localization.sensor.SensorData;
import hulo.localization.utils.JSONUtils;

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

import org.apache.wink.json4j.JSONException;
import org.apache.wink.json4j.JSONObject;

/**
 * This class generates synthetic motion data (Accelerometer and gyroscope data.)
 */
public class SyntheticMotionDataGenerator {

	List<Sample> samples;

	Random rand = new Random();

	double[] accStds = {0.0, 0.0, 0.0};
	double[] attStds = {0.0, 0.0, 0.0};
	double orientationZeroPoint = 0;

	int freq = 100;
	int dTS = 1000/freq;

	double oriBias = 0;
	double diffOriBias = 0.01;

	static final double G = 9.8;
	double ampG = 2.5;

	double stepPerSec = 2.0;

	static final String ACC = "Acc";
	static final String MOTION = "Motion";
	static final String MOVING = "Moving";

	public SyntheticMotionDataGenerator(){}

	public SyntheticMotionDataGenerator(JSONObject json){
		setJSON(json);
	}

	public SyntheticMotionDataGenerator setRandom(Random rand){
		this.rand = rand;
		return this;
	}

	SyntheticMotionDataGenerator setJSON(JSONObject json){
		try{
			accStds = JSONUtils.array1DfromJSONArray(json.getJSONArray("accStds"));
			attStds = JSONUtils.array1DfromJSONArray(json.getJSONArray("attStds"));
			freq = json.getInt("freq");
			dTS = 1000/freq;
			ampG = json.getDouble("ampG");
			stepPerSec = json.getDouble("stepPerSec");
		}catch(JSONException e){
			e.printStackTrace();
		}
		return this;
	}

	class LocationTime extends Location{
		long timestamp;

		double vx = 0;
		double vy = 0;
		double vabs = 0;
		double orientation = 0;

		boolean isWalking = false;

		LocationTime(long timestamp, double x, double y, double z){
			super(x,y,z);
			this.timestamp = timestamp;
		}

		public LocationTime setTimestamp(long timestamp){
			this.timestamp = timestamp;
			return this;
		}

		public long getTimestamp(){
			return timestamp;
		}

		@Override
		public String toString(){
			return timestamp+","+super.toString()+","+vabs+","+orientation;
		}

	}

	public SyntheticMotionDataGenerator setWalkingSamples(List<Sample> samples){
		this.samples = samples;
		return this;
	}

	public List<Sample> generate(){

		List<LocationTime> locTimes = interpolateLocations(samples, dTS);

		List<SensorData> motions = genMotion(locTimes);
		List<SensorData> accs = genMotionAcc(locTimes);
		List<SensorData> movings = genMovings(locTimes);

		List<SensorData> sensors = mergeSensors(accs, movings);
		sensors = mergeSensors(sensors, motions);

		List<Sample> samplesNew = merge(samples, sensors);

		return samplesNew;
	}



	List<SensorData> genMotionAcc(List<LocationTime> locTimes){
		List<SensorData> sensors = new ArrayList<>();

		LocationTime loc0 = locTimes.get(0);
		long t0 = loc0.getTimestamp();

		for(LocationTime locT: locTimes){
			long t = locT.getTimestamp();
			if(! locT.isWalking){
				t0 = t;
			}
			// Accelerations in x and y axes are ignored although it is incorrect assumption.
			double ax = 0.0 + accStds[0]*rand.nextGaussian();
			double ay = 0.0 + accStds[1]*rand.nextGaussian();
			// Acceleration in z axis is generated by a sine curve.
			double az = ( -G +  ampG*getUnitAmpZ(t0, t, stepPerSec) ) /G +  attStds[2]*rand.nextGaussian();
			double[] data = {ax, ay, az};
			SensorData s = new SensorData();
			s.setTimestamp(t);
			s.setType(ACC);
			s.setData(data);

			sensors.add(s);
		}

		return sensors;
	}

	List<SensorData> genMovings(List<LocationTime> locTimes){
		List<SensorData> sensors = new ArrayList<>();

		for(LocationTime locT: locTimes){
			long t = locT.getTimestamp();
			int isMoving = locT.isWalking? 1 : 0;
			double[] data = {isMoving};
			SensorData s = new SensorData();
			s.setTimestamp(t);
			s.setType(MOVING);
			s.setData(data);

			sensors.add(s);
		}

		return sensors;
	}


	double getUnitAmpZ(long tstart, long t, double stepPerSec){
		double tScaled = stepPerSec*(t-tstart)/1000;
		double unitAmpZ = Math.sin(2*Math.PI*tScaled);
		return unitAmpZ;
	}


	List<SensorData> genMotion(List<LocationTime> locTimes){

		List<SensorData> sensors = new ArrayList<>();

		for(LocationTime locT: locTimes){
			long t = locT.getTimestamp();
			double pitch = attStds[0]*rand.nextGaussian();
			double roll = attStds[1]*rand.nextGaussian();
			double yaw = locT.orientation + orientationZeroPoint + attStds[2]*rand.nextGaussian();

			double[] data = {pitch, roll, yaw};
			SensorData s = new SensorData();
			s.setTimestamp(t);
			s.setType(MOTION);
			s.setData(data);

			sensors.add(s);
		}
		return sensors;
	}



	List<LocationTime> interpolateLocations(List<Sample> samples, int interval){
		int n = samples.size();

		List<LocationTime> locTimesNew = new ArrayList<>();

		for(int i=0; i<n-1; i++){
			Sample s0 = samples.get(i);
			Sample s1 = samples.get(i+1);

			long t0 = s0.getTimeStamp();
			Location l0 = s0.getLocation();
			long t1 = s1.getTimeStamp();
			Location l1 = s1.getLocation();

			double x0 = l0.getX();
			double y0 = l0.getY();
			double z0 = l0.getZ();
			double x1 = l1.getX();
			double y1 = l1.getY();

			long tDiff = t1-t0;
			int m = (int) (tDiff/interval);
			for(int j=0; j<m; j++){
				long t = t0 + interval*j;
				double x = x0 + (x1-x0)/m*j;
				double y = y0 + (y1-y0)/m*j;
				double z = z0;

				double vx = (x1-x0)*(1000/tDiff); // m/s
				double vy = (y1-y0)*(1000/tDiff); // m/s
				double vabs = Math.sqrt(vx*vx + vy*vy);
				double ori = Math.atan2(vy, vx);

				LocationTime loc = new LocationTime(t,x,y,z);
				loc.vx = vx;
				loc.vy = vy;
				loc.vabs = vabs;
				loc.orientation = ori;
				loc.isWalking = vabs==0? false : true;
				locTimesNew.add(loc);
			}
		}
		return locTimesNew;
	}

	public List<Sample> generate(List<Sample> samples){
		return setWalkingSamples(samples).generate();
	}


	public List<SensorData> mergeSensors(List<SensorData> sensors1, List<SensorData> sensors2){
		List<SensorData> sensorsnew = new ArrayList<>();

		int i=0;
		int j=0;
		while(i<=sensors1.size()-1 || j<=sensors2.size()-1){
			if(i>sensors1.size()-1){
				SensorData s2 = sensors2.get(j);
				sensorsnew.add(s2);
				j++;
				continue;
			}else if(j>sensors2.size()-1){
				SensorData s1 = sensors1.get(i);
				sensorsnew.add(s1);
				i++;
				continue;
			}
			SensorData s1 = sensors1.get(i);
			SensorData s2 = sensors2.get(j);

			long t1 = s1.getTimestamp();
			long t2 = s2.getTimestamp();

			if(t1<=t2){
				sensorsnew.add(s1);
				i++;
			}else{
				sensorsnew.add(s2);
				j++;
			}
		}
		return sensorsnew;
	}

	public List<Sample> merge(List<Sample> samples, List<SensorData> sensorDataList){

		int n = samples.size();
		int m = sensorDataList.size();

		List<Sample> samplesNew = new ArrayList<>();

		int j_memo = 0;
		for(int i=0; i<n; i++){
			List<SensorData> sensors = new ArrayList<>();
			Sample smp = samples.get(i);
			long t = smp.getTimeStamp();
			for(int j=j_memo; j<m; j++){
				SensorData sen = sensorDataList.get(j);
				if(sen.getTimestamp() < t){
					sensors.add(sen);
				}else{
					j_memo = j;
					break;
				}
			}
			if(sensors.size()!=0){
				smp.setSensorDataList(sensors);
				samplesNew.add(smp);
			}
		}
		return samplesNew;
	}

	public void print(){
		for(Sample smp: samples){
			long t = smp.getTimeStamp();
			Location loc = smp.getLocation();
			System.out.println("time="+t+", location="+loc);
		}
	}
}
