/*
 * ports.c
 *
 *  Created on: Mar 21, 2023
 *      Author: Keith.Lloyd
 */
/*
 * 	Return accessory associated with voltage read
 */
#include "fsl_spi.h"
#include "pin_mux.h"
#include "board.h"
#include "fsl_debug_console.h"

#include <stdbool.h>
#include <stdint.h>
#include "arm_math.h"
#include "ports.h"
#include "spi.h"
#include "display.h"
#include "utils.h"
#include "adc.h"
#include "mode.h"
#include "psu_ctrl.h"
#include "timer.h"
#include "init.h"
#include "amps.h"
#include "fsl_sctimer.h"
#include "pwm.h"
#include "pwr_level.h"
#include "System/system.h"
#include "usbComms.h"
#include "driver.h"
#include "broadcast.h"
#include "direct.h"

uint16_t	Port_timer;
uint16_t	Taps_adjust_timer;
extern uint8_t	Port_changed_flag,Init_Done;
extern uint8_t Port_State[],Cur_Mode,old_freq,Bcast_Pwr_Level,Power_Level;
extern MODE_REC_t mode_Array[MODE_MAX_NUM];
extern uint8_t frequency,Over_Voltage_Flag;
extern FREQUENCY_t freqArray[FREQ_MAX_NUM];
extern volatile uint8_t BC_Duty_Cycle;
extern uint16_t Pot_Value_AB[];
extern uint16_t Pot_Value[];
extern uint8_t Dds_Pot_Val[];

extern SYSTEM_DATA_t sys;

uint8_t whatever;

bool ACCY_timedOut(ACCESSORY_t *accy)
{
	return (sys.systemTime >= accy->stateTimer);
}

void ACCY_setTimeout(ACCESSORY_t *accy, uint32_t msec)
{
	accy->stateTimer = sys.systemTime + msec;
}

static int handleClamp(ACCESSORY_t *accy)
{
	switch (accy->state)
	{
		case PORT_STATE_INIT:
		{
			if (accy->initState)
			{

			}
			break;
		}

		case PORT_STATE_DEINIT:
			accy->state = PORT_STATE_STANDBY;
			break;

		default:
			return -1;
	}

	return 0;
}

static int handleDirectConnect(ACCESSORY_t *accy)
{
	switch (accy->state)
	{
		case PORT_STATE_INIT:
		{
			if (accy->initState)
			{
				accy->stateTimer = sys.systemTime + 2000;

				USB_SendString("Direct connect initializing...");
			}

			if (sys.systemTime >= accy->stateTimer)
			{
				accy->state = PORT_STATE_RUNNING;
			}
			break;
		}

		case PORT_STATE_DEINIT:
			accy->state = PORT_STATE_STANDBY;
			break;

		case PORT_STATE_STANDBY:
			break;

		case PORT_STATE_RUNNING:
		{
			if (accy->initState)
			{
				USB_SendString("Direct connect running!");
			}
			break;
		}
		default:
			return -1;
	}
	return 0;
}

static int initAccessory(ACCESSORY_t *accy)
{
	accy->state = PORT_STATE_INIT;
	accy->initState = true;
	//accy->handler(accy);
}

static int deinitAccessory(ACCESSORY_t *accy)
{
	if (accy->isConnected)
	{
		accy->state = PORT_STATE_DEINIT;
		accy->initState = true;
	}
	//accy->handler(accy);
}

void ACCY_service(void)
{
	ACCY_Update();
	
	ACCESSORY_t *active = sys.activeAccessory;

	// if there is an accessory waiting to be switched, check that the current active is in standby before initializing
	if (active != NULL)
	{
		if (active->isConnected)
		{
			// service the current accessory
			PortState_t prevState = active->state;
			active->handler(active);
			active->initState = (prevState != active->state);
		}
		else
		{
			// active accessory was disconnected
			// fall back to induction for now
			ACCY_setActive(&sys.ports[ACCY_PORT_INDUCTION], CHANNEL_A);
		}


		if ((sys.nextAccessory) != NULL)
		{
			// wait until current accessory has been deinitialized before switching
			if (active->state == PORT_STATE_STANDBY)
			{
				sys.activeAccessory = sys.nextAccessory;
				sys.nextAccessory = NULL;
				initAccessory(sys.activeAccessory);
			}
		}
	}
	else
	{
		// if nothing active, set broadcast
		ACCY_setActive(&sys.ports[ACCY_PORT_INDUCTION], CHANNEL_A);
	}

	
}

void ACCY_setActiveChannel(AccyChannelId_t channel)
{
	// TODO: handle anything needed for switching here
	sys.activeAccessory->activeChannel = channel;
}

void ACCY_next(void)
{

	if (sys.activeAccessory == NULL)
	{
		ACCY_setActive(&sys.ports[ACCY_PORT_INDUCTION], CHANNEL_A);
		return;
	}

	// cycle through accessories and channels to find the next available
	AccessoryPortId_t currentPort = sys.activeAccessory->portId;
	AccyChannelId_t currentChannel = sys.activeAccessory->activeChannel;


	AccessoryPortId_t port = currentPort;
	AccyChannelId_t channel = currentChannel + 1;

	
	do
	{

		if (channel >= NUM_CHANNELS)
		{
			port++;

			if (port >= NUM_PORTS)
			{
				port = 0;
			}

			channel = CHANNEL_A;
		}

		AccessoryChannel_t *ch = &sys.ports[port].channels[channel];

		if (ch->connected)
		{
			ACCY_setActive(&sys.ports[port], channel);
			break;
		}

		channel++;

	} while ((port != currentPort) || (channel != currentChannel));

}

