#ifndef INCLUDED_IMBE_FRAME_H
#define INCLUDED_IMBE_FRAME_H

#include "op25_yank.h"
#include "op25_golay.h"
#include "op25_hamming.h"

#include <cstddef>
#include <stdint.h>
#include <vector>

typedef std::vector<bool> voice_codeword;
typedef const std::vector<bool> const_bit_vector;
typedef std::vector<bool> bit_vector;

static const size_t nof_voice_codewords = 9, voice_codeword_sz = 144;

static const uint16_t imbe_ldu_NID_bits[] = {
   48,   49,   50,   51,   52,   53,   54,   55,   56,   57,   58,   59,
   60,   61,   62,   63,   64,   65,   66,   67,   68,   69,   72,   73,
   74,   75,   76,   77,   78,   79,   80,   81,   82,   83,   84,   85,
   86,   87,   88,   89,   90,   91,   92,   93,   94,   95,   96,   97,
   98,   99,  100,  101,  102,  103,  104,  105,  106,  107,  108,  109,
  110,  111,  112,  113 };

static const uint16_t imbe_ldu_status_bits[] = {
   70,   71,  142,  143,  214,  215,  286,  287,  358,  359,  430,  431,
  502,  503,  574,  575,  646,  647,  718,  719,  790,  791,  862,  863,
  934,  935, 1006, 1007, 1078, 1079, 1150, 1151, 1222, 1223, 1294, 1295,
 1366, 1367, 1438, 1439, 1510, 1511, 1582, 1583, 1654, 1655, 1726, 1727 };

static const uint16_t imbe_ldu_lcf_bits[] = {
 410, 411, 412, 413, 414, 415, 420, 421,
 422, 423, 424, 425, 432, 433, 434, 435,
 436, 437, 442, 443, 444, 445, 446, 447,
 600, 601, 602, 603, 604, 605, 610, 611,
 612, 613, 614, 615, 620, 621, 622, 623,
 624, 625, 630, 631, 632, 633, 634, 635,
 788, 789, 792, 793, 794, 795, 800, 801,
 802, 803, 804, 805, 810, 811, 812, 813,
 814, 815, 820, 821, 822, 823, 824, 825 };

// FIXME: separate these into their respective fields
static const uint16_t imbe_ldu_ls_data_bits[] = {
  410,  411,  412,  413,  414,  415,  416,  417,  418,  419,  420,  421,
  422,  423,  424,  425,  426,  427,  428,  429,  432,  433,  434,  435,
  436,  437,  438,  439,  440,  441,  442,  443,  444,  445,  446,  447,
  448,  449,  450,  451,  600,  601,  602,  603,  604,  605,  606,  607,
  608,  609,  610,  611,  612,  613,  614,  615,  616,  617,  618,  619,
  620,  621,  622,  623,  624,  625,  626,  627,  628,  629,  630,  631,
  632,  633,  634,  635,  636,  637,  638,  639,  788,  789,  792,  793,
  794,  795,  796,  797,  798,  799,  800,  801,  802,  803,  804,  805,
  806,  807,  808,  809,  810,  811,  812,  813,  814,  815,  816,  817,
  818,  819,  820,  821,  822,  823,  824,  825,  826,  827,  828,  829,
  978,  979,  980,  981,  982,  983,  984,  985,  986,  987,  988,  989,
  990,  991,  992,  993,  994,  995,  996,  997,  998,  999, 1000, 1001,
 1002, 1003, 1004, 1005, 1008, 1009, 1010, 1011, 1012, 1013, 1014, 1015,
 1016, 1017, 1018, 1019, 1168, 1169, 1170, 1171, 1172, 1173, 1174, 1175,
 1176, 1177, 1178, 1179, 1180, 1181, 1182, 1183, 1184, 1185, 1186, 1187,
 1188, 1189, 1190, 1191, 1192, 1193, 1194, 1195, 1196, 1197, 1198, 1199,
 1200, 1201, 1202, 1203, 1204, 1205, 1206, 1207, 1356, 1357, 1358, 1359,
 1360, 1361, 1362, 1363, 1364, 1365, 1368, 1369, 1370, 1371, 1372, 1373,
 1374, 1375, 1376, 1377, 1378, 1379, 1380, 1381, 1382, 1383, 1384, 1385,
 1386, 1387, 1388, 1389, 1390, 1391, 1392, 1393, 1394, 1395, 1396, 1397,
 1546, 1547, 1548, 1549, 1550, 1551, 1552, 1553, 1554, 1555, 1556, 1557,
 1558, 1559, 1560, 1561, 1562, 1563, 1564, 1565, 1566, 1567, 1568, 1569,
 1570, 1571, 1572, 1573, 1574, 1575, 1576, 1577 };

