#include <stdio.h>
#include "main.h"
#include <string.h>
#include "cmsis_os.h"
#include "commonMsg.h"

/* Demo includes. */
#include "can_bus.h"
#include "signals.h"
#include "sample_service.h"
#include "utils.h"
#include "pcanpro_can.h"

/*-----------------------------------------------------------*/
extern CAN_HandleTypeDef hcan1;

GREEN_HOUSE_SENSORS_STRUCT greenHouseSensors = {
		5,
		{
			{ 0x320, METEO, 4, {0}, 5},
			{ 0x321, LEVEL, 1, {0}, 12 },
			{ 0x340, SOIL, 2, {0}, 10 },
			{ 0x341, SOIL, 2, {0}, 3 },
			{ 0x342, SOIL, 2, {0}, 10 },
		}
};

uint16_t sensorTicks[5] = { 0 };
uint8_t can_echo_flag = 0;
uint16_t filterStationID = 0;

extern osTimerId CanTimeoutHandle;
volatile bool isInitialized = false;
uint16_t discoveredStations[25] = {0}, i_discoveredStations = 0;
uint16_t discoveredStations_bitfield = 0, r_discoveredStations_bitfield = 0;
volatile bool errorDiscoveringStation = false;


/* USER CODE BEGIN PFP */

int findIndexDiscoveringStationsByID( uint16_t id )
{
int index = -1;

	for( int i=0; i < i_discoveredStations; i++ )
	{
		if( discoveredStations[i] == id )
		{
			index = i;
			break;
		}
	}
	return index;
}

void newCANbusStation( uint16_t id )
{
	if( id == MCU_S_SUBSONICGAUGE )
		return;
	discoveredStations[i_discoveredStations] = id;
	discoveredStations_bitfield |= (1<<i_discoveredStations);
	i_discoveredStations++;
}

void discoveringStation( uint16_t id )
{
	int index = findIndexDiscoveringStationsByID( id );
	if( index >= 0 )
		r_discoveredStations_bitfield |= (1<<index);
	else
	{
		newCANbusStation( id );
	}
}

// Used to check if CAN-bus sensor is always running
void CanTimeoutCallback( void const * arg )
{
	if( !isInitialized )
	{
		// record sensors automatically
		//osTimerStart( CanTimeoutHandle, 800 );
		isInitialized = true;
	}
	else
	{
		// check if sensors are running
		errorDiscoveringStation = (r_discoveredStations_bitfield != discoveredStations_bitfield);
		r_discoveredStations_bitfield = 0;
	}
}

void LoraNotification()
{
osMessageQId destQueue;

	destQueue = getQueueId(LORA_COMM);

	MSG_STRUCT *p_sendMsg = allocMemoryPoolMessage(); // receiver must free
	p_sendMsg->opcode = OP_LORA_EVENT1;
	osMessagePut(destQueue, (uint32_t)p_sendMsg, osWaitForever);
}

int findSensorIdByAddress( uint16_t addr )
{
uint16_t addr16 = 0x300;

	addr16 += addr;
	for( int i=0; i<greenHouseSensors.nbSensors; i++ )
	{
		if( greenHouseSensors.sensors[i].address == addr16 )
			return i;
	}
	return -1;
}

void echoCanMessage( uint16_t addr, uint8_t* data, uint16_t sz)
{
osMessageQId destQueue;
uint8_t buff[15];

	destQueue = (osMessageQId)getQueueId(BLE_COMM);

	MSG_STRUCT *p_sendMsg = allocMemoryPoolMessage(); // receiver must free
	p_sendMsg->opcode = CAN_ECHO;
	p_sendMsg->lParam[0] = sz+4;
	memcpy(buff,"#C",2);
	HOST_TO_LE_16(&buff[2],addr);
	memcpy(&buff[4],data,sz);
	memcpy(&(p_sendMsg->data[2]),buff,sz+4);
	osMessagePut(destQueue, (uint32_t)p_sendMsg, osWaitForever);
}

int CAN_filterConfig(void)
{
	int ret = 0;

	//pcan_can_set_filter_mask( CAN_BUS_1, 0, 0, 0, 0 );
	CAN_FilterTypeDef sFilterConfig;

	sFilterConfig.FilterBank = 0;
	sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
	sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
	sFilterConfig.FilterIdHigh = 0x000 << 5;           // CAN address
	sFilterConfig.FilterIdLow = 0;
	sFilterConfig.FilterMaskIdHigh = 0x000 << 5;       // CAN address mask
	sFilterConfig.FilterMaskIdLow = 0;
	sFilterConfig.FilterFIFOAssignment = CAN_RX_FIFO0;
	sFilterConfig.FilterActivation = ENABLE;
	sFilterConfig.SlaveStartFilterBank = 14;

	HAL_CAN_ConfigFilter(&hcan1, &sFilterConfig);


	if( HAL_CAN_ActivateNotification( &hcan1, INTERNAL_CAN_IT_FLAGS ) != HAL_OK )
		ret = -1;

	if( HAL_CAN_Start( &hcan1 ) != HAL_OK )
	    ret = -1;

	return ret;
}

