/*
 * taps.c
 *
 *  Created on: Mar 16, 2023
 *      Author: Keith.Lloyd
 */
#include	"arm_math.h"
#include	"fsl_gpio.h"
#include	"spi.h"

#include "taps.h"
#include "display.h"
#include "ports.h"
#include "mode.h"
#include "frq.h"
#include "amps.h"
#include "adc.h"
#include "utils.h"
#include "io.h"
#include "System/system.h"

extern SYSTEM_DATA_t sys;
extern uint8_t Port_State[];
extern uint8_t Cur_Mode,Test_Mode;
extern uint32_t new_freq;
extern float32_t Ohms;
extern uint16_t Taps_adjust_timer;
extern uint8_t frequency;
extern FREQUENCY_t freqArray[FREQ_MAX_NUM];
float32_t last_Ohms;
extern uint8_t Taps_Flag;
void Check_Taps(void)
{
	Taps_adjust_timer = DELAY_ONE_SECOND;

	if(Cur_Mode != BROADCAST)
	{
		if (new_freq <= MAX_DTYPE)
			Set_taps_LF();
		else
//			Set_taps_HF();
			Set_taps_LF();

	}
}

void taps_setTapLF(uint8_t tap)
{
	//EXPANDER_CLEAR(~TAPS_OFF_MASK, NO_UPDATE);

	switch (tap)
	{
		case 2:
		{
			EXPANDER_SET(_TAP2_LF_ON, UPDATE);
			break;
		}
		case 3:
		{
			EXPANDER_SET(_TAP3_LF_ON, UPDATE);
			break;
		}
		case 4:
		{
			EXPANDER_SET(_TAP4_LF_ON, UPDATE);
			break;
		}
		default:
		{
			EXPANDER_SET(_TAP1_LF_ON, UPDATE);
			break;
		}
	}
	
}

void Set_taps_LF(void) // D' Type Amplifier
{
	uint16_t New_Ohms;

	if((!Taps_Flag) && (!Check_For_Clamp()))

//	if((!Test_Mode && (ACCY_GetConnectedAccessory(1) != ID_CLAMP) && (ACCY_GetConnectedAccessory(2) != ID_CLAMP)))
	{
		if(!Taps_Flag)
		{
			Port_State[BOTTOM_SR] &= TAPS_OFF_MASK;
			New_Ohms = (sys.adc.Ohms_slowfilt);

			switch (New_Ohms)
			{

				case 0 ... 25:
					Port_State[BOTTOM_SR] |= TAP1_LF_ON;
				break;

				case 26 ... 50:
					if (freqArray[frequency].frequency1 <= 8192)
						Port_State[BOTTOM_SR] |= TAP2_LF_ON;
					else
						Port_State[BOTTOM_SR] |= TAP1_LF_ON; // v1.7 = TAP1 1.7X TAP2
				break;

				case 51 ... 250:
					if (freqArray[frequency].frequency1 <= 29430) // 9820
						Port_State[BOTTOM_SR] |= TAP3_LF_ON;//3
					if (freqArray[frequency].frequency1 >= 29430 && freqArray[frequency].frequency1 <= MAX_DTYPE)
						Port_State[BOTTOM_SR] |= TAP2_LF_ON; //1,2
					else
						if(freqArray[frequency].frequency1 >= MAX_DTYPE)
							Port_State[BOTTOM_SR] |= TAP2_LF_ON;
				break;

				case 251 ... 500:
					if (freqArray[frequency].frequency1 >= 29430 && freqArray[frequency].frequency1 <= MAX_DTYPE)
						Port_State[BOTTOM_SR] |= TAP2_LF_ON;
					else
						Port_State[BOTTOM_SR] |= TAP4_LF_ON;
				break;

				case 501 ... 1000:
					if (freqArray[frequency].frequency1 == MAX_DTYPE || freqArray[frequency].frequency1 < 512)
						Port_State[BOTTOM_SR] |= TAP3_LF_ON;
					else
						Port_State[BOTTOM_SR] |= TAP4_LF_ON; //4 1.7 =T3 1.7X =T4
				break;


				default:
					Port_State[BOTTOM_SR] |= TAP4_LF_ON;
				break;


			}

			SPI0_SendBytes(Port_State, 3, EXPANDER);

			last_Ohms = sys.adc.Ohms_slowfilt;
			Taps_Flag = true;

		}
	}
	else
	{
		Port_State[BOTTOM_SR] &= TAPS_OFF_MASK;
		Port_State[BOTTOM_SR] |= TAP4_LF_ON;	// Set TAPS to Fixed Test setting
		SPI0_SendBytes(Port_State, 3, EXPANDER);

	}

}


void Set_taps_HF(void)			// AB Amplifier
{
	if(Hys_Taps)
		{
		last_Ohms = sys.adc.Ohms_slowfilt;

		if(sys.adc.Ohms_slowfilt < 200)

			Port_State[MID_SR] &= TAP102_HF_OFF; // 1:1
		else
			Port_State[MID_SR] |= TAP102_HF_ON; // 1:5

		SPI0_SendBytes(Port_State, 3, EXPANDER);
		}
}

bool Hys_Taps(void) //Check for a +/-10% change in the impedance
{
float32_t	test;

	test = last_Ohms + last_Ohms/10;

	if(sys.adc.Ohms_slowfilt > test )
		return(true);

	test = last_Ohms - last_Ohms/10;

	if(sys.adc.Ohms_slowfilt < test)
		return(true);
	else
		return(false);
}