/*
 * aesa_emu.cpp
 *
 *  Created on: 22/set/2023
 */

#include "aesa_emu.h"

#include "aesa_if_types.h"
#include "grifo_aesa_fw_if.h"

#include "grifo_fw_blob.h"

#include <string.h>

#include "rtlog.h"

#pragma GCC optimize("-O0")

struct aesa_emu_t
{
	int inj_cti_crc;
	int inj_cti_coding;
	int inj_cti_timeout;
	int inj_link_err;
	int inj_duty_err;
	int inj_beam_err;
	int inj_plank_resp_err;
	int inj_rfif_resp_err;
	int inj_rfif_duty_err;
	int inj_rfif_cti_err;

	int inj_counter;

	unsigned int plank_present_mask;
	unsigned int plank_mask;
	unsigned int trc_mask[32];

	aesa_emu_t()
	{
		plank_present_mask=0
		|(1<<1)
		|(1<<6)
		|(1<<7)
		|(1<<25)
		|(1<<29);

		plank_mask=0
		|(1<<0)
		|(1<<2)
		|(1<<3)
		|(1<<4)
		|(1<<5)
		|(1<<26)
		|(1<<27)
		|(1<<28)
		|(1<<30)
		|(1<<31);

#if 1
		trc_mask[9]=0
				|(1<<1)
				|(1<<8)
				|(1<<11)
				|(1<<12)
				|(1<<13);

		trc_mask[12]=0
				|(1<<18);

		trc_mask[19]=0
				|(1<<2)
				|(1<<8);

		trc_mask[21]=0
				|(1<<4)
				|(1<<6)
				|(1<<7);

		trc_mask[23]=0
				|(1<<1)
				|(1<<4)
				|(1<<16)
				|(1<<17);

		trc_mask[24]=0
				|(1<<1)
				|(1<<2)
				|(1<<6)
				|(1<<7)
				|(1<<8)
				|(1<<10)
				|(1<<17);

		trc_mask[31]=0
				|(1<<4)
				|(1<<8);
#endif
	}
};

aesa_emu_t autoMonitorable_aesaemu;

enum emu_common_enums_t
{
    MAX_PLANKS = 64,
    MAX_TRC_PER_PLANKS = 32,
    NUM_PLANK_PCBS = 32,
    NUM_TRCS = 20
};

//## type trc_map_status_t
enum emu_trc_map_status_t
{
    NO_MAPPING_FAILURE = 0,
    LINK_FAILURE = 1,
    MAP_PASS_1_BIT_FAILURE = 2,
    MAP_PASS_2_BIT_FAILURE = 3
};

struct EmuAesaStatusTrcResponse
{
    enum { trc_per_byte=4, trc_per_plank=20};

    uint8_t trcx_raw[trc_per_plank/trc_per_byte];

    uint8_t extractByte(unsigned int trcn) const
    {
        unsigned int bindex=trcn/4;
        return trcx_raw[bindex];
    }
    uint8_t extractTrc(unsigned int trcn) const
    {
        unsigned int bindex=trcn/4;
        uint8_t sts=trcx_raw[bindex];
        unsigned int shift=trcn & (trc_per_byte-1);
        sts=sts>>(shift*2);
        sts=sts & 0x3;
        return sts;
    }

    bool fail(unsigned int trcn) const
    {
        return extractTrc(trcn)!=0;
    }
    bool commFault(unsigned int trcn) const
    {
        return extractTrc(trcn) & 1;
    }
    bool otherFault(unsigned int trcn) const
    {
        return extractTrc(trcn) & 2;
    }
};
//#]

//#[ type AesaStatusResponse
struct AesaStatusResponse
{
    uint8_t first_plank;
    uint8_t last_plank;