static const uint16_t voice_codeword_bits[nof_voice_codewords][voice_codeword_sz] = {

      { 114, 121, 126, 133, 138, 147, 152, 159, 164, 171, 176, 183,
        188, 195, 200, 207, 212, 221, 226, 233, 238, 245, 250, 257,
        115, 120, 127, 132, 139, 146, 153, 158, 165, 170, 177, 182,
        189, 194, 201, 206, 213, 220, 227, 232, 239, 244, 251, 256,
        116, 123, 128, 135, 140, 149, 154, 161, 166, 173, 178, 185,
        190, 197, 202, 209, 216, 223, 228, 235, 240, 247, 252, 259,
        117, 122, 129, 134, 141, 148, 155, 160, 167, 172, 179, 184,
        191, 196, 203, 208, 217, 222, 229, 234, 241, 246, 253, 258,
        118, 125, 130, 137, 144, 151, 156, 163, 168, 175, 180, 187,
        192, 199, 204, 211, 218, 225, 230, 237, 242, 249, 254, 261,
        119, 124, 131, 136, 145, 150, 157, 162, 169, 174, 181, 186,
        193, 198, 205, 210, 219, 224, 231, 236, 243, 248, 255, 260 },

      { 262, 269, 274, 281, 288, 295, 300, 307, 312, 319, 324, 331,
        336, 343, 348, 355, 362, 369, 374, 381, 386, 393, 398, 405,
        263, 268, 275, 280, 289, 294, 301, 306, 313, 318, 325, 330,
        337, 342, 349, 354, 363, 368, 375, 380, 387, 392, 399, 404,
        264, 271, 276, 283, 290, 297, 302, 309, 314, 321, 326, 333,
        338, 345, 350, 357, 364, 371, 376, 383, 388, 395, 400, 407,
        265, 270, 277, 282, 291, 296, 303, 308, 315, 320, 327, 332,
        339, 344, 351, 356, 365, 370, 377, 382, 389, 394, 401, 406,
        266, 273, 278, 285, 292, 299, 304, 311, 316, 323, 328, 335,
        340, 347, 352, 361, 366, 373, 378, 385, 390, 397, 402, 409,
        267, 272, 279, 284, 293, 298, 305, 310, 317, 322, 329, 334,
        341, 346, 353, 360, 367, 372, 379, 384, 391, 396, 403, 408 },

      { 452, 459, 464, 471, 476, 483, 488, 495, 500, 509, 514, 521,
        526, 533, 538, 545, 550, 557, 562, 569, 576, 583, 588, 595,
        453, 458, 465, 470, 477, 482, 489, 494, 501, 508, 515, 520,
        527, 532, 539, 544, 551, 556, 563, 568, 577, 582, 589, 594,
        454, 461, 466, 473, 478, 485, 490, 497, 504, 511, 516, 523,
        528, 535, 540, 547, 552, 559, 564, 571, 578, 585, 590, 597,
        455, 460, 467, 472, 479, 484, 491, 496, 505, 510, 517, 522,
        529, 534, 541, 546, 553, 558, 565, 570, 579, 584, 591, 596,
        456, 463, 468, 475, 480, 487, 492, 499, 506, 513, 518, 525,
        530, 537, 542, 549, 554, 561, 566, 573, 580, 587, 592, 599,
        457, 462, 469, 474, 481, 486, 493, 498, 507, 512, 519, 524,
        531, 536, 543, 548, 555, 560, 567, 572, 581, 586, 593, 598 },

      { 640, 649, 654, 661, 666, 673, 678, 685, 690, 697, 702, 709,
        714, 723, 728, 735, 740, 747, 752, 759, 764, 771, 776, 783,
        641, 648, 655, 660, 667, 672, 679, 684, 691, 696, 703, 708,
        715, 722, 729, 734, 741, 746, 753, 758, 765, 770, 777, 782,
        642, 651, 656, 663, 668, 675, 680, 687, 692, 699, 704, 711,
        716, 725, 730, 737, 742, 749, 754, 761, 766, 773, 778, 785,
        643, 650, 657, 662, 669, 674, 681, 686, 693, 698, 705, 710,
        717, 724, 731, 736, 743, 748, 755, 760, 767, 772, 779, 784,
        644, 653, 658, 665, 670, 677, 682, 689, 694, 701, 706, 713,
        720, 727, 732, 739, 744, 751, 756, 763, 768, 775, 780, 787,
        645, 652, 659, 664, 671, 676, 683, 688, 695, 700, 707, 712,
        721, 726, 733, 738, 745, 750, 757, 762, 769, 774, 781, 786 },

      { 830, 837, 842, 849, 854, 861, 868, 875, 880, 887, 892, 899,
        904, 911, 916, 923, 928, 937, 942, 949, 954, 961, 966, 973,
        831, 836, 843, 848, 855, 860, 869, 874, 881, 886, 893, 898,
        905, 910, 917, 922, 929, 936, 943, 948, 955, 960, 967, 972,
        832, 839, 844, 851, 856, 865, 870, 877, 882, 889, 894, 901,
        906, 913, 918, 925, 930, 939, 944, 951, 956, 963, 968, 975,
        833, 838, 845, 850, 857, 864, 871, 876, 883, 888, 895, 900,
        907, 912, 919, 924, 931, 938, 945, 950, 957, 962, 969, 974,
        834, 841, 846, 853, 858, 867, 872, 879, 884, 891, 896, 903,
        908, 915, 920, 927, 932, 941, 946, 953, 958, 965, 970, 977,
        835, 840, 847, 852, 859, 866, 873, 878, 885, 890, 897, 902,
        909, 914, 921, 926, 933, 940, 947, 952, 959, 964, 971, 976 },

      { 1020, 1027, 1032, 1039, 1044, 1051, 1056, 1063, 1068, 1075, 1082, 1089,
        1094, 1101, 1106, 1113, 1118, 1125, 1130, 1137, 1142, 1149, 1156, 1163,
        1021, 1026, 1033, 1038, 1045, 1050, 1057, 1062, 1069, 1074, 1083, 1088,
        1095, 1100, 1107, 1112, 1119, 1124, 1131, 1136, 1143, 1148, 1157, 1162,
        1022, 1029, 1034, 1041, 1046, 1053, 1058, 1065, 1070, 1077, 1084, 1091,
        1096, 1103, 1108, 1115, 1120, 1127, 1132, 1139, 1144, 1153, 1158, 1165,
        1023, 1028, 1035, 1040, 1047, 1052, 1059, 1064, 1071, 1076, 1085, 1090,
        1097, 1102, 1109, 1114, 1121, 1126, 1133, 1138, 1145, 1152, 1159, 1164,
        1024, 1031, 1036, 1043, 1048, 1055, 1060, 1067, 1072, 1081, 1086, 1093,
        1098, 1105, 1110, 1117, 1122, 1129, 1134, 1141, 1146, 1155, 1160, 1167,
        1025, 1030, 1037, 1042, 1049, 1054, 1061, 1066, 1073, 1080, 1087, 1092,
        1099, 1104, 1111, 1116, 1123, 1128, 1135, 1140, 1147, 1154, 1161, 1166 },

      { 1208, 1215, 1220, 1229, 1234, 1241, 1246, 1253, 1258, 1265, 1270, 1277,
        1282, 1289, 1296, 1303, 1308, 1315, 1320, 1327, 1332, 1339, 1344, 1351,
        1209, 1214, 1221, 1228, 1235, 1240, 1247, 1252, 1259, 1264, 1271, 1276,
        1283, 1288, 1297, 1302, 1309, 1314, 1321, 1326, 1333, 1338, 1345, 1350,
        1210, 1217, 1224, 1231, 1236, 1243, 1248, 1255, 1260, 1267, 1272, 1279,
        1284, 1291, 1298, 1305, 1310, 1317, 1322, 1329, 1334, 1341, 1346, 1353,
        1211, 1216, 1225, 1230, 1237, 1242, 1249, 1254, 1261, 1266, 1273, 1278,
        1285, 1290, 1299, 1304, 1311, 1316, 1323, 1328, 1335, 1340, 1347, 1352,
        1212, 1219, 1226, 1233, 1238, 1245, 1250, 1257, 1262, 1269, 1274, 1281,
        1286, 1293, 1300, 1307, 1312, 1319, 1324, 1331, 1336, 1343, 1348, 1355,
        1213, 1218, 1227, 1232, 1239, 1244, 1251, 1256, 1263, 1268, 1275, 1280,
        1287, 1292, 1301, 1306, 1313, 1318, 1325, 1330, 1337, 1342, 1349, 1354 },

      { 1398, 1405, 1410, 1417, 1422, 1429, 1434, 1443, 1448, 1455, 1460, 1467,
        1472, 1479, 1484, 1491, 1496, 1503, 1508, 1517, 1522, 1529, 1534, 1541,
        1399, 1404, 1411, 1416, 1423, 1428, 1435, 1442, 1449, 1454, 1461, 1466,
        1473, 1478, 1485, 1490, 1497, 1502, 1509, 1516, 1523, 1528, 1535, 1540,
        1400, 1407, 1412, 1419, 1424, 1431, 1436, 1445, 1450, 1457, 1462, 1469,
        1474, 1481, 1486, 1493, 1498, 1505, 1512, 1519, 1524, 1531, 1536, 1543,
        1401, 1406, 1413, 1418, 1425, 1430, 1437, 1444, 1451, 1456, 1463, 1468,
        1475, 1480, 1487, 1492, 1499, 1504, 1513, 1518, 1525, 1530, 1537, 1542,
        1402, 1409, 1414, 1421, 1426, 1433, 1440, 1447, 1452, 1459, 1464, 1471,
        1476, 1483, 1488, 1495, 1500, 1507, 1514, 1521, 1526, 1533, 1538, 1545,
        1403, 1408, 1415, 1420, 1427, 1432, 1441, 1446, 1453, 1458, 1465, 1470,
        1477, 1482, 1489, 1494, 1501, 1506, 1515, 1520, 1527, 1532, 1539, 1544 },
      
      { 1578, 1587, 1592, 1599, 1604, 1611, 1616, 1623, 1628, 1635, 1640, 1647,
        1652, 1661, 1666, 1673, 1678, 1685, 1690, 1697, 1702, 1709, 1714, 1721,
        1579, 1586, 1593, 1598, 1605, 1610, 1617, 1622, 1629, 1634, 1641, 1646,
        1653, 1660, 1667, 1672, 1679, 1684, 1691, 1696, 1703, 1708, 1715, 1720,
        1580, 1589, 1594, 1601, 1606, 1613, 1618, 1625, 1630, 1637, 1642, 1649,
        1656, 1663, 1668, 1675, 1680, 1687, 1692, 1699, 1704, 1711, 1716, 1723,
        1581, 1588, 1595, 1600, 1607, 1612, 1619, 1624, 1631, 1636, 1643, 1648,
        1657, 1662, 1669, 1674, 1681, 1686, 1693, 1698, 1705, 1710, 1717, 1722,
        1584, 1591, 1596, 1603, 1608, 1615, 1620, 1627, 1632, 1639, 1644, 1651,
        1658, 1665, 1670, 1677, 1682, 1689, 1694, 1701, 1706, 1713, 1718, 1725,
        1585, 1590, 1597, 1602, 1609, 1614, 1621, 1626, 1633, 1638, 1645, 1650,
        1659, 1664, 1671, 1676, 1683, 1688, 1695, 1700, 1707, 1712, 1719, 1724 },

   };

