#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);
}

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

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)
{
	uint8_t data[2];
	data[0] = 0x0;
	data[1] = value;

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

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;

	// initialize amplitude pot
	driver_setAmplitude(0);

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



	fgen_init();

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

	_driver.lastPowerChangeTime = _sys->systemTime;

}

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

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

	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);
}

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;

			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);
	}
}

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

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

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

}

void driver_service(void)
{
	switch (_driver.state)
	{
		case DRIVER_STATE_INIT:
		{
			
			break;
		}

		case DRIVER_STATE_WAIT_PSU:
		{
			break;
		}

		case DRIVER_STATE_SWITCH_FREQUENCY:
		{
			break;
		}
	}
}