    aesa::cti_status_t cti_sts;
    aesa::cti_extended_status_t cti_ext_sts;
    aesa::cti_eti_readback_status_t cti_eti;
    struct plank_trcx_sts_t
    {
        aesa::plank_status_t plank_sts;
        EmuAesaStatusTrcResponse trc_sts;
    } plank[NUM_PLANK_PCBS];
};
//#]

//#[ type DownloadMapResponse
struct DownloadMapResponse
{
    enum { RESERVED_BYTES=35 };

    uint8_t first_plank;
    uint8_t last_plank;

    aesa::cti_status_t cti_sts;
    aesa::cti_extended_status_t cti_ext_sts;
    aesa::cti_eti_readback_status_t cti_eti;
    uint8_t reserved[RESERVED_BYTES];
    struct plank_ext_trcx_sts_t
    {
        aesa::plank_status_t plank_sts;
        aesa::plank_extended_status_t plank_ext_sts;
        aesa::dwl_map_trc_status_t trc_status[MAX_TRC_PER_PLANKS];
    } plank[NUM_PLANK_PCBS];
};

static unsigned int xerr(int x)
{
	if (x==0) return 0;
	if (x==1) return 1;

	x=(1<<x);
	if (x & autoMonitorable_aesaemu.inj_counter)
		return 1;
	return 0;
}

struct aesa_emu_msg_t
{
	aesa::cti_status_t cti_sts;

	aesa::aesa_firmware_status_t fw_sts;

	aesa::rfif_status_t rfif_sts;
	aesa::rfif_fw_status_t rfif_fw;

	aesa::rfif_bit_status_t rfif_bit;

	aesa::aesa_broadcast_status_t bcst_sts;

	AesaStatusResponse stsResp;
	DownloadMapResponse mapResp;

	uint32_t imuResp[9];

	aesa_emu_msg_t()
	{
		fw_sts.aesa_fw_status_1.cti_fw_version.major=2;
		fw_sts.aesa_fw_status_1.cti_fw_version.minor=5;

		stsResp.first_plank=1;
		stsResp.last_plank=NUM_PLANK_PCBS;

		mapResp.first_plank=1;
		mapResp.last_plank=NUM_PLANK_PCBS;

		rebuildMap(true);
	}

	void rebuildBroadcastStatus()
	{
		bcst_sts.byte_4.bit.rfif_response_error=xerr(autoMonitorable_aesaemu.inj_rfif_resp_err);
		bcst_sts.byte_0.raw=0;
		bcst_sts.byte_1.raw=0;
		bcst_sts.byte_2.raw=0;
		bcst_sts.byte_3.raw=0;
		if (autoMonitorable_aesaemu.inj_plank_resp_err)
		{
			unsigned int rmap=autoMonitorable_aesaemu.inj_plank_resp_err;
			uint8_t* b=&bcst_sts.byte_0.raw;
			for(unsigned int i=0; i<32; ++i)
			{
				if (rmap & (1<<i))
				{
					unsigned int n=i % 8;
					unsigned int bit=i & 0x0FF;
					b[n]|=(1<<bit);
				}
			}
		}

	}

	void rebuildRfifSts()
	{
		rfif_sts.byte_2.bit.cti_coding_error=xerr(autoMonitorable_aesaemu.inj_rfif_cti_err);
		rfif_sts.byte_5.bit.rfif_rxn1_lost=xerr(autoMonitorable_aesaemu.inj_rfif_duty_err);
	}

