package com.imdroid.sideslope.bd;


import com.imdroid.common.util.ByteUtil;
import com.imdroid.common.util.StringUtil;

import java.math.BigDecimal;

import static java.lang.Math.*;

public class RtcmGgaUtil {

    //gga样本：*后面跟的是校验和，其中76代表校验和，对$和*之间的数据（不包括这两个字符）按字节进行异或运算（二进制）的结果
    private static final String ggaExample = "$GNGGA,020850.50,2258.10508,N,11317.67958,E,4,12,0.74,3.9,M,-5.4,M,1.3,0000*76";

    /**
     * 获取rtcm数据类型
     * @param bytes
     * @return
     */
    public static int getType(byte[] bytes){
        String binaryStrFromByteArr = ByteUtil.getBinaryStrFromByteArr(bytes);
        if(binaryStrFromByteArr.length() > 8 + 6 + 10 + 12){
            String substring = binaryStrFromByteArr.substring(8 + 6 + 10, 8 + 6 + 10 + 12);
            int i = ByteUtil.binaryToInt(substring);
            return i;
        }else{
            return -1;
        }
    }

    public static String ggaMock(double lat, double lon){
        String originLat = getOriginLatLon(lat);
        String originLon = getOriginLatLon(lon);
        StringBuilder sb = new StringBuilder(ggaExample);
        sb.replace(sb.indexOf(",N,")-10,sb.indexOf(",N,"),originLat);
        sb.replace(sb.indexOf(",E,")-11,sb.indexOf(",E,"),originLon);
        String substring = sb.substring(sb.indexOf("$") + 1, sb.indexOf("*"));
        byte[] bytes = substring.getBytes();
        //校验位
        byte b = bytes[0];
        for (int i = 1; i < bytes.length; i++) {
            b ^= bytes[i];
        }
        sb.replace(sb.length()-2,sb.length(),ByteUtil.byteToHexString(b));
        return sb.toString();
    }

    public static String getGNGGAString(byte[] bytes){
        String hex = ByteUtil.bytesToHexString(bytes);
        int start = hex.indexOf("24474e474741");//$GNGGA
        int end = hex.indexOf("0d0a");//\r\n
        if(start != -1 && end > start){
            String substring = hex.substring(start,end);
            //System.out.println(new String(ByteUtil.hexStringTobyte(substring)));
            return new String(ByteUtil.hexStringTobyte(substring));
        }
        return "";
    }

    public static Gga getGga(String msg){
        try{
            String[] strings = msg.split(",");
            double latitude = getLatLon(Double.parseDouble(strings[2]));
            double longitude = getLatLon(Double.parseDouble(strings[4]));
            double altitude = 100*(Double.parseDouble(strings[9]) + Double.parseDouble(strings[11]));
            int status = Integer.parseInt(strings[6]);
            return new Gga(latitude,longitude,altitude,status);
        }catch (Exception e){
            return null;
        }
    }


    /**
     * 解析阉割版gga，为了那点流量...
     * @param msg
     * @return
     */
    public static Gga getGga2(String msg){
        try{
            String[] split = msg.split(",");
            double lat = getLatLon(Double.parseDouble(split[1]));
            double lon = getLatLon(Double.parseDouble(split[3]));
            double altitude =  Double.parseDouble(split[5]);
            return new Gga(lat,lon,altitude,0);
        }catch (Exception e){
            return null;
        }
    }

    /**
     * 解析阉割版gga，为了那点流量...
     * @param msg
     * @return
     */
    public static Gga getGga3(String msg){
        try{
            String[] split = msg.split(",");
            double lat = getLatLon(Double.parseDouble(split[1]));
            double lon = getLatLon(Double.parseDouble(split[3]));
            int status =  Integer.parseInt(split[5]);
            double altitude = Double.parseDouble(split[6]);
            return new Gga(lat,lon,altitude,status);
        }catch (Exception e){
            return null;
        }
    }


    /**
     * 求相对坐标
     * @param gps_info
     * @param origin
     * @return
     */
    public static double[] ggaToXyz(Gga gps_info,Gga origin){
        return ggaToXyz(gps_info,origin,0);
    }

    /**
     * 求相对坐标
     * @param gps_info
     * @param origin
     * @param angle 与北偏角
     * @return
     */
    public static double[] ggaToXyz(Gga gps_info,Gga origin, double angle){
        double Ea = 6378137 * 100;    //赤道的半径，单位是cm
        //cout << "Ea" << Ea << endl;
        double Eb = 6356725 * 100;    //极半径，单位是cm
        double Db = 39940638;        //极周长，单位是cm，如果直接 39940538*100 double类型会溢出，所以把*100放到 dN 的计算式中
        //cout << "Db=" << Db << endl;
        double R = Eb + (Ea - Eb) * (90 - origin.getLatitude()) / 90;
        double pD = R * 2 * PI * cos(origin.getLatitude() * PI / 180);
        double dE = pD * (gps_info.getLongitude() - origin.getLongitude()) / 360;    //计算在正东方向上的位移
        double dN = Db * (gps_info.getLatitude() - origin.getLatitude()) / 360 * 100;        //计算在正北方向上的位移
        //接下来根据场地的朝向来把朝东朝北的偏移转化成x方向和y方向的位移
        //这个位移也就是车辆在场地中的x坐标和y坐标
        double x = dE * cos(angle * PI / 180) - dN * sin(angle * PI / 180);
        double y = dN * cos(angle * PI / 180) + dE * sin(angle * PI / 180);
        double z = gps_info.getAltitude() - origin.getAltitude();
        return new double[]{x,y,z};
    }

