#include "direct.h"
#include "driver.h"
#include "ports.h"
#include "fgen.h"
#include "utils.h"
#include "System/system.h"

extern SYSTEM_DATA_t sys;

enum
{
	SET_ALKALINE = 0,
	SET_LITHIUM,
	SET_HF,
	NUM_SETPOINTS
};



static float _setpoints[NUM_SETPOINTS][POWER_LEVEL_COUNT] = {
	{ 0.010, 0.050, 0.100, 0.150, 0.200 },		// Alkaline
	{ 0.010, 0.050, 0.150, 0.400, 1.000 },		// Lithium
	{ 0.010, 0.050, 0.100, 0.150, 0.200 }		// HF
};

static const float DIRECT_KP = 10.1f;
static const float DIRECT_KI = 10.5f;
static const float DIRECT_KD = 0.0f;

enum
{
	KP = 0,
	KI,
	KD,
	TERM_COUNT
};

typedef struct 
{
	float k[TERM_COUNT];
	float err;
	float output;
	float err_int;
	float setpoint;
	bool  enabled;
} Controller_t;


static FREQUENCY_t * setFrequency(ACCESSORY_t *accy, FREQUENCY_t *freq)
{
	// is this an appropriate frequency?  if not, return one that is

    return freq;
}

static int setPower(ACCESSORY_t *accy, PowerLevel_t power)
{
    FREQUENCY_t *f = driver_getFrequency();
	PowerLevel_t prevPower = driver_getPowerLevel();

	Controller_t *controller = (Controller_t*)accy->data;

	if (prevPower == POWER_LEVEL_0 && power > POWER_LEVEL_0)
	{
		driver_bypass(true);
		driver_isolateOutput(false);


		controller->enabled = true;
		
	}
	else
	if (prevPower > POWER_LEVEL_0 && power == POWER_LEVEL_0)
	{
		driver_bypass(false);
		driver_isolateOutput(true);

		driver_setAmplitude(0);
		controller->enabled = false;
	}

	if (driver_isLowFreq())
	{
		if (sys.batteryType == ALKALINE)
		{
			controller->setpoint = _setpoints[SET_ALKALINE][power];
		}
		else
		{
			controller->setpoint = _setpoints[SET_LITHIUM][power];
		}
	}
	else
	{
		controller->setpoint = _setpoints[SET_HF][power];
	}

#if 0
	driver_setAmplitude(power * 20);

	if (prevPower == POWER_LEVEL_0 && power > POWER_LEVEL_0)
	{
		driver_bypass(true);
		driver_isolateOutput(false);
	}
	else
	if (prevPower > POWER_LEVEL_0 && power == POWER_LEVEL_0)
	{
		driver_bypass(false);
		driver_isolateOutput(true);
	}
#endif
    return 0;
}




static void regulate(ACCESSORY_t *accy)
{
	Controller_t *c = (Controller_t*)accy->data;

	// check for disconnect
	if (accy->loadConnected)
	{
		if (sys.adc.Ohms_slowfilt > 10000)
		{
			driver_setAmplitude(20);
			accy->loadConnected = false;
			c->enabled = false;
		}
	}
	else
	{
		if (sys.adc.Ohms_slowfilt < 3000)
		{
			driver_setAmplitude(20);
			accy->loadConnected = true;
			c->enabled = true;
		}
	}

	if (!c->enabled)
	{
		return;
	}

#if 0

	float err = c->setpoint - sys.adc.I_OUT_SlowFilt;
	float err_int = c->err_int + err;

	c->output = err * c->k[KP] + err_int * c->k[KI];

	if (c->output > 180.0f)
	{
		c->output = 180.0f;
	}
	else
	{
		c->err_int = err_int;
	}
	
	driver_setAmplitude((uint8_t)(c->output + 0.5f));
#endif	

	float targetCurrent = driver_maxLoadCurrent();

	if (c->setpoint < targetCurrent)
	{
		targetCurrent = c->setpoint;
	}

	int pot = driver_getAmplitude();
	int newPot = 3;

	if (pot > 0)
	{
		newPot = (targetCurrent / sys.adc.I_OUT_SlowFilt) * pot;  // Request divided by Pot current contents
	}


	// limit change to 20
	if (newPot - pot > 20)
	{
		newPot = pot + 20;
	}
	else
	if (newPot - pot < -20)
	{
		newPot = pot - 20;
	}

	if (newPot > 255)
	{
		newPot = 255;
	}

	// limit if the estimated voltage is over the maximum

	driver_setAmplitude((uint8_t)newPot);

}

static void _stateInit(ACCESSORY_t *accy)
{
    FREQUENCY_t *freq = driver_getFrequency();

    if (accy->initState)
    {
        accy->setFrequency = setFrequency;
        accy->setPower = setPower;

        accy->signalPath = SIGNAL_PATH_AMP;

		accy->driveVoltage[LOW_FREQ] = V_36V;
        accy->driveVoltage[HIGH_FREQ] = V_24V;

        driver_setFrequency(freq);

        driver_broadcastOn(true);
        
        ACCY_setTimeout(accy, 500);
    }

    if (ACCY_timedOut(accy))
    {
        accy->state = PORT_STATE_WAIT_FOR_DRIVER;
    }
}


int direct_service(ACCESSORY_t *accy)
{
	switch (accy->state)
	{
		case PORT_STATE_INIT:
		{
            _stateInit(accy);
			break;
		}

		case PORT_STATE_DEINIT:
		{
			if (accy->initState)
			{

				accy->stateTimer = sys.systemTime + 500;
			}

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

			break;
		}

		case PORT_STATE_WAIT_FOR_DRIVER:
		{
			if (driver_outputReady())
			{
				accy->state = PORT_STATE_RUNNING;
			}
			break;
		}

		case PORT_STATE_RUNNING:
		{
			if (accy->initState)
			{
				Controller_t *controller = (Controller_t*)accy->data;
				controller->k[KP] = DIRECT_KP;
				controller->k[KI] = DIRECT_KI;
				controller->k[KD] = DIRECT_KD;

				controller->err_int = 0.0f;

				controller->enabled = false;

				setPower(accy, driver_getPowerLevel());

			}

			regulate(accy);
			break;	
		}
		
		default:
			return -1;
	}

	return 0;
}