package com.amap.api.maps.utils;

import com.amap.api.maps.AMapUtils;
import com.amap.api.maps.model.LatLng;

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

/**
 * 轨迹优化工具类
 * Created by my94493 on 2017/3/31.
 * <p>
 * 使用方法：
 * <p>
 *     PathSmoothTool pathSmoothTool = new PathSmoothTool();
 *     pathSmoothTool.setIntensity(2);//设置滤波强度，默认3
 *     List<LatLng> mList = LatpathSmoothTool.kalmanFilterPath(list);
 */

public class PathSmoothTool {
    private int mIntensity = 3;
    private float mThreshhold = 0.3f;
    private float mNoiseThreshhold = 10;

    public PathSmoothTool(){

    }

    public int getIntensity() {
        return mIntensity;
    }

    public void setIntensity(int mIntensity) {
        this.mIntensity = mIntensity;
    }

    public float getThreshhold() {
        return mThreshhold;
    }

    public void setThreshhold(float mThreshhold) {
        this.mThreshhold = mThreshhold;
    }

    public void setNoiseThreshhold(float mnoiseThreshhold) {
        this.mNoiseThreshhold = mnoiseThreshhold;
    }

    /**
     * 轨迹平滑优化
     * @param originlist 原始轨迹list,list.size大于2
     * @return 优化后轨迹list
     */
    public List<LatLng> pathOptimize(List<LatLng> originlist){

        List<LatLng> list = removeNoisePoint(originlist);//去噪
        List<LatLng> afterList = kalmanFilterPath(list,mIntensity);//滤波
        List<LatLng> pathoptimizeList = reducerVerticalThreshold(afterList,mThreshhold);//抽稀
//        Log.i("MY","originlist: "+originlist.size());
//        Log.i("MY","list: "+list.size());
//        Log.i("MY","afterList: "+afterList.size());
//        Log.i("MY","pathoptimizeList: "+pathoptimizeList.size());
        return pathoptimizeList;
    }

    /**
     * 轨迹线路滤波
     * @param originlist 原始轨迹list,list.size大于2
     * @return 滤波处理后的轨迹list
     */
    public List<LatLng> kalmanFilterPath(List<LatLng> originlist) {
        return kalmanFilterPath(originlist,mIntensity);
    }


    /**
     * 轨迹去噪，删除垂距大于20m的点
     * @param originlist 原始轨迹list,list.size大于2
     * @return
     */
    public List<LatLng> removeNoisePoint(List<LatLng> originlist){
        return reduceNoisePoint(originlist,mNoiseThreshhold);
    }

    /**
     * 单点滤波
     * @param lastLoc 上次定位点坐标
     * @param curLoc 本次定位点坐标
     * @return 滤波后本次定位点坐标值
     */
    public LatLng kalmanFilterPoint(LatLng lastLoc, LatLng curLoc) {
        return kalmanFilterPoint(lastLoc,curLoc,mIntensity);
    }

    /**
     * 轨迹抽稀
     * @param inPoints 待抽稀的轨迹list，至少包含两个点，删除垂距小于mThreshhold的点
     * @return 抽稀后的轨迹list
     */
    public List<LatLng> reducerVerticalThreshold(List<LatLng> inPoints) {
        return reducerVerticalThreshold(inPoints,mThreshhold);
    }

    /********************************************************************************************************/
    /**
     * 轨迹线路滤波
     * @param originlist 原始轨迹list,list.size大于2
     * @param intensity 滤波强度（1—5）
     * @return
     */
    private List<LatLng> kalmanFilterPath(List<LatLng> originlist,int intensity) {
        List<LatLng> kalmanFilterList = new ArrayList<LatLng>();
        if (originlist == null || originlist.size() <= 2)
            return kalmanFilterList;
        initial();//初始化滤波参数
        LatLng latLng = null;
        LatLng lastLoc = originlist.get(0);
        kalmanFilterList.add(lastLoc);
        for (int i = 1; i < originlist.size(); i++) {
            LatLng curLoc = originlist.get(i);
            latLng = kalmanFilterPoint(lastLoc,curLoc,intensity);
            if (latLng != null) {
                kalmanFilterList.add(latLng);
                lastLoc = latLng;
            }
        }
        return kalmanFilterList;
    }

    /**
     * 单点滤波
     * @param lastLoc 上次定位点坐标
     * @param curLoc 本次定位点坐标
     * @param intensity 滤波强度（1—5）
     * @return 滤波后本次定位点坐标值
     */
    private LatLng kalmanFilterPoint(LatLng lastLoc, LatLng curLoc, int intensity) {
        if (pdelt_x == 0 || pdelt_y == 0 ){
            initial();
        }
        LatLng kalmanLatlng = null;
        if (lastLoc == null || curLoc == null){
            return kalmanLatlng;
        }
        if (intensity < 1){
            intensity = 1;
        } else if (intensity > 5){
            intensity = 5;
        }
        for (int j = 0; j < intensity; j++){
            kalmanLatlng = kalmanFilter(lastLoc.longitude,curLoc.longitude,lastLoc.latitude,curLoc.latitude);
            curLoc = kalmanLatlng;
        }
        return kalmanLatlng;
    }


