from mutils import *

# This script computes the per-SV correlation function slopes.
# The data is used in mm_everest.c to calibrate Everest biases.
# Usage:
#  python group_mp_slope.py
# Then use the final output to update tables in mm_everest.c, e.g.:
#  b1_grp0_prns[] = [11, 47]
#  b1_grp1_prns[] = [1, 10, 12, 13, 14, 15, 16, 48]
#
# The GPS info is mainly provided for reference.  The BeiDou data
# is useful for when new satellites are found.  Basically:
#  1- copy updated lm_compass_codes.c:start_state_beidou_B1_B2_2[] to
#     the bds_code() function below
#  2- run script to get new b1_grp?_prns[] arrays for mm_everest.c

def lfsr( taps, reg ):
    """\
    Model a linear feedback shift register:
     taps = bits to use for feedback.  Valid range = [1, len(reg)]
     reg = array([]) of 1/0 bits
    Returns: new value for reg
    """
    fb = 0
    for t in taps:
        fb ^= reg[t-1]
    reg[:] = roll(reg,1)
    reg[0] = fb
    return reg

def hex2array(val,n_bits):
    """\
    Convert hex to array([])
    """
    return array([(val>>i)&1 for i in range(n_bits)])

def gps_icd_code(prn):
    """\
    Generate code for GPS satellite based on ICD method
    """
    n_bits = 10
    g1 = hex2array(0x3ff, n_bits)
    g2 = hex2array(0x3ff, n_bits)
    g1_taps = [3,10] # 1 + x**3 + x**10
    g2_taps = [2,3,6,8,9,10]
    # from ICD:
    icd_comb_taps = [(2, 6),
                     (3, 7),
                     (4, 8),
                     (5, 9),
                     (1, 9),
                     (2, 10),
                     (1, 8),
                     (2, 9),
                     (3, 10),
                     (2, 3),
                     (3, 4),
                     (5, 6),
                     (6, 7),
                     (7, 8),
                     (8, 9),
                     (9, 10),
                     (1, 4),
                     (2, 5),
                     (3, 6),
                     (4, 7),
                     (5, 8),
                     (6, 9),
                     (1, 3),
                     (4, 6),
                     (5, 7),
                     (6, 8),
                     (7, 9),
                     (8, 10),
                     (1, 6),
                     (2, 7),
                     (3, 8),
                     (4, 9)]
    comb_taps = icd_comb_taps[prn-1]
    code = zeros(1023)
    for i in range(len(code)):
        if g1[-1] ^ g2[comb_taps[0]-1] ^ g2[comb_taps[1]-1]:
            code[i] = 1
        else:
            code[i] = -1
        lfsr( g1_taps, g1 )
        lfsr( g2_taps, g2 )
    return code


# copied from stinger/lm/lm_catab.c
gps_g2_offset = [  1023-5,         # SV #1 */
                   1023-6,         # SV #2 */
                   1023-7,         # SV #3 */
                   1023-8,         # SV #4 */
                   1023-17,        # SV #5 */
                   1023-18,        # SV #6 */
                   1023-139,       # SV #7 */
                   1023-140,       # SV #8 */
                   1023-141,       # SV #9 */
                   1023-251,       # SV #10 */
                   1023-252,       # SV #11 */
                   1023-254,       # SV #12 */
                   1023-255,       # SV #13 */
                   1023-256,       # SV #14 */
                   1023-257,       # SV #15 */
                   1023-258,       # SV #16 */
                   1023-469,       # SV #17 */
                   1023-470,       # SV #18 */
                   1023-471,       # SV #19 */
                   1023-472,       # SV #20 */
                   1023-473,       # SV #21 */
                   1023-474,       # SV #22 */
                   1023-509,       # SV #23 */
                   1023-512,       # SV #24 */
                   1023-513,       # SV #25 */
                   1023-514,       # SV #26 */
                   1023-515,       # SV #27 */
                   1023-516,       # SV #28 */
                   1023-859,       # SV #29 */
                   1023-860,       # SV #30 */
                   1023-861,       # SV #31 */
                   1023-862 ]      # SV #32 */