void ACCY_setActive(ACCESSORY_t *accy, AccyChannelId_t channel)
{
	accy->activeChannel = channel;

	// if just changing channels on the active accessory, don't deinit/init
	// assume accessory's function will detect the channel change and respond
	if (accy == sys.activeAccessory)
	{
		return;
	}

	// deinitialize current accessory
	if (sys.activeAccessory != NULL)
	{
		deinitAccessory(sys.activeAccessory);

		// initialize the accessory to switch when complete
		sys.nextAccessory = accy;

	}
	else
	{
		// nothing active, switch now
		sys.activeAccessory = accy;
		initAccessory(sys.activeAccessory);
	}

	

}

void ACCY_Disconnect(ACCESSORY_t *accy)
{

	sprintf(sys.tmpString, "Accessory disconnected: %d", accy->connected);
	USB_SendString(sys.tmpString);

	accy->connected = ID_NONE;
	accy->isConnected = false;

	accy->handler = NULL;

	accy->state = PORT_STATE_STANDBY;

	accy->channels[CHANNEL_A].id = CHANNEL_A;
	accy->channels[CHANNEL_A].connected = false;

	accy->channels[CHANNEL_B].id = CHANNEL_B;
	accy->channels[CHANNEL_B].connected = false;


}


// setup the accessory as connected but do not initialize it until set as active
bool ACCY_Connect(ACCESSORY_t *accy, ACCY_ID_t id)
{
	accy->connected = id;
	accy->isConnected = true;

	accy->state = PORT_STATE_STANDBY;

	accy->channels[CHANNEL_A].id = CHANNEL_A;
	accy->channels[CHANNEL_A].connected = false;

	accy->channels[CHANNEL_B].id = CHANNEL_B;
	accy->channels[CHANNEL_B].connected = false;

	switch (id)
	{
		case ID_TX_DUAL_DIRECT:
		{
			accy->channels[CHANNEL_B].connected = true;
			// intentional fall through
		}
		case ID_TX_SINGLE_DIRECT:
		{
			accy->channels[CHANNEL_A].connected = true;
			accy->handler = direct_service;
			break;
		}

		case ID_CLAMP:
		case ID_CLAMP2:
		{
			accy->channels[CHANNEL_A].connected = true;
			accy->handler = handleClamp;
			break;
		}

		case ID_BROADCAST:
		{
			accy->channels[CHANNEL_A].connected = true;
			accy->handler = broadcast_service;
			break;
		}

		default:
		{
			accy->handler = NULL;
			accy->connected = ID_NONE;
			accy->isConnected = false;
		}
	}

	sprintf(sys.tmpString, "New accessory connected: %d", accy->connected);
	USB_SendString(sys.tmpString);

	return accy->isConnected;
}

static ACCY_ID_t ReadAccessory(uint8_t port)
{
	if (port >= NUM_PORTS)
	{
		return ID_NONE;
	}

	ACCESSORY_t *accy = &sys.ports[port];

	uint32_t idNumber = (uint32_t)((*accy->idVoltage * 3.3333f) + 0.5f);		//multiply by 3.3333 and round down to nearest integer


	return (ACCY_ID_t)idNumber;
}

void  ACCY_Init(void)
{

	for (int i=0; i<NUM_PORTS; ++i)
	{
		sys.ports[i].connected = ID_NONE;
		sys.ports[i].consecutiveScans = 0;
		sys.ports[i].isConnected = false;

		sys.ports[i].portId = i;
	}

	sys.ports[ACCY_PORT_1].idVoltage = &sys.adc.V_ID1;	
	sys.ports[ACCY_PORT_2].idVoltage = &sys.adc.V_ID2;	

	sys.activeAccessory = NULL;
	sys.nextAccessory = NULL;

}

void ACCY_Update(void)
{
	
	for (int i=ACCY_PORT_1; i<NUM_PORTS; ++i)
	{
		ACCESSORY_t *accy = &sys.ports[i];
		ACCY_ID_t detected = ReadAccessory(i);

		if(!accy->isConnected)		//Nothing connected. Look for accessory to connect
		{
			if((detected == accy->lastDetected) && (detected != ID_NONE))	//If detected same as last time and not ID_NONE
			{

				if(++accy->consecutiveScans == AC_NUM_SCANS_TO_CONNECT)		//Connect on 3rd consecutive scan of same accessory
				{
					// we found a newly connected accessory.  switch to it.
					if (ACCY_Connect(accy, detected))
					{
						ACCY_setActive(accy, CHANNEL_A);
					}
				}
			}
			else									//If different than last scan
			{
				accy->lastDetected = detected;		//remember what was scanned last time
				accy->consecutiveScans = 0;
			}
		}
		else if (detected != accy->connected)		//If connected and detected reads different, disconnect
		{
			ACCY_Disconnect(accy);
		}
	}

}

//Call @ 10Hz to detect, connect, and disconnect accessories

/*
 * 	Connect accessory
 */
bool ACCY_isKnown(ACCY_ID_t id)
{
	bool known = false;

	switch(id)
	{
		case ID_TX_SINGLE_DIRECT:
		case ID_TX_DUAL_DIRECT:
		case ID_CLAMP:
		case ID_CLAMP2:
			known = true;
			break;
	}

	return known;
}





void Read_Tx_Ports(void) // check for whats plugged in at the ports every 100mS.
{
	uint8_t chip;

	Port_timer = DELAY_100MS;


	ACCY_Update();


}