    /***************************卡尔曼滤波开始********************************/
    private double lastLocation_x; //上次位置
    private double currentLocation_x;//这次位置
    private double lastLocation_y; //上次位置
    private double currentLocation_y;//这次位置
    private double estimate_x; //修正后数据
    private double estimate_y; //修正后数据
    private double pdelt_x; //自预估偏差
    private double pdelt_y; //自预估偏差
    private double mdelt_x; //上次模型偏差
    private double mdelt_y; //上次模型偏差
    private double gauss_x; //高斯噪音偏差
    private double gauss_y; //高斯噪音偏差
    private double kalmanGain_x; //卡尔曼增益
    private double kalmanGain_y; //卡尔曼增益

    private double m_R= 0;
    private double m_Q= 0;
    //初始模型
    private void initial(){
        pdelt_x =  0.001;
        pdelt_y =  0.001;
//        mdelt_x = 0;
//        mdelt_y = 0;
        mdelt_x =  5.698402909980532E-4;
        mdelt_y =  5.698402909980532E-4;
    }
    private LatLng kalmanFilter(double oldValue_x, double value_x, double oldValue_y, double value_y){
        lastLocation_x = oldValue_x;
        currentLocation_x= value_x;
        gauss_x = Math.sqrt(pdelt_x * pdelt_x + mdelt_x * mdelt_x)+m_Q;     //计算高斯噪音偏差
        kalmanGain_x = Math.sqrt((gauss_x * gauss_x)/(gauss_x * gauss_x + pdelt_x * pdelt_x)) +m_R; //计算卡尔曼增益
        estimate_x = kalmanGain_x * (currentLocation_x - lastLocation_x) + lastLocation_x;    //修正定位点
        mdelt_x = Math.sqrt((1-kalmanGain_x) * gauss_x *gauss_x);      //修正模型偏差

        lastLocation_y = oldValue_y;
        currentLocation_y = value_y;
        gauss_y = Math.sqrt(pdelt_y * pdelt_y + mdelt_y * mdelt_y)+m_Q;     //计算高斯噪音偏差
        kalmanGain_y = Math.sqrt((gauss_y * gauss_y)/(gauss_y * gauss_y + pdelt_y * pdelt_y)) +m_R; //计算卡尔曼增益
        estimate_y = kalmanGain_y * (currentLocation_y - lastLocation_y) + lastLocation_y;    //修正定位点
        mdelt_y = Math.sqrt((1-kalmanGain_y) * gauss_y * gauss_y);      //修正模型偏差

        LatLng latlng = new LatLng(estimate_y,estimate_x);


        return latlng;
    }
    /***************************卡尔曼滤波结束**********************************/

    /***************************抽稀算法*************************************/
    private List<LatLng> reducerVerticalThreshold(List<LatLng> inPoints,
                                          float threshHold) {
        if (inPoints == null) {
            return null;
        }
        if (inPoints.size() <= 2) {
            return inPoints;
        }
        List<LatLng> ret = new ArrayList<LatLng>();
        for (int i = 0; i < inPoints.size(); i++) {
            LatLng pre = getLastLocation(ret);
            LatLng cur = inPoints.get(i);
            if (pre == null || i == inPoints.size() - 1) {
                ret.add(cur);
                continue;
            }
            LatLng next = inPoints.get(i + 1);
            double distance = calculateDistanceFromPoint(cur, pre, next);
            if (distance > threshHold){
                ret.add(cur);
            }
        }
        return ret;
    }
    private static LatLng getLastLocation(List<LatLng> oneGraspList) {
        if (oneGraspList == null || oneGraspList.size() == 0) {
            return null;
        }
        int locListSize = oneGraspList.size();
        LatLng lastLocation = oneGraspList.get(locListSize - 1);
        return lastLocation;
    }

    /**
     * 计算当前点到线的垂线距离
     * @param p 当前点
     * @param lineBegin 线的起点
     * @param lineEnd 线的终点
     *
     */
    private static double calculateDistanceFromPoint(LatLng p, LatLng lineBegin,
                                      LatLng lineEnd) {
        double A = p.longitude - lineBegin.longitude;
        double B = p.latitude - lineBegin.latitude;
        double C = lineEnd.longitude - lineBegin.longitude;
        double D = lineEnd.latitude - lineBegin.latitude;

        double dot = A * C + B * D;
        double len_sq = C * C + D * D;
        double param = dot / len_sq;

        double xx, yy;

        if (param < 0 || (lineBegin.longitude == lineEnd.longitude
                && lineBegin.latitude == lineEnd.latitude)) {
            xx = lineBegin.longitude;
            yy = lineBegin.latitude;
//            return -1;
        } else if (param > 1) {
            xx = lineEnd.longitude;
            yy = lineEnd.latitude;
//            return -1;
        } else {
            xx = lineBegin.longitude + param * C;
            yy = lineBegin.latitude + param * D;
        }
        return AMapUtils.calculateLineDistance(p,new LatLng(yy,xx));
    }
    /***************************抽稀算法结束*********************************/

    private List<LatLng> reduceNoisePoint(List<LatLng> inPoints, float threshHold) {
            if (inPoints == null) {
                return null;
            }
            if (inPoints.size() <= 2) {
                return inPoints;
            }
            List<LatLng> ret = new ArrayList<LatLng>();
            for (int i = 0; i < inPoints.size(); i++) {
                LatLng pre = getLastLocation(ret);
                LatLng cur = inPoints.get(i);
                if (pre == null || i == inPoints.size() - 1) {
                    ret.add(cur);
                    continue;
                }
                LatLng next = inPoints.get(i + 1);
                double distance = calculateDistanceFromPoint(cur, pre, next);
                if (distance < threshHold){
                    ret.add(cur);
                }
            }
            return ret;
        }
}