#include "driver.h"
#include "io.h"
#include "System/system.h"
#include "spi.h"
#include "pwm.h"
#include "utils.h"

static SYSTEM_DATA_t *_sys = NULL;
static TxDriver_t _driver;

static bool powerChangeAllowed(void)
{
	return ((_sys->systemTime - _driver.lastPowerChangeTime) > POWER_CHANGE_TIME);
}

static void setTapLF(TransformerTap_t tap, bool update)
{

	TransformerTap_t prevTap = _driver.tap;

	EXPANDER_CLEAR(TAPS_OFF, NO_UPDATE);

	switch (tap)
	{
		case TAP_2:
		{
			EXPANDER_SET(_TAP2_LF_ON, update);
			break;
		}
		case TAP_3:
		{
			EXPANDER_SET(_TAP3_LF_ON, update);
			break;
		}
		case TAP_4:
		{
			EXPANDER_SET(_TAP4_LF_ON, update);
			break;
		}
		default:
		{
			EXPANDER_SET(_TAP1_LF_ON, update);
			break;
		}
	}

	if (update) io_update();

	_driver.tap = tap;

}

static void setAmplifier(Amplifier_t amp)
{
	Amplifier_t prevAmp = _driver.amplifier;

	switch (amp)
	{
		case AMPLIFIER_D:
		{
			io_ampABOn(false, UPDATE);
			delayms(5);

			io_connectAmpAB(false, UPDATE);
			delayms(5);

			io_selectAmpD(UPDATE);

			io_ampDOn(true, UPDATE);
			delayms(10);

			io_ampDEnable(true, UPDATE);
			

			break;
		}

		case AMPLIFIER_AB:
		{
			// turn off AB
			io_ampABOn(false, NO_UPDATE);
			io_connectAmpAB(false, NO_UPDATE);
			io_update();

			// set the voltage appropriate for the accessory
			driver_setPSUVoltage(_sys->activeAccessory->driveVoltage[HIGH_FREQ]);
			
			_driver.state = DRIVER_STATE_SWITCH_AMP;
			break;
		}

		default:
		{
			// turn off all amplifiers
			
			io_ampABOn(false, NO_UPDATE);
			io_ampDOn(false, NO_UPDATE);
			io_ampDEnable(false, NO_UPDATE);
			
			io_connectAmpAB(false, NO_UPDATE);
			io_broadcastOn(false, NO_UPDATE);
			
			// update values at the same time
			io_update();

			amp = AMPLIFIER_NONE;
		}
	}

	_driver.amplifier = amp;
}

static void allAmpsOff(void)
{
	setAmplifier(AMPLIFIER_NONE);
}

float driver_maxLoadCurrent(void)
{
	// get max current based on load and frquency and battery
	float targetPower = _sys->maxPowerLimit;

	if (!driver_isLowFreq())
	{
		targetPower = 1.0f;
	}
	
	return (sqrtf(targetPower / _sys->adc.Ohms_slowfilt));
}

bool driver_isLowFreq(void)
{
	return (_driver.frequency->frequency <= MAX_D_FREQ);
}

TxDriver_t * driver_getDriver(void)
{
	return &_driver;
}

void driver_optimizeTransformer(void)
{
	// update the tap settings based on the load
	// must be requested by the accessory
}

void driver_setPSUVoltage(uint8_t value)
{
	uint8_t data[2];
	data[0] = 0x0;
	data[1] = value;

	SPI0_SendBytes(data, 2, PSU_VCTRL); // Update the Pot
}

void driver_setAmplitude(uint8_t value)
{
	fgen_setAmplitude(value);
}

void driver_isolateOutput(bool isolate)
{
	if (_sys != NULL)
	{
		_sys->status[ESTOP] = isolate;
	}

	if (isolate)
	{
		GPIO_WRITE(PIN_ESTOP, LOW);		// Isolated
	}
	else
	{
		GPIO_WRITE(PIN_ESTOP, HIGH);	// Not Isolated
	}
		
}