	void rebuildMap(bool fullMap)
	{
		for(unsigned int i=0; i<32; ++i)
		{
			stsResp.plank[i].plank_sts.byte_0.plank_id=i+1;
			mapResp.plank[i].plank_sts.byte_0.plank_id=i+1;
			mapResp.plank[i].plank_ext_sts.byte_0.plank_id=i+1;

			if ((autoMonitorable_aesaemu.plank_present_mask & (1<<i))==0)
			{
				stsResp.plank[i].plank_sts.byte_2.bit.trc_init_complete=1;

				mapResp.plank[i].plank_sts.byte_2.bit.trc_init_complete=1;
				mapResp.plank[i].plank_ext_sts.byte_5.bit.input_vrx_supply_present=1;
				mapResp.plank[i].plank_ext_sts.byte_5.bit.input_vtx_supply_present=1;

				if ((autoMonitorable_aesaemu.plank_mask & (1<<i)))
				{
					stsResp.plank[i].plank_sts.byte_1.bit.flash_beam_calc_ibit_error=1;
					mapResp.plank[i].plank_sts.byte_1.bit.flash_beam_calc_ibit_error=1;
				}
				if (fullMap)
				{
					for(unsigned int j=0; j<32; ++j)
					{
						if (autoMonitorable_aesaemu.trc_mask[i] & (1<<j))
						{
							mapResp.plank[i].trc_status[j].byte_1.raw=0x1;
						}
					}
				}
			}

		}
		bcst_sts.byte_0.bit.plank_2_response_error=1;

		pending=-1;
	}

	unsigned int tx[2][4096];
	unsigned int rx[2][4096];

	int pending;

	uint32_t* rxp[2];
};

aesa_emu_msg_t aesa_emu_msg;

class EmuAesaStream
{
public:
    uint8_t* d;
    unsigned int s;
    unsigned int pos;

    EmuAesaStream(void* p, unsigned int max_size):
        d(reinterpret_cast<uint8_t*>(p)),
        s(max_size),
        pos(3)
    {
    }

    void push(uint8_t v)
    {
        d[pos]=v;
        if (pos==0)
        {
            pos=3;
            d+=4;
        }
        else
            --pos;
    }
    void push32(uint32_t v)
    {
        push(v & 0xFF);
        push((v>>8) & 0xFF);
        push((v>>16) & 0xFF);
        push((v>>24) & 0xFF);
    }
    void push16(uint16_t v)
    {
        push(v & 0xFF);
        push((v>>8) & 0xFF);
    }
    void push16(int16_t v)
    {
        push16(static_cast<uint16_t>(v));
    }
};

static void add_response(uint32_t* dst, unsigned int& pos, unsigned int id, unsigned int tag, const void* data, unsigned int len)
{
	dst[pos++]=grifo_fw::grifo_fw_block_raw_helper_t::k_som;
	const uint8_t* src=reinterpret_cast<const uint8_t*>(data);
	EmuAesaStream s(&dst[pos], len);
	s.push(id);
	s.push(tag);
	for(unsigned int i=0; i<(len-2); ++i)
	{
		s.push(src[i]);
	}
	unsigned int r_len=(len+3)>>2;
	pos+=r_len;
	dst[pos++]=r_len<<16;
	dst[pos]=grifo_fw::grifo_fw_block_raw_helper_t::k_eob;
}