irnss_l5_g2_offset = [
  1023-448,       #/* SV #1 - IRNSS L band */
  1023-379,       #/* SV #2 - IRNSS L band */
  1023-142,       #/* SV #3 - IRNSS L band */
  1023-183,       #/* SV #4 - IRNSS L band */
  1023-273,       #/* SV #5 - IRNSS L band */
  1023-637,       #/* SV #6 - IRNSS L band */
  1023-970,       #/* SV #7 - IRNSS L band */
  1023-739,       #/* SV #8 - IRNSS L band */
  1023-800,       #/* SV #9 - IRNSS L band */
  1023-871,       #/* SV #10 - IRNSS L band */
  1023-319,       #/* SV #11 - IRNSS L band */
  1023-242,       #/* SV #12 - IRNSS L band */
  1023-1011,      #/* SV #13 - IRNSS L band */
  1023-1000       #/* SV #14 - IRNSS L band */
]
irnss_s1_g2_offset = [
  1023-541,       #/* SV #1 - IRNSS S band */
  1023-563,       #/* SV #2 - IRNSS S band */
  1023-485,       #/* SV #3 - IRNSS S band */
  1023-840,       #/* SV #4 - IRNSS S band */
  1023-344,       #/* SV #5 - IRNSS S band */
  1023-424,       #/* SV #6 - IRNSS S band */
  1023-462,       #/* SV #7 - IRNSS S band */
  1023-802,       #/* SV #8 - IRNSS S band */
  1023-551,       #/* SV #9 - IRNSS S band */
  1023-564,       #/* SV #10 - IRNSS S band */
  1023-147,       #/* SV #11 - IRNSS S band */
  1023-927,       #/* SV #12 - IRNSS S band */
  1023-845,       #/* SV #13 - IRNSS S band */
  1023-952        #/* SV #14 - IRNSS S band */
]

def ca_code(prn,g2_offset):
    """\
    Generate code for GPS-like CA code based on Maxwell/Stinger method
    """
    n_bits = 10
    g1 = hex2array(0x3ff, n_bits)
    g2 = hex2array(0x3ff, n_bits)

    g1_taps = [3,10] # 1 + x**3 + x**10
    g2_taps = [2,3,6,8,9,10]
    # Shift g2 by offset
    for i in range(g2_offset[prn-1]):
        lfsr( g2_taps, g2 )

    code = zeros(1023)
    for i in range(len(code)):
        if g1[-1] ^ g2[-1]:
            code[i] = 1
        else:
            code[i] = -1
        lfsr( g1_taps, g1 )
        lfsr( g2_taps, g2 )
    return code

def bds_code(prn):
    """\
    Generate code for BeiDou satellite based on Maxwell/Stinger method
    """
    # Maxwell 6/7 uses a 13-bit register, so model that here
    n_bits = 13

    # copied from stinger/lm/lm_compass_codes.c
    g2_init = [0x061f, 0x18e5, 0x0798, 0x1826, 0x1816,
               0x07e1, 0x181a, 0x07e7, 0x1c13, 0x1efa,
               0x0187, 0x1e39, 0x1e09, 0x01fe, 0x1e05,
               0x01f8, 0x1f7d, 0x00c3, 0x00f3, 0x1f04,
               0x00ff, 0x1f02, 0x1fbe, 0x1f8e, 0x0079,
               0x1f82, 0x007f, 0x0030, 0x1fc7, 0x003c,
               0x1fc1, 0x1ff7, 0x000c, 0x1ff1, 0x1ffb,
               0x0006, 0x1ffd, 0x115f, 0x13b6, 0x1375,
               0x1345, 0x1349, 0x0cb4, 0x1231, 0x1248,
               0x12f2, 0x12c2, 0x12ce, 0x0d33, 0x128b,
               0x12bb, 0x12b7, 0x0d4a, 0x1740, 0x16c7,
               0x16be, 0x142e, 0x1457, 0x14ed, 0x14dd,
               0x14d1, 0x0b2c, 0x1494
    ]

    g1 = hex2array(0x0AA9, n_bits)
    g2 = hex2array(g2_init[prn-1], n_bits)
    g1_taps = [1, 7, 8, 9, 10, 11]
    g2_taps = [1, 2, 3, 4, 5, 8, 9, 11]
    code = zeros(2046)
    for i in range(len(code)):
        if g1[-1] ^ g2[-1]:
            code[i] = 1
        else:
            code[i] = -1
        lfsr( g1_taps, g1 )
        lfsr( g2_taps, g2 )
    return code

