#include <automation.h>
#include "main.h"
#include <string.h>
#include <math.h>
#include <stdbool.h>
#include "cmsis_os.h"
#include "commonMsg.h"

/* Demo includes. */
#include "outputs.h"
#include "utils.h"
#include "signals.h"

#define LSBFIRST 0
#define MSBFIRST 1
#define lowByte(w) ((uint8_t) ((w) & 0xff))
#define highByte(w) ((uint8_t) ((w) >> 8))

#define FLUSHING_VALVE		15//8
#define HOLD_PRESSURE		ON

/*-----------------------------------------------------------*/

extern FLASH_STRUCT Flash;
extern osMutexId flashMutexHandle;
extern TIM_HandleTypeDef htim8;
extern osThreadId automationTaskHandle;

volatile uint16_t outputs74HC955 = 0;			// Current register state of the two 74HC955

uint8_t countOpenedValves( uint8_t outputs )
{
uint8_t count = 0;

	for(int b=0; b<8; b++)
	{
		if( outputs & (1<<b) )
		{
			count++;
		}
	}
	return count;
}

void shiftOut( uint8_t bitOrder, uint8_t val )
{
    uint8_t i;

    for (i = 0; i < 8; i++)
    {
        HAL_GPIO_WritePin(CLK_OUT_GPIO_Port, CLK_OUT_Pin, GPIO_PIN_RESET);
        if (bitOrder == LSBFIRST)
        	HAL_GPIO_WritePin(DATA_OUT_GPIO_Port, DATA_OUT_Pin, !!(val & (1 << i)));
        else
        	HAL_GPIO_WritePin(DATA_OUT_GPIO_Port, DATA_OUT_Pin, !!(val & (1 << (7 - i))));

        osDelay(1);
        HAL_GPIO_WritePin(CLK_OUT_GPIO_Port, CLK_OUT_Pin, GPIO_PIN_SET);
    }
}

void writeOut( uint16_t data )
{
	HAL_GPIO_WritePin(LATCH_OUT_GPIO_Port, LATCH_OUT_Pin, GPIO_PIN_RESET);

	shiftOut(MSBFIRST, highByte(data));
	shiftOut(MSBFIRST, lowByte(data));

    osDelay(1);
    HAL_GPIO_WritePin(LATCH_OUT_GPIO_Port, LATCH_OUT_Pin, GPIO_PIN_SET);
}

bool updateSpeedRef( uint8_t cy )
{
	if( cy != UNDEF_FLOWRATE ) {
		uint32_t tDelay = 100 - cy;
		if( tDelay < 3 )
			tDelay = 3;
		else if( tDelay > 97)
			tDelay = 97;
		htim8.Instance->CCR2 = tDelay;
		return true;
	}
	return false;
}

void runACmotor( uint8_t cycle )
{
	if( updateSpeedRef( cycle ) ) {
		HAL_TIM_OC_Start(&htim8,TIM_CHANNEL_2); // Start Timer for drive converter
	}
}

void stopACmotor()
{
	HAL_TIM_OC_Stop(&htim8,TIM_CHANNEL_2);	// Stop Timer for drive converter
}

static uint16_t _updateOutputs( uint16_t outputs )
{
uint16_t cs_outputs;
char binaryHi[9] = {0};
char binaryLo[9] = {0};

	cs_outputs = CONVERT_STATE( outputs );
	itoa(highByte(cs_outputs),binaryHi,2);
	itoa(lowByte(cs_outputs),binaryLo,2);
	APP_LOG(TS_ON, VLEVEL_M, "Set outputs(direct) %05s %08s\r\n",binaryHi,binaryLo);

	writeOut( cs_outputs );

	return cs_outputs ^ ALL_VALVES_MASK; // Used by Measurements task to detect changes
}

