#include "clamp.h"

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

extern SYSTEM_DATA_t sys;

static FREQUENCY_t * setFrequency(ACCESSORY_t *accy, FREQUENCY_t *freq)
{

    accy->setPower(accy, sys.driver->powerLevel);

    return freq;
}

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

    return 0;
}

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_27V;
        accy->driveVoltage[HIGH_FREQ] = V_42V;

        driver_setFrequency(freq);

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

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


int clamp_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_RUNNING:
		{
			if (accy->initState)
			{
			}
			break;	
		}
		default:
			return -1;
	}

	return 0;
}