#if DEBUG
/*
 * Convert bit vector to hex dump format and print
 */
static inline void
dump_cw(const voice_codeword& cw, int len, FILE* fp)  // len in bytes
{
        int i, j;
        for (i = 0; i < len; i++){
                int p = 0;
                for (j = 0; j <  8; j++){
                        p = (p << 1) + cw[ i*8 + j ];
                }
                fprintf(fp, "%02x ", p);
                if (!((i+1) % 16))
                        fprintf(fp, "\n");
        }
        fprintf(fp, "\n");
}
#endif

static inline void
imbe_store_bits(voice_codeword& cw, int s, int l, uint32_t v)
{
	for (int i = l-1; i >= s; i--) {
		cw[i] = v & 1;
		v >>= 1;
	}
}

static inline uint32_t
pngen15(uint32_t& Pr)
{
   int n = 0;
   for(int i = 14; i >= 0; --i) {
      Pr = (173 * Pr + 13849) & 0xffffu;
      if(Pr & 32768) {
         n += (1 << i);
      }
   }
   return n;
}

static inline uint32_t
pngen23(uint32_t& Pr)
{
   int n = 0;
   for(int i = 22; i >= 0; --i) {
      Pr = (173 * Pr + 13849) & 0xffffu;
      if(Pr & 32768) {
         n += (1 << i);
      }
   }
   return  n;
}

