/*
 * (C) 2016 by sysmocom - s.f.m.c. GmbH
 * All Rights Reserved
 *
 * SPDX-License-Identifier: GPL-2.0+
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>

#include <osmocom/core/utils.h>
#include <osmocom/codec/codec.h>

const uint8_t sid_update[] = {0x20, 0x44, 0x29, 0xc2, 0x92, 0x91, 0xf4};
const uint8_t sid_first[] = {0x20, 0x44, 0x00, 0x00, 0x00, 0x00, 0x04};

#define PAYLOAD_LEN 34
#define SID_LEN 7

static const char * cmpr(int a, int b)
{
	return (a == b) ? "OK" : "BAD";
}

static void test_sid_dec(const uint8_t *t, size_t len)
{
	uint8_t cmr, tmp[SID_LEN], *t2 = NULL;
	enum osmo_amr_type ft;
	enum osmo_amr_quality bfi;
	int8_t sti, cmi;
	if (t) {
		memcpy(tmp, t, SID_LEN);
		t2 = tmp;
	}
	int rc = osmo_amr_rtp_dec(t2, len, &cmr, &cmi, &ft, &bfi, &sti);
	if (rc < 0)
		return;
	printf("[%d] decode RTP %s%s: FT %s, CMR %s, CMI is %d, SID type %s\t",
	       rc, osmo_hexdump(tmp, len), cmpr(bfi, AMR_GOOD),
	       get_value_string(osmo_amr_type_names, ft),
	       get_value_string(osmo_amr_type_names, cmr),
	       cmi, sti ? "UPDATE" : "FIRST");
	if (sti == -1)
		printf("FAIL: incompatible STI for SID\n");
	rc = osmo_amr_rtp_enc(tmp, cmr, ft, bfi);
	printf("[%d] encode [%d]\n", rc, memcmp(tmp, t, SID_LEN));
}

static void test_amr_rt(uint8_t _cmr, enum osmo_amr_type _ft,
			enum osmo_amr_quality _bfi)
{
	uint8_t cmr, payload[PAYLOAD_LEN];
	enum osmo_amr_type ft;
	enum osmo_amr_quality bfi;
	int8_t sti, cmi;
	int rc, re = osmo_amr_rtp_enc(payload, _cmr, _ft, _bfi);
	rc = osmo_amr_rtp_dec(payload, PAYLOAD_LEN, &cmr, &cmi, &ft, &bfi, &sti);
	printf("[%d/%d] %s, CMR: %s, FT: %s, BFI: %s, CMI: %d, STI: %d\n", re,
	       rc, get_value_string(osmo_amr_type_names, ft),
	       cmpr(_cmr, cmr), cmpr(_ft, ft), cmpr(_bfi, bfi), cmi, sti);
}

uint8_t fr[] = {0xd8, 0xa9, 0xb5, 0x1d, 0xda, 0xa8, 0x82, 0xcc, 0xec, 0x52,
		      0x29, 0x05, 0xa8, 0xc3, 0xe3, 0x0e, 0xb0, 0x89, 0x7a, 0xee,
		      0x42, 0xca, 0xc4, 0x97, 0x22, 0xe6, 0x9e, 0xa8, 0xb8, 0xec,
		      0x52, 0x26, 0xbd};
uint8_t sid_fr[] = {0xd7, 0x27, 0x93, 0xe5, 0xe3, 0x00, 0x00, 0x00, 0x00,
			  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
			  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
			  0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

uint8_t hr[] = {0x06, 0x46, 0x76, 0xb1, 0x8e, 0x48, 0x9a, 0x2f, 0x5e, 0x4c,
		      0x22, 0x2b, 0x62, 0x25};
uint8_t sid_hr[] = {0x03, 0x8e, 0xb6, 0xcb, 0xff, 0xff, 0xff, 0xff, 0xff,
			  0xff, 0xff, 0xff, 0xff, 0xff};

static void test_sid_xr(uint8_t *t, size_t len, bool hr)
{
	printf("%s SID ? %s:: %d\n", hr ? "HR" : "FR", osmo_hexdump(t, len),
	       hr ? osmo_hr_check_sid(t, len) : osmo_fr_check_sid(t, len));
}

const uint8_t fr_sid1[33] = { 0xd7, 0xe0, 0xa3, 0x61, 0x2c, 0x00, 0x00,
	0x80, 0x49, 0x00, 0x00, 0x49, 0x00, 0x00, 0x80, 0x00, 0x04,
	0x00, 0x00, 0x00, 0x00, 0x12, 0x40, 0x00, 0x10, 0x08, 0x00,
	0x00, 0x00, 0x08, 0x2d, 0x04, 0x09 };

const uint8_t fr_sid2[33] = { 0xd7, 0xa2, 0xbb, 0x65, 0xe5, 0x00, 0x00,
	0x10, 0x00, 0x04, 0x82, 0x00, 0x00, 0x00, 0x00, 0x49, 0x00,
	0x02, 0x41, 0x00, 0x00, 0x00, 0x00, 0x20, 0x80, 0x01, 0x00,
	0x00, 0x00, 0x42, 0x40, 0x10, 0xd2 };

const uint8_t fr_sid3[33] = { 0xd9, 0x60, 0xab, 0x21, 0xea, 0x00, 0x00,
	0x80, 0x48, 0x20, 0x00, 0x09, 0x00, 0x00, 0x00, 0x00, 0x24,
	0x80, 0x00, 0x00, 0x00, 0x10, 0x08, 0x00, 0x10, 0x08, 0x00,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x08 };

const uint8_t fr_sid4[33] = { 0xd8, 0x21, 0xab, 0x25, 0xea, 0x00, 0x00,
	0x00, 0x00, 0x04, 0x10, 0x00, 0x00, 0x00, 0x02, 0x41, 0x00,
	0x02, 0x48, 0x00, 0x00, 0x00, 0x01, 0x20, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x42, 0x00, 0x02, 0x50 };

const uint8_t fr_sid5[33] = { 0xd9, 0x61, 0x9a, 0x65, 0x60, 0x00, 0x00,
	0x10, 0x00, 0x04, 0x02, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
	0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x01, 0x00,
	0x00, 0x00, 0x02, 0x40, 0x82, 0x52 };

const uint8_t fr_sid6[33] = { 0xd9, 0xa5, 0x93, 0xe1, 0xa0, 0x00, 0x00,
	0x10, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x02, 0x09, 0x00,
	0x02, 0x40, 0x00, 0x00, 0x00, 0x01, 0x20, 0x80, 0x01, 0x00,
	0x00, 0x90, 0x02, 0x40, 0x02, 0x02 };

const uint8_t fr_sid7[33] = { 0xd8, 0x20, 0xa3, 0xe5, 0xaa, 0x00, 0x00,
	0x80, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x80, 0x00, 0x04,
	0x80, 0x00, 0x00, 0x00, 0x10, 0x08, 0x00, 0x12, 0x08, 0x00,
	0x00, 0x00, 0x00, 0x08, 0x00, 0x09 };

const uint8_t fr_sid8[33] = { 0xd7, 0xe0, 0xa3, 0xa1, 0x60, 0x00, 0x00,
	0x10, 0x00, 0x04, 0x92, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00,
	0x00, 0x40, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x12, 0x00, 0x40, 0x12, 0x00 };

const uint8_t fr_sid9[33] = { 0xd8, 0x20, 0xa3, 0xa5, 0x62, 0x00, 0x00,
	0x10, 0x00, 0x04, 0x82, 0x00, 0x00, 0x00, 0x02, 0x49, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x24, 0x00, 0x00, 0x00,
	0x00, 0x10, 0x02, 0x00, 0x82, 0x02 };

const uint8_t fr_sid10[33] = { 0xd4, 0xaa, 0xba, 0x6e, 0xbb, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x10, 0x00, 0x20,
	0x90, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x02, 0x08, 0x00,
	0x00, 0x00, 0x08, 0x2c, 0x20, 0x00 };

const uint8_t fr_sid11[33] = { 0xd9, 0x64, 0xbb, 0x6d, 0x62, 0x00, 0x00,
	0x80, 0x41, 0x00, 0x00, 0x48, 0x00, 0x00, 0x10, 0x00, 0x04,
	0x90, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x2c, 0x24, 0x01 };

const uint8_t fr_sid12[33] = { 0xd8, 0x61, 0xb2, 0xa5, 0x62, 0x00, 0x00,
	0x00, 0x41, 0x20, 0x00, 0x48, 0x00, 0x00, 0x10, 0x00, 0x04,
	0x90, 0x00, 0x00, 0x00, 0x10, 0x08, 0x00, 0x10, 0x40, 0x00,
	0x00, 0x00, 0x01, 0x29, 0x24, 0x08 };

const uint8_t fr_sid13[33] = { 0xd9, 0x23, 0xba, 0xe5, 0xe2, 0x00, 0x00,
	0x80, 0x41, 0x20, 0x00, 0x01, 0x00, 0x00, 0x10, 0x00, 0x04,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x48, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x09, 0x05, 0x20, 0x00 };

const uint8_t fr_sid14[33] = { 0xd8, 0x62, 0xa2, 0x61, 0x60, 0x00, 0x00,
	0x10, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00,
	0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x01, 0x00,
	0x00, 0x80, 0x00, 0x40, 0x02, 0x40 };

const uint8_t fr_sid15[33] = { 0xd9, 0xe4, 0xc3, 0x6d, 0x12, 0x00, 0x00,
	0x80, 0x00, 0x20, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x10, 0x00, 0x00, 0x00, 0x10, 0x48, 0x00, 0x10, 0x48, 0x00,
	0x00, 0x00, 0x00, 0x2d, 0x04, 0x00 };

const uint8_t fr_sid16[33] = { 0xd9, 0xa4, 0xc3, 0x29, 0x59, 0x00, 0x00,
	0x10, 0x00, 0x00, 0x12, 0x00, 0x00, 0x00, 0x00, 0x41, 0x00,
	0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x80, 0x00, 0x00,
	0x00, 0x00, 0x42, 0x00, 0x12, 0x02 };

static const uint8_t *fr_sids[] = { fr_sid1, fr_sid2, fr_sid3, fr_sid4,
	fr_sid5, fr_sid6, fr_sid7, fr_sid8, fr_sid9, fr_sid10, fr_sid11,
	fr_sid12, fr_sid13, fr_sid14, fr_sid15, fr_sid16 };

static void test_sid_fr(void)
{
	unsigned int i;

	for (i = 0; i < ARRAY_SIZE(fr_sids); i++) {
		int rc = osmo_fr_check_sid(fr_sids[i], 33);
		printf("FR SID %s: %d\n", osmo_hexdump(fr_sids[i], 33), rc);
	}
}



static void test_amr_s_d(void)
{
	ubit_t in[244];
	ubit_t mid[244];
	ubit_t out[244];
	int i, j;

	for (j = AMR_4_75; j <= AMR_12_2; j++) {
		unsigned int n_bits = gsm690_bitlength[j];

		printf("=> AMR Mode %d (%d bits)\n", j, n_bits);
		/* set a single bit in the input buffer */
		for (i = 0; i < n_bits; i++) {

			memset(in, 0, sizeof(in));
			in[i] = 1;

			/* re-order from s to d */
			osmo_amr_s_to_d(mid, in, n_bits, j);

			/* and back to d */
			osmo_amr_d_to_s(out, mid, n_bits, j);

			if (memcmp(in, out, n_bits)) {
				printf("Error in bit %d of mode %d!\n", i, j);
				printf("inp s-bits: %s\n", osmo_ubit_dump(in, n_bits));
				printf("mid d-bits: %s\n", osmo_ubit_dump(mid, n_bits));
				printf("out s-bits: %s\n", osmo_ubit_dump(out, n_bits));
				//OSMO_ASSERT(0);
			}
		}
	}
}

int main(int argc, char **argv)
{
	printf("AMR RTP payload decoder test:\n");
	test_sid_dec(sid_first, 7);
	test_sid_dec(sid_update, 7);
	test_sid_dec(NULL, 7);
	test_amr_rt(0, AMR_NO_DATA, AMR_BAD);
	test_amr_rt(0, AMR_NO_DATA, AMR_GOOD);
	test_amr_rt(AMR_12_2, AMR_12_2, AMR_BAD);
	test_amr_rt(AMR_12_2, AMR_12_2, AMR_GOOD);
	test_amr_rt(AMR_7_40, AMR_7_40, AMR_BAD);
	test_amr_rt(AMR_7_40, AMR_7_40, AMR_GOOD);
	printf("FR RTP payload SID test:\n");
	test_sid_xr(sid_fr, 33, false);
	test_sid_xr(fr, 33, false);

	printf("HR RTP payload SID test:\n");
	test_sid_xr(sid_hr, 14, true);
	test_sid_xr(hr, 14, true);

	printf("FR RTP payload SID test:\n");
	test_sid_fr();

	printf("AMR s/d bit re-ordering test:\n");
	test_amr_s_d();

	return 0;
}