void driver_enablePower(bool enable)
{
	if (enable)
    {
        GPIO_WRITE(PIN_POWER_CTL, HIGH);            // Switch or keep power on
    }
	else
    {
		GPIO_WRITE(PIN_POWER_CTL, LOW);			// Switch power off
    }
}


void driver_init(void)
{
    _sys = system_getSys();

	// isolate output
    driver_isolateOutput(true);

	_driver.state = DRIVER_STATE_INIT;
	_driver.initState = true;
	_driver.signalPath = SIGNAL_PATH_NONE;

	// initialize amplitude pot
	driver_setAmplitude(0);

	// initialize PSU
	_driver.psuValueMax = MAX_AB_PSU;
	driver_setPSUVoltage(_driver.psuValueMax);
	driver_enablePower(true);


	_driver.prevAccy = NULL;

	_driver.fgen = fgen_init();

	// HACK: pick the first frequency for now
	_driver.powerLevel = POWER_LEVEL_0;
	_driver.frequency = fgen_getByIndex(0);

	_driver.lastPowerChangeTime = _sys->systemTime;

	setTapLF(TAP_2, UPDATE);

}



void driver_broadcastOn(bool on)
{
	// turn off all amplifiers
	io_broadcastOn(!on, NO_UPDATE);
	io_connectAmpAB(!on, NO_UPDATE);
	io_ampDOn(!on, NO_UPDATE);

	// update values at the same time
	io_update();

	delayms(10);

	driver_setPSUVoltage((on ? V_27V : V_24V));

	// turn on broadcast amp
	io_broadcastOn(on, UPDATE);
	delayms(5);

	// turn on the amplifier power supply
	io_ampPsuOn(on, UPDATE);

	_driver.amplifier = AMPLIFIER_NONE;

	if (!on)
	{
		// turn off PWM
		driver_setDuty(0);
	}
}

void driver_setDuty(uint32_t duty)
{
	uint32_t frequency;

	if (duty == 0)
	{
		frequency = DEFAULT_PWM_FREQ;
	}
	else
	{
		frequency = _driver.frequency->frequency;
	}
	
	PWM_Setup(frequency, duty);
}

void driver_setFrequency(FREQUENCY_t *freq)
{

	if (_sys->activeAccessory != NULL)
	{
		freq = _sys->activeAccessory->setFrequency(_sys->activeAccessory, freq);

		if (freq != NULL)
		{
			// accessory may select a different frequency
			_driver.frequency = freq;

			fgen_setFrequency(freq);

			driver_setPower(_driver.powerLevel);
		}
	}

}



void driver_setPower(PowerLevel_t powerLevel)
{
	if (_sys->activeAccessory != NULL)
	{
		if (powerLevel > POWER_LEVEL_4)
		{
			powerLevel = POWER_LEVEL_4;
		}

		if (0 == _sys->activeAccessory->setPower(_sys->activeAccessory, powerLevel))
		{
			_driver.powerLevel = powerLevel;
		}
	}
	
	_driver.lastPowerChangeTime = _sys->systemTime;

}

void driver_powerUp(void)
{
	// limit power change rate
	if (!powerChangeAllowed()) return;

	PowerLevel_t level = _driver.powerLevel;

	if (level < POWER_LEVEL_4)
	{
		level++;
	}

	driver_setPower(level);
}

void driver_powerDown(void)
{
	if (!powerChangeAllowed()) return;

	PowerLevel_t level = _driver.powerLevel;

	if (level > POWER_LEVEL_0)
	{
		level--;
	}

	driver_setPower(level);
}

void driver_setSafe(bool safe)
{
	_sys->safeMode = safe;
	//Select_Estop((safe ? ISOLATED : CONNECTED));
	driver_isolateOutput(true);

	if (safe)
	{
		//clear shift registers to known state
		io_expanderSetSafe();
		delayms(10);
		
		//dds_t *dds = fgen_getDDS(DDS_SIGNAL);
		//dds_sleep(dds, true, true);
	}
}

