/*
 * Copyright (C) 2019 Kai Ludwig, DG4KLU
 *           (C) 2020-2022 Roger Clark, VK3KYY / G4KYF
 *
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
 *    in the documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * 4. Use of this source code or binary releases for commercial purposes is strictly forbidden. This includes, without limitation,
 *    incorporation in a commercial product or incorporation into a product or project which allows commercial use.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "dmr_codec/codec.h"
#include "functions/voicePrompts.h"
#include <string.h>

static uint16_t bitbuffer_encode[72];

void codecDecode(uint8_t *indata_ptr, int numbBlocks)
{
	uint16_t bitbuffer_decode[49];

    for (int idx = 0; idx < numbBlocks; idx++)
    {
		initFrame(indata_ptr, bitbuffer_decode);
		indata_ptr += 9;

		soundSetupBuffer();// this just sets currentWaveBuffer but the compiler seems to optimise out the code if I try to do it in this file

		asm volatile (
			"MOV R2, %0\n"
			"MOV R0, %1\n"
			"MOV R1, %2\n"
			"PUSH {R4-R11}\n"
			"SUB SP, SP, #0x10\n"
			"STR R1, [SP, #0x08]\n"
			"LDR R1, =0\n"
			"STR R1, [SP, #0x04]\n"
			"LDR R1, =0\n"
			"STR R1, [SP, #0x00]\n"
			"LDR R3, =0\n"
			"LDR R1, =80\n"
			"BL " QU(AMBE_DECODE)
			"ADD SP, SP, #0x10\n"
			"POP {R4-R11}"
			:
			: "r"(bitbuffer_decode),
			  "r"(currentWaveBuffer),
			  "r"(ambebuffer_decode)
			: "memory"
		);

		soundStoreBuffer();

		soundSetupBuffer();// this just sets currentWaveBuffer but the compiler seems to optimise out the code if I try to do it in this file

		asm volatile (
			"MOV R2, %0\n"
			"MOV R0, %1\n"
			"MOV R1, %2\n"
			"PUSH {R4-R11}\n"
			"SUB SP, SP, #0x10\n"
			"STR R1, [SP, #0x08]\n"
			"LDR R1, =1\n"
			"STR R1, [SP, #0x04]\n"
			"LDR R1, =0\n"
			"STR R1, [SP, #0x00]\n"
			"LDR R3, =0\n"
			"LDR R1, =80\n"
			"BL " QU(AMBE_DECODE)
			"ADD SP, SP, #0x10\n"
			"POP {R4-R11}"
			:
			: "r"(bitbuffer_decode),
			  "r"(currentWaveBuffer),
			  "r"(ambebuffer_decode)
			: "memory"
		);

		soundStoreBuffer();
    }
}

void codecEncodeBlock(uint8_t *outdata_ptr)
{
	memset((uint8_t *)outdata_ptr, 0, 9);// fills with zeros
	memset(bitbuffer_encode, 0, sizeof(bitbuffer_encode));// faster to call memset as it will be compiled as optimised code


	soundRetrieveBuffer();// gets currentWaveBuffer pointer used as input r2 to the encoder

	asm volatile (
		"MOV R0, %0\n"
		"MOV R2, %1\n"
		"MOV R1, %2\n"
		"PUSH {R4-R11}\n"
		"SUB SP, SP, #0x14\n"
		"STR R1, [SP, #0x0C]\n"
		"LDR R1, =0x00002000\n"
		"STR R1, [SP, #0x08]\n"
		"LDR R1, =0\n"
		"STR R1, [SP, #0x04]\n"
		"LDR R1, =0x00001840\n"
		"STR R1, [SP, #0x00]\n"
		"LDR R3, =80\n"
		"LDR R1, =0\n"
		"BL " QU(AMBE_ENCODE)
		"ADD SP, SP, #0x14\n"
		"POP {R4-R11}"
		:
		: "r"(bitbuffer_encode),
		  "r"(currentWaveBuffer), //tmp_wavbuffer
		  /* seems to be a hard coded (defined) memory address of
		     0x1FFF6B60. I'm not sure why it has to be hard coded,
		     since its passed as a parameter (register) */
		  "r"(ambebuffer_encode)
		: "memory"
	);

	soundRetrieveBuffer();// gets currentWaveBuffer pointer used as input r2 to the encoder

	asm volatile (
		"MOV R0, %0\n"
		"MOV R2, %1\n"
		"MOV R1, %2\n"
		"PUSH {R4-R11}\n"
		"SUB SP, SP, #0x14\n"
		"STR R1, [SP, #0x0C]\n"
		"LDR R1, =0x00002000\n"
		"STR R1, [SP, #0x08]\n"
		"LDR R1, =1\n"
		"STR R1, [SP, #0x04]\n"
		"LDR R1, =0x00000800\n"
		"STR R1, [SP, #0x00]\n"
		"LDR R3, =80\n"
		"LDR R1, =0\n"
		"BL " QU(AMBE_ENCODE)
		"ADD SP, SP, #0x14\n"
		"POP {R4-R11}"
		:
		: "r"(bitbuffer_encode),
		  "r"(currentWaveBuffer), //tmp_wavbuffer
		  "r"(ambebuffer_encode)
		: "memory"
	);

	asm volatile (
		"MOV R0, %0\n"
		"MOV R1, %1\n"
		"PUSH {R4-R11}\n"
		"SUB SP, SP, #0x14\n"
		"MOV R3, R1\n"
		"LDR R2, =0\n"
		"MOV R1, R0\n"
		"BL " QU(AMBE_ENCODE_ECC)
		"ADD SP, SP, #0x14\n"
		"POP {R4-R11}"
		:
		: "r"(bitbuffer_encode),
		  "r"(ambebuffer_encode_ecc)
		: "memory"
	);

	for (int i = 0; i < 72; i++)
	{
		if (bitbuffer_encode[i] & 1)
		{
			outdata_ptr[i >> 3] |= 128 >> (i & 7);
		}
	}
}