int pcan_rx_frame( uint8_t channel, struct t_can_msg *pmsg )
{
	HAL_GPIO_TogglePin(CAN_LED_GPIO_Port,CAN_LED_Pin);

	if( can_echo_flag )
	{
		echoCanMessage( pmsg->id, pmsg->data, pmsg->size);
	}

	if( !isInitialized )
	{
		int index = findIndexDiscoveringStationsByID( pmsg->id );
		if( index < 0 )
			newCANbusStation( pmsg->id );
	}
	else
	{
		// Set discovered stations register bitfield
		discoveringStation(pmsg->id);

		if ( pmsg->id == MCU_S_ANEMOMETER )
		{
			dev_anemometer device;

			memcpy(device.data,pmsg->data,pmsg->size);
			// Read temperature
			gh_outsideTemperature[0] = getOutsideTemperature(device);
			gh_atmPressure[0] = getAtmPressure(device);
			gh_windMaxBeaufort[0] = getWindBeaufort(device);
			gh_windDirection[0] = getWindDirection(device);
			gh_windSpeed[0] = getWindSpeed(device);

			//APP_LOG(TS_OFF, VLEVEL_M, "Greenhouse data: External Temp. %.1f°C, Atm. Pressure %dhPa\r\n",gh_outsideTemperature1, gh_atmPressure1 );
		}
		else if ( pmsg->id == MCU_S_WATERTANK )
		{
			dev_watertank device;

			memcpy(device.data,pmsg->data,pmsg->size);
			// Read tank level
			gh_waterTankLevel[0] = getWaterTankLevel(device);

			//APP_LOG(TS_OFF, VLEVEL_M, "Greenhouse data: Water tank level %d%%, Rain(%s)\r\n", gh_rain[0] == 1 ? "YES" : "NO" );
		}
		else if ( pmsg->id == MCU_S_PYRANOMETER )
		{
			dev_pyranometer device;

			memcpy(device.data,pmsg->data,pmsg->size);
			// Read solar irradiance inside the greenhouse
			gh_solarIrradiance[0] = getSolarIrradiance(device);
		}
		else if ( pmsg->id == MCU_S_THERMOMETER )
		{
			dev_thermometer device;

			memcpy(device.data,pmsg->data,pmsg->size);
			// Read current temperature inside the greenhouse
			gh_insideTemperature[0] = getInsideTemperature(device);
			// Read current relative humidity inside the greenhouse
			gh_insideHumidity[0] = getInsideRelativeHumidity(device);
		}
		else if ( pmsg->id == MCU_S_SUBSONICGAUGE ) // experimental function!!
		{
			dev_subsonic_gauge device;

			memcpy(device.data,pmsg->data,pmsg->size);
			// Read subsonic AI similarity
			gh_soilVibesAiSimilarity[0] = getSubsonicAiSimilarity(device);

			// Event system required lorawan messaging
			LoraNotification();
		}
		else if ( pmsg->id == MCU_S_DISDROMETER )
		{
			dev_disdrometer device;

			memcpy(device.data,pmsg->data,pmsg->size);

			// Read rain rate in mm/h
			gh_averageRainDropRate[0] = getAvgRainDropRate(device);
		}


		/*else if ( pmsg->id == MCU_S_RAINGAUGE )
		{
			dev_rain_gauge device;

			memcpy(device.data,pmsg->data,pmsg->size);

			// Read rain condition
			gh_rainingCondition[0] = getRainingCondition(device);
		}*/

	}
	return 0;
}

static void Can_Bus_processMsg(MSG_STRUCT *p_msg)
{
struct t_can_msg msg;

	if( p_msg->opcode == CAN_ECHO )
	{
		can_echo_flag = 1;
		filterStationID = p_msg->lParam[0];
	}
	else if( p_msg->opcode == CAN_NOECHO )
	{
		can_echo_flag = 0;
		filterStationID = 0;
	}
	else if( p_msg->opcode == OP_CAN )
	{
		dev_command_request request;

		request.RQ_Device = p_msg->lParam[0];
		request.RQ_OpCode = p_msg->lParam[1];
		request.setpoints[0] = p_msg->lParam[2];
		request.setpoints[1] = p_msg->lParam[3];

		msg.id = MCU_R_COMMAND;
		msg.size = 6;
		msg.flags = MSG_FLAG_STD;
		memcpy(msg.data,&request,msg.size);

		pcan_can_write( CAN_BUS_1, &msg );
	}

	osDelay( 10 );
}

static void Can_Bus_processFrame()
{
	pcan_can_poll();

	if( errorDiscoveringStation )
		addErrorToList( 0x207 );
	osDelay( 10 );
}

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

	osMessageQId myQueueId = getQueueId(CAN_COMM);

	pcan_can_install_rx_callback( CAN_BUS_1, pcan_rx_frame );

	CAN_filterConfig();
	APP_LOG(TS_OFF, VLEVEL_M, "CAN-bus started\r\n");
	HAL_GPIO_WritePin(CAN_LED_GPIO_Port,CAN_LED_Pin,GPIO_PIN_RESET);

	osStatus status = osTimerStart( CanTimeoutHandle, 5000 );
	if( status != osOK )
		APP_LOG(TS_OFF, VLEVEL_M, "Error discovering CAN-bus sensors\r\n");

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

	      Can_Bus_processMsg(p_recvMsg);
	      freeMemoryPoolMessage(p_recvMsg);
	    }
	    else if (event.status == osEventTimeout) {
	      Can_Bus_processFrame();
	    }
	}
}
