package com.imdroid.sideslope.bd;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

/**
 * RTCM 1005 message generation
 * Converted from rtcm1005.c
 */
public class Rtcm1005 {
    private static final Logger logger = LoggerFactory.getLogger(Rtcm1005.class);
    private static final int MAXBUFF = 1024;
    
    static class RTCM {
        byte[] buff = new byte[MAXBUFF];  // 消息缓冲区
        int nbit;                         // 缓冲区中的位数
        int staid;                        // 站点ID
        double[] sta_pos = new double[3]; // 站点位置(ECEF)(m)
    }
    
    /**
     * 设置无符号位
     * @param buff 缓冲区
     * @param pos 起始位置
     * @param len 位长度
     * @param data 要设置的数据
     */
    private static void setbitu(byte[] buff, int pos, int len, long data) {
        long mask = 1L << (len - 1);
        
        if (len <= 0 || 32 < len) return;
        
        for (int i = 0; i < len; i++, mask >>= 1) {
            if ((data & mask) != 0) 
                buff[pos / 8] |= (byte)(0x80 >> (pos % 8));
            else 
                buff[pos / 8] &= (byte)(~(0x80 >> (pos % 8)));
            pos++;
        }
    }
    
    /**
     * 设置有符号38位字段
     * @param buff 缓冲区
     * @param pos 起始位置
     * @param value 要设置的值
     */
    private static void set38bits(byte[] buff, int pos, double value) {
        long word = Math.round(value);
        int word_h = (int)(word >> 32);
        int word_l = (int)(word & 0xFFFFFFFFL);
        
        setbitu(buff, pos, 6, word_h);
        setbitu(buff, pos + 6, 32, word_l);
    }
    
    /**
     * 计算CRC24Q校验和
     * @param buff 缓冲区
     * @param len 长度
     * @return 校验和
     */
    private static int crc24q(byte[] buff, int len) {
        int crc = 0;
        
        for (int i = 0; i < len; i++) {
            crc ^= (buff[i] & 0xFF) << 16;
            for (int j = 0; j < 8; j++) {
                crc <<= 1;
                if ((crc & 0x1000000) != 0) crc ^= 0x1864CFB;
            }
        }
        return crc & 0xFFFFFF;
    }
    
    /**
     * 生成RTCM 1005消息
     * @param rtcm RTCM控制结构
     * @param sync 同步标志(1:同步,0:异步)
     * @return 状态(1:成功,0:错误)
     */
    private static int encode_type1005(RTCM rtcm, int sync) {
        double[] p = rtcm.sta_pos;
        int i = 0;
        int crc;
        int len;
        
        // RTCM消息头
        setbitu(rtcm.buff, i, 8, 0xD3); i += 8;  // 前导码
        setbitu(rtcm.buff, i, 6, 0);    i += 6;  // 保留位
        setbitu(rtcm.buff, i, 10, 19);  i += 10; // 消息长度
        setbitu(rtcm.buff, i, 12, 1005); i += 12; // 消息号
        setbitu(rtcm.buff, i, 12, rtcm.staid); i += 12; // 参考站ID
        setbitu(rtcm.buff, i, 6, 0);    i += 6;  // ITRF参考年份
        setbitu(rtcm.buff, i, 1, 1);    i += 1;  // GPS指示器
        setbitu(rtcm.buff, i, 1, 1);    i += 1;  // GLONASS指示器
        setbitu(rtcm.buff, i, 1, 0);    i += 1;  // Galileo指示器
        setbitu(rtcm.buff, i, 1, 0);    i += 1;  // 参考站指示器
        
        // 站点坐标
        set38bits(rtcm.buff, i, p[0] / 0.0001); i += 38; // 天线参考点ECEF-X
        setbitu(rtcm.buff, i, 1, 1);    i += 1;  // 振荡器指示器
        setbitu(rtcm.buff, i, 1, 0);    i += 1;  // 保留位
        set38bits(rtcm.buff, i, p[1] / 0.0001); i += 38; // 天线参考点ECEF-Y
        setbitu(rtcm.buff, i, 2, 0);    i += 2;  // 四分之一周期指示器
        set38bits(rtcm.buff, i, p[2] / 0.0001); i += 38; // 天线参考点ECEF-Z
        
        // 添加CRC24Q
        len = i / 8;
        crc = crc24q(rtcm.buff, len);
        setbitu(rtcm.buff, i, 24, crc); i += 24;
        
        rtcm.nbit = i;
        return 1;
    }
    
    /**
     * 从ECEF坐标生成RTCM 1005消息
     * @param ecef ECEF坐标(m) [x,y,z]
     * @param staid 站点ID
     * @param buff 输出缓冲区
     * @return 输出缓冲区长度(字节)，失败返回-1
     */
    public static int gen_rtcm1005(double[] ecef, int staid, byte[] buff) {
        RTCM rtcm = new RTCM();
        
        // 检查输入
        if (ecef == null || buff == null) return -1;
        
        // 设置站点信息
        rtcm.staid = staid;
        System.arraycopy(ecef, 0, rtcm.sta_pos, 0, 3);
        
        // 生成消息
        int ret = encode_type1005(rtcm, 0);
        if (ret == 0) return -1;
        
        // 复制消息到输出缓冲区
        int len = (rtcm.nbit + 7) / 8;
        System.arraycopy(rtcm.buff, 0, buff, 0, len);
        
        return len;
    }
    
    /**
     * 将字节数组转换为十六进制字符串
     * @param bytes 字节数组
     * @param len 长度
     * @param upperCase 是否大写
     * @param addSpaces 是否添加空格
     * @return 十六进制字符串
     */
    public static String bytesToHex(byte[] bytes, int len, boolean upperCase, boolean addSpaces) {
        StringBuilder sb = new StringBuilder();
        String format = upperCase ? "%02X" : "%02x";
        
        for (int i = 0; i < len; i++) {
            sb.append(String.format(format, bytes[i] & 0xFF));
            if (addSpaces && i < len - 1) sb.append(" ");
        }
        
        return sb.toString();
    }

    /**
     * 生成RTCM 1005消息并返回十六进制字符串
     * @param ecef ECEF坐标(m) [x,y,z]
     * @param staid 站点ID
     * @return RTCM 1005消息的十六进制字符串，失败返回null
     */
    public static String generateRtcm1005Hex(double[] ecef, int staid) {
        byte[] buff = new byte[MAXBUFF];
        int len = gen_rtcm1005(ecef, staid, buff);
        if (len > 0) {
            return bytesToHex(buff, len, false, false);
        }
        return null;
    }
}