/* APCO IMBE header decoder.
 *
 * extracts 88 bits of IMBE parameters given an input 144-bit frame
 *
 * \param cw Codeword containing bits to be decoded
 * \param u0-u7 Result output vectors
 */

static inline void
imbe_header_decode(const voice_codeword& cw, uint32_t& u0, uint32_t& u1, uint32_t& u2, uint32_t& u3, uint32_t& u4, uint32_t& u5, uint32_t& u6, uint32_t& u7, uint32_t& E0, uint32_t& ET)
{
   ET = 0;

   size_t errs = 0;
   uint32_t v0 = extract(cw, 0, 23);
   errs = golay_23_decode(v0);
   u0 = v0;
   E0 = ET;

   uint32_t pn = u0 << 4;
   uint32_t m1 = pngen23(pn);
   uint32_t v1 = extract(cw, 23, 46) ^ m1;
   errs += golay_23_decode(v1);
   u1 = v1;

   uint32_t m2 = pngen23(pn);
   uint32_t v2 = extract(cw, 46, 69) ^ m2;
   errs += golay_23_decode(v2);
   u2 = v2;

   uint32_t m3 = pngen23(pn);
   uint32_t v3 = extract(cw, 69, 92) ^ m3;
   errs += golay_23_decode(v3);
   u3 = v3;

   uint32_t m4 = pngen15(pn);
   uint16_t v4 = extract(cw, 92, 107) ^ m4;
   errs += hamming_15_decode(v4);
   u4 = v4;

   uint32_t m5 = pngen15(pn);
   uint16_t v5 = extract(cw, 107, 122) ^ m5;
   errs += hamming_15_decode(v5);
   u5 = v5;

   uint32_t m6 = pngen15(pn);
   uint16_t v6 = extract(cw, 122, 137) ^ m6;
   errs += hamming_15_decode(v6);
   u6 = v6;

   u7 = extract(cw, 137, 144);
   u7 <<= 1; /* so that bit0 is free (see note about BOT bit */
}

