#include "at32f403a_407.h"
#include "at32f403a_407_board.h"
#include "at32f403a_407_clock.h"
#include "common_config.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "usb_eth.h"
#include "mux.h"
#include "misc.h"
#include "spi_common.h"
#include "extended_sram.h"
#include "modbus.h"
#include "common_gpio.h"
#include "io.h"
#include "input.h"
#include "wdt.h"
#include <stdio.h>
#include <stdbool.h>
#include <string.h>


// 
bool jump_to_app(uint32_t address);
void (*pftarget)(void);

bool boot_failed = false;



int main()
{
    uint8_t load_mode;
	uint8_t boot_try;
	uint8_t fw_invalid = 0;
    
    nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);
    
    crm_periph_clock_enable(CRM_PWC_PERIPH_CLOCK, TRUE);
    crm_periph_clock_enable(CRM_BPR_PERIPH_CLOCK, TRUE);
    pwc_battery_powered_domain_access(TRUE);
    
    system_clock_config();
    
    delay_init();

    uart_print_init(115200);
    
    // Флаг - нужно ли обновляться
    load_mode = bpr_data_read(BPR_DATA1);
    
    // Число попыток загрузки основного ПО
    boot_try = bpr_data_read(BPR_DATA2);
    
    DBG printf("[IAP] load_mode: %u, boot_try: %u\r\n", load_mode, boot_try);
    
    // Если есть попытки, то пытаемся загрузить FW.
	// Используем ограниченное число попыток загрузить основное FW.
    if (boot_try > 1)
    {
        boot_try--;
        bpr_data_write(BPR_DATA2, boot_try);
        
        if (!jump_to_app(USER_FLASH_FIRST_PAGE_ADDRESS))
        {
            // Флеш пустая. Нечего загружать
            fw_invalid = 1;
            boot_failed = false;
            DBG printf("FW empty! Bootloader starting...\n\r");
        }
    }
    else if (boot_try == 1)
    {
        boot_failed = true;
        DBG printf("FW boot failed. Bootloader starting...\r\n");
        
        load_mode = 1;
        boot_try = 0;
        bpr_data_write(BPR_DATA1, load_mode);
        bpr_data_write(BPR_DATA2, boot_try);
    }
    
    if  ((load_mode == 0) && (!fw_invalid))
    {
        DBG printf("Run main FW\r\n");
        bpr_data_write(BPR_DATA2, BOOT_TRY);
        DBG printf("Go to main FW...\r\n");
        jump_to_app(USER_FLASH_FIRST_PAGE_ADDRESS);
    }
    
        
    DBG printf("IAP starting...\r\n");
        
    // Для теста. Сброс.
    DBG printf("For test reset load_mode flag and jump to FW\r\n");
    bpr_data_write(BPR_DATA1, 0);
    jump_to_app(0x08021000); 
    
    
    while (1) {}
}


// 
bool jump_to_app(uint32_t address)
{
    uint32_t stkptr, jumpaddr;
    stkptr = *(uint32_t *)address;
    jumpaddr = *(uint32_t *)(address + sizeof(uint32_t));
 
    if (((*(__IO uint32_t*)USER_FLASH_FIRST_PAGE_ADDRESS) & 0x20000000) == 0x20000000) 
    {
        __set_MSP(stkptr);
        pftarget = (void (*) (void))jumpaddr;
        pftarget();
    }
    return false;
}