static void aesa_execute(void* reply, const void* cmd)
{
	uint32_t* dst=(uint32_t*)reply;
	unsigned int cursor=0;

	for(const uint32_t* p=&((uint32_t*)cmd)[8]; p[0]==grifo_fw::grifo_fw_block_raw_helper_t::k_som;)
	{
		grifo_fw::aesa_tx_header_t ctrl;
		ctrl.cw.raw=p[1];

		auto len=ctrl.cw.fields.payload_len;
		auto nresp=ctrl.cw.fields.response;

        //memset(cmd_buffer, 0, sizeof cmd_buffer);
        //grifo_fw::aesa_payload_swap(cmd_buffer, &p[2], len);
		unsigned int msg_id=p[2]>>24;
		unsigned int tag=(p[2]>>16) & 0x0FF;

		dst[cursor++]=grifo_fw::grifo_fw_block_raw_helper_t::k_som;

		if (xerr(autoMonitorable_aesaemu.inj_cti_timeout))
		{
			dst[cursor++]=grifo_fw::aesa_message_descriptor_t::timeout_on_rx_res;
		}
		else
		{
			EmuAesaStream s(&dst[cursor], CTI_STATUS_RSP_SIZE);
			s.push(CTI_STATUS_RSP);
			s.push((unsigned char)tag);

			aesa_emu_msg.cti_sts.byte_0.bit.coding_error=xerr(autoMonitorable_aesaemu.inj_cti_coding);
					//autoMonitorable_aesaemu.inj_cti_coding ? 1 : 0;
			aesa_emu_msg.cti_sts.byte_0.bit.crc_error=xerr(autoMonitorable_aesaemu.inj_cti_crc);

			aesa_emu_msg.cti_sts.byte_1.bit.tx_rx_duty_ratio_error=xerr(autoMonitorable_aesaemu.inj_duty_err);
			aesa_emu_msg.cti_sts.byte_1.bit.cmd_link1_message_fail=xerr(autoMonitorable_aesaemu.inj_link_err); // ? 1 : 0;
			aesa_emu_msg.cti_sts.byte_1.bit.px_uv_beam_pointing_cmd_error=xerr(autoMonitorable_aesaemu.inj_beam_err);

			s.push(aesa_emu_msg.cti_sts.byte_0.raw);
			s.push(aesa_emu_msg.cti_sts.byte_1.raw);
			s.push(aesa_emu_msg.cti_sts.byte_2.raw);
			s.push(aesa_emu_msg.cti_sts.byte_3.raw);

	#if 0
			for(unsigned int i=2; i<(CTI_STATUS_RSP_SIZE); ++i)
				s.push((unsigned char)0);
	#endif
			unsigned int r_len=(CTI_STATUS_RSP_SIZE+3)>>2;
			cursor+=r_len;
			dst[cursor++]=r_len<<16;

			unsigned int trailes=nresp;

			if ((aesa_emu_msg.cti_sts.byte_0.raw==0)) // && (aesa_emu_msg.cti_sts.byte_1.raw==0) && (aesa_emu_msg.cti_sts.byte_2.raw==0))
			{
				switch(msg_id)
				{
				case AESA_READ_STATUS_CMD:
					aesa_emu_msg.rebuildMap(false);
					add_response(dst, cursor, AESA_STATUS_RSP, tag, &aesa_emu_msg.stsResp, sizeof aesa_emu_msg.stsResp); //AESA_STATUS_RSP_SIZE);
					--trailes;
					break;

				case DOWNLOAD_MAP_CMD:
					aesa_emu_msg.rebuildMap(true);
					add_response(dst, cursor, DOWNLOAD_MAP_RSP, tag, &aesa_emu_msg.mapResp, sizeof aesa_emu_msg.mapResp);
					--trailes;
					break;

				case AESA_READ_FIRMWARE_STATUS_CMD:
					add_response(dst, cursor, AESA_FIRMWARE_STATUS_RSP, tag, &aesa_emu_msg.fw_sts, AESA_FIRMWARE_STATUS_RSP_SIZE);
					--trailes;
					break;
				case RFIF_READ_STATUS_CMD:
					aesa_emu_msg.rebuildRfifSts();
					add_response(dst, cursor,RFIF_STATUS_RSP, tag,  &aesa_emu_msg.rfif_sts, RFIF_STATUS_RSP_SIZE);
					--trailes;
					break;
				case RFIF_READ_FW_STATUS_CMD:
					add_response(dst, cursor,RFIF_FIRMWARE_STATUS_RSP, tag,  &aesa_emu_msg.rfif_fw, RFIF_FIRMWARE_STATUS_RSP_SIZE);
					--trailes;
					break;
				case RFIF_READ_BIT_STATUS_CMD:
					add_response(dst, cursor,RFIF_BIT_STATUS_RSP, tag,  &aesa_emu_msg.rfif_bit, RFIF_BIT_STATUS_RSP_SIZE);
					--trailes;
					break;
				case RFIF_MODE_CMD:
					aesa_emu_msg.rebuildRfifSts();
					add_response(dst, cursor,RFIF_STATUS_RSP, tag,  &aesa_emu_msg.rfif_sts, RFIF_STATUS_RSP_SIZE);
					--trailes;
					break;

				case PLANK_TRC_INITIALISE_CMD:
					aesa_emu_msg.rebuildBroadcastStatus();
					add_response(dst, cursor,AESA_BROADCAST_STATUS_RSP, tag,  &aesa_emu_msg.bcst_sts, AESA_BROADCAST_STATUS_RSP);
					--trailes;
					break;
				case PLANK_VTX_CONTROL_CMD:
					aesa_emu_msg.rebuildBroadcastStatus();
					add_response(dst, cursor,AESA_BROADCAST_STATUS_RSP, tag,  &aesa_emu_msg.bcst_sts, AESA_BROADCAST_STATUS_RSP);
					--trailes;
					break;

				case BEAM_CALCULATION_CMD:
					++autoMonitorable_aesaemu.inj_counter;
					add_response(dst, cursor,CTI_IMU_STATUS_RSP, tag,  &aesa_emu_msg.imuResp, CTI_IMU_STATUS_RSP_SIZE);
					--trailes;
					aesa_emu_msg.rebuildBroadcastStatus();
					add_response(dst, cursor,AESA_BROADCAST_STATUS_RSP, tag,  &aesa_emu_msg.bcst_sts, AESA_BROADCAST_STATUS_RSP);
					--trailes;
					break;

				default:
                    //rtlog_printf("AEMU", rtlog_error|rtlog_breaking_news, "N/P cmd 0x%2.2X", msg_id);
					break;
				}
			}

			if (trailes)
			{
                //rtlog_printf("AEMU", rtlog_warning|rtlog_breaking_news, "N/P %u response 0x%2.2X", trailes, msg_id);
				for(unsigned int i=0; i<trailes; ++i)
				{
					dst[cursor++]=grifo_fw::grifo_fw_block_raw_helper_t::k_som;
					dst[cursor++]=grifo_fw::aesa_message_descriptor_t::timeout_on_rx_res;
				}
			}
		}

		dst[cursor]=grifo_fw::grifo_fw_block_raw_helper_t::k_eob;

        len=(len+3)>>2;
        p+=(2+len);
	}
}

