#include "variable_frequency_digital_source.h"

#include <math.h>

#define VF_BASE_MASK	0x1
#define VF_EXPONENT_MASK	0x2
#define VF_READY_MASK		0x4

/*
 * variable_frequency_digital_source_read_exponent_bus
 */
SMX_DLL_UINT32 variable_frequency_digital_source_read_frequency_bus(
	p_smx_dll_simulation_context context_p
	,
	p_smx_dll_device device_p
	,
	p_variable_frequency_digital_source_default_pointers default_pointers_p
	,
	SMX_DLL_ERROR *rv
) {

	s_smx_dll_bus_conversion
		conversion;

	// reset rv
	*rv = SMX_DLL_NO_ERROR;

	// setup conversion
	conversion.encoding = SMX_DLL_ENCODING_UNSIGNED;
	conversion.type     = SMX_DLL_TYPE_32BIT;

	// read bus
	*rv = context_p->funcs->read_bus( default_pointers_p->input_bus_pointers_p->BASE_bus_p, &( conversion ) );

	// return value
	return conversion.uint32;
	
}


/*
 * variable_frequency_digital_source_read_exponent_bus
 */
SMX_DLL_UBYTE8 variable_frequency_digital_source_read_exponent_bus(
	p_smx_dll_simulation_context context_p
	,
	p_smx_dll_device device_p
	,
	p_variable_frequency_digital_source_default_pointers default_pointers_p
	,
	SMX_DLL_ERROR *rv
) {

	s_smx_dll_bus_conversion
		conversion;

	// reset rv
	*rv = SMX_DLL_NO_ERROR;

	// setup conversion
	conversion.encoding = SMX_DLL_ENCODING_UNSIGNED;
	conversion.type     = SMX_DLL_TYPE_8BIT;

	// read bus
	*rv = context_p->funcs->read_bus( default_pointers_p->input_bus_pointers_p->EXPONENT_bus_p, &( conversion ) );

	// return value
	return conversion.ubyte8;
	
}

/*
 * variable_frequency_digital_source_assign_from_buses
 */
SMX_DLL_ERROR variable_frequency_digital_source_assign_from_buses(
	p_smx_dll_simulation_context context_p
	,
	p_smx_dll_device device_p
	,
	p_variable_frequency_digital_source_default_pointers default_pointers_p
	,
	p_variable_frequency_digital_source_managed_storage managed_storage_p
) {

	SMX_DLL_ERROR
		rv1 = SMX_DLL_NO_ERROR;

	SMX_DLL_ERROR
		rv2 = SMX_DLL_NO_ERROR;

	SMX_DLL_DOUBLE
		calculated_frequency = 0;

	// read frequency bus, assign
	managed_storage_p->frequency = variable_frequency_digital_source_read_frequency_bus(
		context_p
		,
		device_p
		,
		default_pointers_p
		,
		&( rv1 )
	);

	// check for error
	if( SMX_DLL_NO_ERROR != rv1 ) {
		
#ifdef _DEBUG
		if( IsDebuggerPresent() ) {
			DebugBreak();
		}
#endif

	}

	// read exponent bus, assign
	managed_storage_p->exponent = variable_frequency_digital_source_read_exponent_bus(
		context_p
		,
		device_p
		,
		default_pointers_p
		,
		&( rv2 )
	);

	// calculate frequency
	calculated_frequency = (double)managed_storage_p->frequency * pow( 10.0, (double)managed_storage_p->exponent );

	// check frequency is not zero
	if( calculated_frequency != 0 ) {
		managed_storage_p->period = 1.0 / calculated_frequency;
	}
	else {
		managed_storage_p->period = 0;
		return SMX_DLL_ERROR_INVALID_STATE;
	}

	// DONE
	return ( rv1 ? rv1 : rv2 );

}

/*
 * variable_frequency_digital_source_action
 *
 *	This function is declared in variable_frequency_digital_source.h
 */