    //一度为60分
    public static double getLatLon(Double location) {
        if (location != 0.0) {
            //int head = Integer.parseInt(String.valueOf(location).substring(0, 2));
            int head = (int)(location/100);
            // System.out.println(head);
            return head + (location / 100 - head) * 10 / 6;
        }
        return 0.0;
    }

    //逆向转化
    public static String getOriginLatLon(double latlon){
        double result = 100*((int)latlon) + (latlon-(int)latlon)*6/10*100;
        BigDecimal bd = new BigDecimal(result);
        result = bd.setScale(5,BigDecimal.ROUND_HALF_EVEN).doubleValue();
        return StringUtil.roundByScale(result, 5);
    }

    /**
     * 求航向角
     * @param backLat
     * @param backLon
     * @param frontLat
     * @param frontLon
     * @return
     */
    public static double latLonToAngle(double backLat, double backLon, double frontLat, double frontLon){
        double[] doubles = ggaToXyz(new Gga(frontLat, frontLon), new Gga(backLat, backLon));
        double angle = Math.atan2(doubles[0],doubles[1]) * 180/ Math.PI;
        angle = (angle + 360) % 360;
        return angle;
    }

    /**
     *       正北
     *          ^     ^ y轴
     *           \    |
     *            \   +-----------+ (x,y)
     *             \  |           |
     *              \ |           |
     *               \|           |
     *         -------+-----------+---->
     *          origin|                x轴
     *                |
     *
     * 求某一个坐标的经纬度
     * @param x 单位厘米
     * @param y
     * @param origin 参考点的经纬度
     * @param angle_from_north y轴与正北夹角
     * @return
     */
    public static Gga xyToGga(double x, double y, Gga origin, double angle_from_north){
        double latitude1 = -x * sin(angle_from_north * PI /180)
                + y * cos(angle_from_north * PI /180);
        double longitude1 = x / cos(angle_from_north * PI /180)
                + (y - (x * tan(angle_from_north * PI /180)))
                * sin(angle_from_north * PI /180);

        Gga ggaInfo1 = new Gga();
        ggaInfo1.setLatitude(origin.getLatitude() + 0.000001);
        ggaInfo1.setLongitude(origin.getLongitude());
        Gga ggaInfo2 = new Gga();
        ggaInfo2.setLatitude(origin.getLatitude());
        ggaInfo2.setLongitude(origin.getLongitude() + 0.000001);

        double[] location1 = ggaToXyz(ggaInfo1,origin);
        double[] location2 = ggaToXyz(ggaInfo2,origin);

        double a = location1[1];
        double b = location2[0];

        double latitude = latitude1 / a * 0.000001;
        double longitude = longitude1 / b * 0.000001;

        Gga ggaInfo = new Gga();
        ggaInfo.setLatitude(origin.getLatitude() + latitude);
        ggaInfo.setLongitude(origin.getLongitude() + longitude);
        return ggaInfo;
    }

    /**
     * 计算两点之间距离，10厘米以下也能计算，精度更高，单位厘米，
     */
    public static double getDistance2(double latitude, double longitude, double nowLatitude, double nowLongitude) {
        Gga gga1 = new Gga(latitude, longitude);
        Gga gga2 = new Gga(nowLatitude, nowLongitude);
        double[] doubles = ggaToXyz(gga1, gga2);
        return Math.sqrt(Math.pow(doubles[0],2) + Math.pow(doubles[1],2));
    }

    /**
     * 计算两点之间距离，单位米
     */
    public static double getDistance(double sourceLat, double sourceLog, double desLat, double desLog) {
        //转为弧度
        double lat1 = (Math.PI / 180) * sourceLat;
        double lat2 = (Math.PI / 180) * desLat;

        double lon1 = (Math.PI / 180) * sourceLog;
        double lon2 = (Math.PI / 180) * desLog;

        //地球半径
        double R = 6371.004;

        //两点间距离 km，如果想要米的话，结果*1000
        double d = Math.acos(Math.sin(lat1) * Math.sin(lat2) + Math.cos(lat1) * Math.cos(lat2) * Math.cos(lon2 - lon1)) * R;

        if (Double.isNaN(d * 1000)) {
            return 0.0;
        }

        return d * 1000;
    }

}