def bds_code_icd(prn):
    """\
    Generate code for BeiDou satellite based on ICD method
    """
    n_bits = 11
    g1 = hex2array(0x2AA, n_bits)
    g2 = g1.copy()

    g2_feedback_1 = [ 1, 1, 1, 1, 1, 1, 1, 1, 2, 3, #  1 - 10
                      3, 3, 3, 3, 3, 3, 4, 4, 4, 4, # 11 - 20
                      4, 4, 5, 5, 5, 5, 5, 6, 6, 6, # 21 - 30
                      6, 8, 8, 8, 9, 9,10, 1, 1, 1, # 31 - 40
                      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, # 41 - 50
                      1, 1, 1, 2, 2, 2, 3, 3, 3, 3, # 51 - 60
                      3, 3, 3                       # 61 - 63
    ]
    g2_feedback_2 = [ 3, 4, 5, 6, 8, 9,10,11, 7, 4, #  1 - 10
                      5, 6, 8, 9,10,11, 5, 6, 8, 9, # 11 - 20
                     10,11, 6, 8, 9,10,11, 8, 9,10, # 21 - 30
                     11, 9,10,11,10,11,11, 2, 3, 3, # 31 - 40
                      3, 3, 3, 4, 4, 5, 5, 5, 5, 6, # 41 - 50
                      8, 9, 9, 3, 5, 7, 4, 4, 5, 5, # 51 - 60
                      5, 5, 6                       # 61 - 63
    ]
    g2_feedback_3 = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, #  1 - 10
                      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, # 11 - 20                
                      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, # 21 - 30                
                      0, 0, 0, 0, 0, 0, 0, 7, 4, 6, # 31 - 40                
                      8,10,11, 5, 9, 6, 8,10,11, 9, # 41 - 50                
                      9,10,11, 7, 7, 9, 5, 9, 6, 8, # 51 - 60
                     10,11, 9                       # 61 - 63
    ]
    if prn > 63:
        return []
    g1_taps = [1, 7, 8, 9, 10, 11]
    g2_taps = [1, 2, 3, 4,  5,  8,  9, 11]
    code = zeros(2046)
    for i in range(len(code)):
        g2_fb = g2[g2_feedback_1[prn-1]-1] ^ g2[g2_feedback_2[prn-1]-1]
        if ( g2_feedback_3[prn-1] > 0 ):
            g2_fb = g2_fb ^ g2[g2_feedback_3[prn-1] - 1]
        if g1[-1] ^ g2_fb:
            code[i] =  1
        else:
            code[i] = -1
        lfsr( g1_taps, g1 )
        lfsr( g2_taps, g2 )
    return code

def show_ca_slopes(txt, max_prn, g2_offset, fixed_scale=1024.):
    """\
    Summarize all GPS satellite correlation function slope info
    """
    print(txt + ":")
    chip_rate = 1.023e6
    chip_m = GnssConst.LIGHT/chip_rate
    epl_space_m = 40e-9 * GnssConst.LIGHT
    groups = {}
    for prn in range(1,max_prn+1):
        x = ca_code(prn, g2_offset)
        top = sum(x*roll(x,0))
        bot = sum(x*roll(x,1))
        print(' SV %2d %5.1f %.1f ~= %.1f/%.0f' % (
            prn,
            bot,
            top,
            epl_space_m/chip_m * (top-bot) * (len(x)/fixed_scale),
            fixed_scale
            )
        )
        if bot == -65:
            groups.setdefault(0,[]).append(prn)
        elif bot == 63:
            groups.setdefault(1,[]).append(prn)
        elif bot == -1:
            groups.setdefault(2,[]).append(prn)
        else:
            raise RuntimeError('fix %s group for prn %d (val = %d)'%(txt,prn,bot))
    return groups

def show_bds_slopes(fixed_scale=1024.):
    """\
    Summarize all known BeiDou satellite correlation function slope info
    """
    print("BeiDou:")
    chip_rate = 2.046e6
    chip_m = GnssConst.LIGHT/chip_rate
    epl_space_m = 40e-9 * GnssConst.LIGHT
    bds_groups = {}
    for prn in range(1,63+1):
        x = bds_code_icd(prn)
        if len(x) == 0:
            continue
        top = sum(x*roll(x,0))
        bot = sum(x*roll(x,1))
        print(' SV %2d %5.1f/%6.1f ~= %.1f/%.1f' % (
            prn,
            bot,
            top,
            epl_space_m/chip_m * (top-bot) * fixed_scale/top,
            fixed_scale
            )
        )
        if bot== 62:
            bds_groups.setdefault(0,[]).append(prn)
        elif abs(bot+64) <= 2:
            bds_groups.setdefault(1,[]).append(prn)
        elif abs(bot)<=2:
            bds_groups.setdefault(2,[]).append(prn)
        else:
            raise RuntimeError('fix BDS group for prn %d (val = %d)'%(prn,bot))
    return bds_groups

if __name__ == '__main__':
    gps_groups = show_ca_slopes("GPS",32,gps_g2_offset)
    irnss_l5_groups = show_ca_slopes("IRNSS L5",14,irnss_l5_g2_offset)
    irnss_s1_groups = show_ca_slopes("IRNSS S1",14,irnss_s1_g2_offset)
    bds_groups = show_bds_slopes()
    print('grp0_prns[] = {}'.format(gps_groups[0]))
    print('grp1_prns[] = {}'.format(gps_groups[1]))
    print('irnss_l5_grp0_prns[] = {}'.format(irnss_l5_groups[0]))
    print('irnss_l5_grp1_prns[] = {}'.format(irnss_l5_groups[1]))
    print('irnss_s1_grp0_prns[] = {}'.format(irnss_s1_groups[0]))
    print('irnss_s1_grp1_prns[] = {}'.format(irnss_s1_groups[1]))
    print('b1_grp0_prns[] = {}'.format(bds_groups[0]))
    print('b1_grp1_prns[] = {}'.format(bds_groups[1]))