// parameter outputs can be on 8bits or 16bits
// Pump needs to run to keep hydraulic circuit under pressure (valves need pressure to be fully closed)
static uint16_t updateOutputs( uint16_t outputs, int8_t pumpRate )
{
uint16_t temp_outputs, cs_outputs;
bool pumpRunning = false;
int16_t pumpDelay;
char binaryHi[9] = {0};
char binaryLo[9] = {0};

	// If pump is running or not, the flushing valve is always closed - mounted on actuators board
	outputs &= ~(BITSHIFT_OUTPUT(FLUSHING_VALVE));

	// Get pump state - so at least one valve is opened
	pumpRunning = outputs74HC955 & BITSHIFT_OUTPUT(Flash.settings.actuators[3].outputNum);

	// Get pump delay according to valves opening
	if( outputs & VALVES_MASK )
	{
		pumpDelay = Flash.settings.PumpStartDelay * 1000;
	}
	else
	{
		pumpDelay = Flash.settings.PumpStopDelay * 1000;
	}

	if( outputs != outputs74HC955 )
	{
		uint8_t nbActiveValves = countOpenedValves( outputs );

		// Start/stop pump with delay
		if( pumpDelay < 0 )
		{
			pumpDelay = (-pumpDelay);

			if( !pumpRunning && nbActiveValves )
			{
				temp_outputs = outputs74HC955 | BITSHIFT_OUTPUT(Flash.settings.actuators[3].outputNum);
				runACmotor(pumpRate);
			}
			else if( pumpRunning && !nbActiveValves )
			{
				temp_outputs = outputs74HC955 & ~(BITSHIFT_OUTPUT(Flash.settings.actuators[3].outputNum));
				stopACmotor();
			}
			else
			{
				temp_outputs = outputs;
			}

			cs_outputs = CONVERT_STATE(temp_outputs);
			writeOut( cs_outputs );
			itoa(highByte(cs_outputs),binaryHi,2);
			itoa(lowByte(cs_outputs),binaryLo,2);
			APP_LOG(TS_ON, VLEVEL_M, "Set outputs(a) %05s %08s - Motor speed ref %d\r\n",binaryHi,binaryLo,pumpRate);

			osDelay(pumpDelay);
			if( temp_outputs == outputs )
				return outputs;

			outputs = (temp_outputs & ACTUATORS_MASK) | (outputs & VALVES_MASK);
			cs_outputs = CONVERT_STATE(outputs);
			writeOut( cs_outputs );
			itoa(highByte(cs_outputs),binaryHi,2);
			itoa(lowByte(cs_outputs),binaryLo,2);
			APP_LOG(TS_ON, VLEVEL_M, "Set outputs(b) %05s %08s - Motor speed ref %d\r\n",binaryHi,binaryLo,pumpRate);
		}
		else
		{
			temp_outputs = outputs;
			cs_outputs = CONVERT_STATE(temp_outputs);
			writeOut( cs_outputs );
			itoa(highByte(cs_outputs),binaryHi,2);
			itoa(lowByte(cs_outputs),binaryLo,2);
			APP_LOG(TS_ON, VLEVEL_M, "Set outputs(c) %05s %08s - Motor speed ref %d\r\n",binaryHi,binaryLo,pumpRate);

			osDelay(pumpDelay);

			if( !pumpRunning && nbActiveValves )
			{
				outputs = temp_outputs | BITSHIFT_OUTPUT(Flash.settings.actuators[3].outputNum);
				runACmotor(pumpRate);
			}
			else if( pumpRunning && !nbActiveValves )
			{
				outputs = temp_outputs & ~(BITSHIFT_OUTPUT(Flash.settings.actuators[3].outputNum));
				stopACmotor();
			}
			else
			{
				return outputs;
			}
			cs_outputs = CONVERT_STATE(outputs);
			writeOut( cs_outputs );
			itoa(highByte(cs_outputs),binaryHi,2);
			itoa(lowByte(cs_outputs),binaryLo,2);
			APP_LOG(TS_ON, VLEVEL_M, "Set outputs(d) %05s %08s - Motor speed ref %d\r\n",binaryHi,binaryLo,pumpRate);
		}
	}
	else
	{
		updateSpeedRef( pumpRate );
		itoa(highByte(CONVERT_STATE(outputs)),binaryHi,2);
		itoa(lowByte(CONVERT_STATE(outputs)),binaryLo,2);
		APP_LOG(TS_ON, VLEVEL_M, "Outputs unchanged %05s %08s - Motor speed ref %d\r\n",binaryHi,binaryLo,pumpRate);
	}

	return outputs;
}

static void Outputs_processFrame()
{
	osDelay(5);
}