/* APCO IMBE header encoder.
 *
 * given 88 bits of IMBE parameters, construct output 144-bit IMBE codeword
 *
 * \param cw Codeword into which 144 bits of results are placed
 * \param u0-u7 input parameters
 */

static inline void
imbe_header_encode(voice_codeword& cw, uint32_t u0, uint32_t u1, uint32_t u2, uint32_t u3, uint32_t u4, uint32_t u5, uint32_t u6, uint32_t u7)
{
	// given input 88-bit IMBE frame (in u0-u7),
	// add golay / hamming bits to produce a 144-bit frame.
	uint32_t c0, c1, c2, c3, c4, c5, c6, c7;

	uint32_t pn = u0 << 4;

	c0 = golay_23_encode(u0);
	imbe_store_bits(cw, 0, 23, c0);

	uint32_t m1 = pngen23(pn);
	c1 = golay_23_encode(u1) ^ m1;
	imbe_store_bits(cw, 23, 46, c1);

	uint32_t m2 = pngen23(pn);
	c2 = golay_23_encode(u2) ^ m2;
	imbe_store_bits(cw, 46, 69, c2);

	uint32_t m3 = pngen23(pn);
	c3 = golay_23_encode(u3) ^ m3;
	imbe_store_bits(cw, 69, 92, c3);

	uint32_t m4 = pngen15(pn);
	c4 = hamming_15_encode(u4) ^ m4;
	imbe_store_bits(cw, 92, 107, c4);

	uint32_t m5 = pngen15(pn);
	c5 = hamming_15_encode(u5) ^ m5;
	imbe_store_bits(cw, 107, 122, c5);

	uint32_t m6 = pngen15(pn);
	c6 = hamming_15_encode(u6) ^ m6;
	imbe_store_bits(cw, 122, 137, c6);

	c7 = u7;
	imbe_store_bits(cw, 137, 144, c7 >> 1);
}

