#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
//#include "xscugic.h"

#include "xsysmon.h"
#include "xspi.h"

#include "project.h"
#include "ethernet.h"
#include "pl_udp_eth.h"
#include "hmc7044.h"
#include "ad9081_hal_functions.h"
#include "ad9081_app_helper.h"
#include "adi_ad9081.h"
#include "adi_hmc7044.h"
#include "novatel.h"
#include "config_flash.h"

XSysMon SysMonInst;
//extern XScuGic xInterruptController;

void set_fan_pwm(int percent) {
    // Set Fan PWM
	// Don't try to turn on fan unless percent is above a certain value, fan has minimum turn on percent
	int period = 1500000;
	int pw = period * percent / 100;
    Xil_Out32(0x40050010, period);
    Xil_Out32(0x40050014, pw);
}

void update_fan_speed(int fpga_temp) {
	int min_temp = 30;
	int max_temp = 60;
	int delta_temp = max_temp - min_temp;
	int min_percent = 50;
	int percent = 0;

	int temp = fpga_temp - min_temp;
	if (temp < 0) {
		temp = 0;
		set_fan_pwm(0);
	}
	else {
		percent = temp * 100 / delta_temp;
		if (percent < min_percent) {
			percent = min_percent;
		}
		set_fan_pwm(percent);
	}

//	xil_printf("Temp - %d, Fan Percent - %d\r\n", fpga_temp, percent);
}

int setBit(uint32_t addr, uint32_t bit) {
	int val = Xil_In32(addr);
	val |= (1 << bit);
	Xil_Out32(addr, val);
	return 0;
}

int clearBit(uint32_t addr, uint32_t bit) {
	int val = Xil_In32(addr);
	val &= ~(1 << bit);
	Xil_Out32(addr, val);
	return 0;
}

int toggleBit(uint32_t addr, uint32_t bit) {
	int val = Xil_In32(addr);
	uint32_t current_bit = val & (1 << bit);
	if (current_bit) {
		val &= ~(1 << bit);
	} else {
		val |= (1 << bit);
	}
	Xil_Out32(addr, val);
	return 0;
}

void error_print(int line_number, int err) {
	xil_printf("Line %d, Error! %d\r\n", line_number, err);
	vTaskDelay(1000);
}

void status_task( void *pvParameters ) {
	XSysMon_Config *ConfigPtr;
	XSysMon *SysMonInstPtr = &SysMonInst;

	// Initialize the SysMon driver.
	ConfigPtr = XSysMon_LookupConfig(XPAR_SYSMON_0_DEVICE_ID);
	XSysMon_CfgInitialize(SysMonInstPtr, ConfigPtr,
				ConfigPtr->BaseAddress);
	XSysMon_SelfTest(SysMonInstPtr);
	
	while (1) {
		u16 temp_data = XSysMon_GetAdcData(SysMonInstPtr, XSM_CH_TEMP);
		int fpga_temp = XSysMon_RawToTemperature(temp_data);
		update_fan_speed(fpga_temp);

		vTaskDelay(100);
	}
}

static void pps_irq_handler(u32 context) {
	xil_printf("pps irq %lu\r\n", utc_time);
	Xil_Out32(TIMING_ENGINE_ADDR + 0x14, utc_time + 1);
}

void main_task( void *pvParameters ) {
 	// Connect PPS Interrupt
 	xPortInstallInterruptHandler(XPAR_MICROBLAZE_0_AXI_INTC_SYSTEM_PPS_INTR, (XInterruptHandler) pps_irq_handler, (void *)0);
 	vPortEnableInterrupt(XPAR_MICROBLAZE_0_AXI_INTC_SYSTEM_PPS_INTR);

	setup_data_converter();

// 	config_flash_sector_erase(CONFIG_BASE_ADDRESS);

	while (1) {
		toggleBit(GPO_REG, 0);  // Toggle LED
		vTaskDelay(100);
	}

}

int main(void) {
    xil_printf("\n\r\n\r================= Start ====================\n\r\n\r");

    Xil_Out32(GPO_REG, 0x11);
    config_flash_init();

	xTaskCreate( 	status_task,
					( const char * ) "status",
					0x1000,
					NULL,
					TASK_PRIORITY_STATUS,
					NULL );

	xTaskCreate( 	main_task,
					( const char * ) "main",
					0x20000,
					NULL,
					TASK_PRIORITY_MAIN,
					NULL );

#ifndef IBERT_TESTING
	xTaskCreate( 	pl_udp_task,
					( const char * ) "pludp",
					0x8000,
					NULL,
					TASK_PRIORITY_MAIN,
					NULL );
#endif

	// This is the thread that hosts lwip.  It must be created using sys_thread_new
	sys_thread_new("tcp_server_task", (void(*)(void*))tcp_server_task, 0,
					1024,
					TASK_PRIORITY_LWIP);

    /* Start the tasks and timer running. */
    vTaskStartScheduler();

    return 0;
}