package com.imdroid.sideslope.bd;

import io.netty.buffer.ByteBuf;

/**
 * b562 01 3c 4000 01 00 0000 40235d21 8dffffff 08ffffff 00000000 11010000 8af4750100000000ddeab84864000000640000006400000064000000c55100000000000037010000 996e
 * b562固定开头，01是class，3c是id，40 00是有效数据长度，低字节序号，所以有效数据长度为0x40，接下来是有效数据，最后996e是校验位
 */
public class UBXUtil {
    public static int FIX_RESULT = 2;
    public static int FLOAT_RESULT = 1;
    public static int NO_B562 = 0;

    public static double[] getLocation(ByteBuf data){
        boolean has_b562 = false;
        byte[] b562_flag = {(byte) 0xb5, (byte)0x62};
        int pos = 26; // 从惯导之后开始

        for(; pos<data.readableBytes() - 1; pos++){
            byte d0 = data.getByte(pos);
            byte d1 = data.getByte(pos+1);
            if(d0 == b562_flag[0] && d1 == b562_flag[1]){
                has_b562 = true;
                break;
            }
        }
        if(!has_b562) return new double[]{0,0,0,NO_B562};
        pos+=6;

        // 单位cm
        double relPosN = data.getIntLE(pos+8) + data.getByte(pos+32)*0.01;
        double relPosE = data.getIntLE(pos+12) + data.getByte(pos+33)*0.01;
        double relPosD = data.getIntLE(pos+16) + data.getByte(pos+34)*0.01;
        relPosD = -relPosD;
        short status = (short) ((data.getUnsignedByte(pos+60) & 0x18)>>3); //mask: 00011000

        // 转成mm返回
        if(status == 2){
            return new double[]{relPosE*10,relPosN*10,relPosD*10,FIX_RESULT};
        }
        else if(status == 1){
            return new double[]{relPosE*10,relPosN*10,relPosD*10,FLOAT_RESULT};
        }
        else{
            return new double[]{0,0,0,FLOAT_RESULT};
        }
    }
}