static inline void
imbe_deinterleave (const_bit_vector& frame_body, voice_codeword& cw, uint32_t frame_nr)
{
      for(size_t j = 0; j < voice_codeword_sz; ++j) {
         cw[j] = frame_body[voice_codeword_bits[frame_nr][j]];
      }
}


static inline void
imbe_interleave(bit_vector& frame_body, const voice_codeword& cw, uint32_t frame_nr)
{
      for(size_t j = 0; j < voice_codeword_sz; ++j) {
         frame_body[voice_codeword_bits[frame_nr][j]] = cw[j];
      }
}

static inline void
imbe_frame_unpack(uint8_t *A, uint32_t& u0, uint32_t& u1, uint32_t& u2, uint32_t& u3, uint32_t& u4, uint32_t& u5, uint32_t& u6, uint32_t& u7, uint32_t& E0, uint32_t& ET)
{
   E0 = A[0];
   ET = A[1];
   u0 = A[4] + (E0 & 240) * 16;
   u1 = A[5] + (E0 & 15) * 256;
   u2 = A[6] + (ET & 240) * 16;
   u3 = A[7] + (ET & 15) * 256;
   E0 = A[2];
   ET = A[3];
   u4 = A[8] + (E0 & 224) * 8;
   u5 = A[9] + (E0 & 28) * 64;
   u6 = A[10] + (ET & 128) * 2 + (E0 & 3) * 512;
   u7 = ET & 127;
   E0 = A[11];
   if(E0 & 192) exit(4);
   ET = E0 & 3;
   E0 = (E0 & 84) / 4;
}

/*
 * Given a 144-bit IMBE frame(cw),
 * 1. Decode frame to error-corrected 88-bit format
 * 2. Re-encode frame into 144-bit format with refreshed Hamming/Golay encoding
 * 3. FIXME: track decoding error rates and stats
 */
static inline void
imbe_regenerate_frame(bit_vector& cw) {
        unsigned int u0 = 0;
        unsigned int u1,u2,u3,u4,u5,u6,u7;
        unsigned int E0 = 0;
        unsigned int ET = 0;

        // PN/Hamming/Golay - etc.
        imbe_header_decode(cw, u0, u1, u2, u3, u4, u5, u6, u7, E0, ET) ;
#if DEBUG
	printf("u_0: %x\r\nu_1: %x\r\nu_2: %x\r\nu_3: %x\r\nu_4: %x\r\nu_5: %x\r\nu_6: %x\r\nu_7: %x\r\n\r\n", u0, u1, u2, u3, u4, u5, u6, u7>>1);
#endif
        imbe_header_encode(cw, u0, u1, u2, u3, u4, u5, u6, u7);
}

/*
 * process voice units (ldu1 or ldu2)
 * perform error correction for each of the nine embedded 144-bit frames
 * by regenerating the error-correction coding bits
 */
static inline void
imbe_regenerate_voice_unit(bit_vector& frame_body) {
	voice_codeword cw(voice_codeword_sz);
#if DEBUG
	dump_cw(frame_body, 216, stderr);
#endif
	for(size_t i = 0; i < nof_voice_codewords; ++i) {
		imbe_deinterleave(frame_body, cw, i);
		imbe_regenerate_frame(cw);
		imbe_interleave(frame_body, cw, i);
	}
#if DEBUG
	dump_cw(frame_body, 216, stderr);
#endif
}

#endif /* INCLUDED_IMBE_FRAME_H */