void aesa_write(int pingpong, void* reply, const void* cmd)
{
	memcpy(aesa_emu_msg.tx[pingpong], cmd, 4096);
	aesa_emu_msg.rxp[pingpong]=(uint32_t*)reply;
}

void aesa_rx_event(int pingpong)
{
	if (!aesa_emu_msg.rxp[pingpong])
		return;
	//aesa_emu_msg.rxp[pingpong][0]=grifo_fw::grifo_fw_block_raw_helper_t::k_eob;
}

void aesa_tx_event(int pingpong)
{

	const uint32_t* p=&aesa_emu_msg.tx[pingpong][8]; //((uint32_t*)cmd)[8];
	if (!aesa_emu_msg.rxp[pingpong])
		return;

	if (p[0]!=grifo_fw::grifo_fw_block_raw_helper_t::k_som)
		return;

	uint32_t ctrl=(p[1] & 0x03);
	if (ctrl==1) //immediate?
	{
		aesa_execute(aesa_emu_msg.rxp[pingpong], aesa_emu_msg.tx[pingpong]);
		aesa_emu_msg.pending=-1;
	}
	else
	{
		aesa_emu_msg.pending=pingpong;
	}
}

void aesa_batch()
{
	++autoMonitorable_aesaemu.inj_counter;
}

void aesa_event(int pingpong)
{
	if (pingpong<0)
	{
		pingpong=aesa_emu_msg.pending;
		aesa_emu_msg.pending=-1;
	}

	if ((pingpong>=0) && aesa_emu_msg.rxp[pingpong])
		aesa_execute(aesa_emu_msg.rxp[pingpong], aesa_emu_msg.tx[pingpong]);

}