void variable_frequency_digital_source_action(p_smx_dll_simulation_context context_p, p_smx_dll_device device_p) {
	
	SMX_DLL_ERROR
		rv = SMX_DLL_NO_ERROR;

	p_variable_frequency_digital_source_default_pointers
		default_pointers_p = NULL;

	p_variable_frequency_digital_source_managed_storage
		managed_storage_p = NULL;

	p_smx_dll_pin
		ready_pin_p = NULL;

	s_variable_frequency_digital_source_managed_storage
		previous_values;

	SMX_DLL_DOUBLE
		t1, t2;

	SMX_DLL_UBYTE8
		logic = SMX_DLL_LOGIC_0;

	SMX_DLL_BOOLEAN
		schedule_wakes = SMX_DLL_FALSE;

	// cast unmanaged user storage
	default_pointers_p = (p_variable_frequency_digital_source_default_pointers)device_p->unmanaged_user_storage;

	// cast managed storage
	managed_storage_p = (p_variable_frequency_digital_source_managed_storage)device_p->user_storage;

	// did we ask to be awoken?
	if( SMX_DLL_WAKE_TYPE_WAKE_REQUESTED == context_p->wake_reason.wake_type ) {

		// initial wake?
		if( VARIABLE_FREQUENCY_DIGITAL_SOURCE_INITIAL_SETUP == context_p->wake_reason.identifier ) {

			// check to see if READY is high
			if( context_p->funcs->is_pin_logic_1( default_pointers_p->READY_pin_p, &( rv ) ) ) {
		
				// assign frequency, exponent from buses
				if( SMX_DLL_NO_ERROR != ( rv = variable_frequency_digital_source_assign_from_buses(
					context_p
					,
					device_p
					,
					default_pointers_p
					,
					managed_storage_p

				) ) ) {
					// TODO: panic
				}

			}

			// ask to be awoken
			schedule_wakes = SMX_DLL_TRUE;

		}

		// valid identifier, period?
		if(
			managed_storage_p->active_identifier == context_p->wake_reason.identifier
			&&
			managed_storage_p->period > 0
		) {

			// ask to be awoken
			schedule_wakes = SMX_DLL_TRUE;

		}

		if( context_p->edge_events_and_scheduling_wakes_enabled && schedule_wakes ) {

			// if we're currently high
			if( context_p->funcs->is_pin_logic_1( default_pointers_p->OUT_pin_p, &( rv ) ) ) {

				// calculate event times
				t1 = SMX_DLL_MINIMUM_OUTPUT_DELAY;
				t2 = ( managed_storage_p->period * ( 1.0 - default_pointers_p->parameter_values_p->DUTY_CYCLE ) ) - SMX_DLL_MINIMUM_OUTPUT_DELAY;
				logic = SMX_DLL_LOGIC_0;

			// if we're either low or X
			} else {

				// calculate event times
				t1 = SMX_DLL_MINIMUM_OUTPUT_DELAY;
				t2 = ( managed_storage_p->period * default_pointers_p->parameter_values_p->DUTY_CYCLE ) - SMX_DLL_MINIMUM_OUTPUT_DELAY;
				logic = SMX_DLL_LOGIC_1;

			}

			// schedule output change at t1
			rv = context_p->funcs->set_pin_logic(
				default_pointers_p->OUT_pin_p
				,
				logic
				,
				t1
			);

			// schedule wake at t2
			rv = context_p->funcs->set_scheduled_wake(device_p, t2, &(managed_storage_p->active_identifier));

		}

	}

	// input changed
	else if( SMX_DLL_WAKE_TYPE_INPUT_CHANGED == context_p->wake_reason.wake_type ) {

		// check to see if READY is high
		if( context_p->funcs->is_pin_logic_1( default_pointers_p->READY_pin_p, &( rv ) ) ) {

			// copy old values out
			memcpy_s( &( previous_values ), sizeof( s_variable_frequency_digital_source_managed_storage ), managed_storage_p, sizeof( s_variable_frequency_digital_source_managed_storage ) );

			// assign frequency, exponent from buses
			if( SMX_DLL_NO_ERROR != ( rv = variable_frequency_digital_source_assign_from_buses(
				context_p
				,
				device_p
				,
				default_pointers_p
				,
				managed_storage_p

			) ) ) {
				// TODO: panic
			}

			// compare to old values
			if(
				context_p->edge_events_and_scheduling_wakes_enabled
				&&
				(
					previous_values.frequency != managed_storage_p->frequency
					||
					previous_values.exponent  != managed_storage_p->exponent
				)
			) {

				// bump active identifier
				managed_storage_p->active_identifier++;

				// schedule wake immediately
				rv = context_p->funcs->set_scheduled_wake( device_p, SMX_DLL_MINIMUM_OUTPUT_DELAY, &( managed_storage_p->active_identifier ) );

			}

		}

	}

}