static void processSwitchAmp(void)
{

}



bool driver_outputReady(void)
{
	return (_driver.state == DRIVER_STATE_RUNNING);
}

void driver_bypass(bool bypass)
{
	_sys->status[BYPASS] = bypass;
	io_bypassProtection(bypass, UPDATE);

}

void driver_setSignalPath(SignalPath_t path)
{
	SignalPath_t prevPath = _driver.signalPath;
	 
	if (path == prevPath) return;

	// disconnect external outputs
	driver_isolateOutput(true);

	driver_bypass(false);

	// turn off all amplifiers
	setAmplifier(AMPLIFIER_NONE);

	switch (path)
	{
		case SIGNAL_PATH_ANT:
		{
			
			delayms(10);

			driver_setPSUVoltage(V_27V);

			// turn on broadcast amp
			io_broadcastOn(true, UPDATE);
			delayms(5);

			// turn on the amplifier power supply
			io_ampPsuOn(true, UPDATE);

			// turn off PWM
			driver_setDuty(0);
			
			break;
		}

		case SIGNAL_PATH_AMP:
		{
			// set the frequency generator
			driver_setFrequency(_driver.frequency);

			// select amplifier
			if (driver_isLowFreq())
			{
				setAmplifier(AMPLIFIER_D);
			}
			else
			{
				setAmplifier(AMPLIFIER_AB);
			}

			break;
		}

		default:
		{
		}
	}

	_driver.signalPath = path;
}

FREQUENCY_t* driver_getFrequency(void)
{
	return _driver.frequency;
}

PowerLevel_t driver_getPowerLevel(void)
{
	return _driver.powerLevel;
}

uint8_t driver_getAmplitude(void)
{
	return _driver.fgen->amplitude;
}

TransformerTap_t driver_getTap(void)
{
	return _driver.tap;
}

Amplifier_t driver_getAmplifier(void)
{
	return _driver.amplifier;
}

// safety monitor run from interrupt
void driver_monitor(void)
{

}

static void checkAccessory(void)
{
	ACCESSORY_t *accy = _sys->activeAccessory;

	// do we need to swtich the signal path?
	if (accy != NULL)
	{
		if (accy->state == PORT_STATE_RUNNING)
		{
			// new accessory attached
			if (_driver.prevAccy != accy)
			{
				if (_driver.signalPath != accy->signalPath)
				{
					// change the signal path
					driver_setSignalPath(accy->signalPath);
				}

				io_setOutputPort(accy->portId, accy->activeChannel);

				_driver.prevAccy = accy;
			}
		}
	}
}

void driver_service(void)
{
	DriverState_t prevState = _driver.state;

	switch (_driver.state)
	{
		case DRIVER_STATE_INIT:
		{
			_driver.state = DRIVER_STATE_RUNNING;
			break;
		}

		case DRIVER_STATE_RUNNING:
		{
			// has the accessory / signal path changed?
			checkAccessory();
			break;
		}

		case DRIVER_STATE_SWITCH_AMP:
		{
			if (_driver.initState)
			{

			}

			if (_driver.amplifier == AMPLIFIER_AB)
			{
				if (_sys->adc.V_PSU < MAX_AB_VOLTAGE)
				{
					// disable D amp
					io_ampDEnable(false, UPDATE);
					delayms(5);
					
					io_ampDOn(false, UPDATE);
					delayms(5);

					// select AB amp
					// turn on AB amp
					io_ampABOn(true, UPDATE);

					_driver.state = DRIVER_STATE_RUNNING;
				}
			}


			break;
		}

		case DRIVER_STATE_SWITCH_FREQUENCY:
		{
			break;
		}
	}

	if (prevState != _driver.state)
	{
		_driver.initState = true;
	}
}