void reg74HC955BleMessage()
{
osMessageQId destQueue;
uint8_t buff[3] = { '#', 'O', 0 };

	destQueue = (osMessageQId)getQueueId(BLE_COMM);

	MSG_STRUCT *p_sendMsg = allocMemoryPoolMessage(); // receiver must free
	p_sendMsg->opcode = OP_OUTPUT_ONCHANGE;
	p_sendMsg->lParam[0] = 3;
	buff[2] = lowByte(outputs74HC955);
	memcpy(&(p_sendMsg->data[2]),buff,3);
	osMessagePut(destQueue, (uint32_t)p_sendMsg, osWaitForever);
}

static void Outputs_processMsg(MSG_STRUCT *p_msg)
{
uint16_t new74HC955;

	if( p_msg->opcode == OP_OUTPUT_ONCHANGE )
	{
		ActuatorStates state = p_msg->changeState;

		switch( state ) {
		case Z_ALL_OFF:
			new74HC955 = ALL_OUTPUTS_OFF;
			break;
		case Z_START:
			new74HC955 = outputs74HC955 | p_msg->outputs;
			break;
		case Z_STOP:
			new74HC955 = outputs74HC955 & (~p_msg->outputs);
			break;
		case Z_UPDATE:
			new74HC955 = (outputs74HC955 & ACTUATORS_MASK) | (p_msg->outputs & VALVES_MASK);
			break;
		default:
			return;
		}
		outputs74HC955 = updateOutputs(new74HC955,p_msg->pumpRate);
		reg74HC955BleMessage();
	}

	osDelay( 5 );
}

uint8_t isCurrentRefSaved()
{
	float totalCurrent = 0;
	for( int a=0; a < 4; a++ )
	{
		totalCurrent += Flash.settings.actuators[a].avgCurrentConsumption;
	}
	return ( totalCurrent > 0 );
}

static void	Outputs_ProcessChecking()
{
	osMessageQId destQueue;
	uint16_t output_state = 0x0000;

	destQueue = (osMessageQId)getQueueId(ADC_MEAS);
	for( int v = 1; v < 9; v++ )
	{
		output_state = _updateOutputs( BITSHIFT_OUTPUT(v) );
		osDelay(100);
        // Send output state to the measurement task
        // Buggy line commented out
		//xQueueSend(destQueue, &output_state, 0);
		osDelay(100);
	}

	for( int a = 9; a < 13; a++ )
	{
		// Keep all valves closed to put them under pressure - Required for better performance
		output_state = _updateOutputs( BITSHIFT_OUTPUT(a) );
		osDelay(100);
        // Send output state to the measurement task
        xQueueSend(destQueue, &output_state, 0);
		osDelay(100);
	}
	// Put hydraulic circuit under pressure to be sure that valves will not leak
	osDelay(1200);
	_updateOutputs(ALL_OUTPUTS_OFF);
}

void vOutputsTask( void *pvParameters )
{
/* Remove warning for unused parameter */
    (void)pvParameters ;

	osMessageQId myQueueId = getQueueId(OUTPUTS);

	// Enable all outputs
	HAL_GPIO_WritePin(OE_GPIO_Port, OE_Pin, GPIO_PIN_RESET);
	taskENTER_CRITICAL();
	_updateOutputs(ALL_OUTPUTS_OFF);
	taskEXIT_CRITICAL();
	APP_LOG(TS_ON, VLEVEL_M, "Switching all Valves off...\r\n");

	// Waiting for reading parameters
	ulTaskNotifyTake( pdFALSE, portMAX_DELAY );
    osStatus status = osMutexWait(flashMutexHandle, 0);
    if( status == osOK )
    {
    	//osDelay(2000);
		APP_LOG(TS_ON, VLEVEL_M, "Starting outputs auto-test\r\n");
		Outputs_ProcessChecking();
		osMutexRelease(flashMutexHandle);
		APP_LOG(TS_ON, VLEVEL_M, "Outputs auto-test done\r\n");
		// Unlock automation task
		xTaskNotifyGive( automationTaskHandle );
    }

	while(1)
	{
		osEvent event;
	    event = osMessageGet( myQueueId,  100 );
	    if (event.status == osEventMessage)
	    {
	    	MSG_STRUCT* p_recvMsg = event.value.p;

	    	Outputs_processMsg(p_recvMsg);
	    	freeMemoryPoolMessage(p_recvMsg);
	    }
	    else if (event.status == osEventTimeout) {
	      Outputs_processFrame();
	    }

	}
}
