/* 
 * This file is part of the  distribution https://git.mosad.xyz/localhorst/ParallelTrafficRouter_FreeRTOS
 * Copyright (c) 2019 Hendrik Schutter.
 * 
 * This program is free software: you can redistribute it and/or modify  
 * it under the terms of the GNU General Public License as published by  
 * the Free Software Foundation, version 3.
 *
 * This program is distributed in the hope that it will be useful, but 
 * WITHOUT ANY WARRANTY; without even the implied warranty of 
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License 
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "demo.h"

#define SIMULATE_COST

SemaphoreHandle_t xMutexData;

void vApplicationMallocFailedHook( void );
void vApplicationIdleHook( void );
void vApplicationTickHook( void );
void search(void* arguments);
void show(void* arguments);

int main()
{
    printf("starting routing ...\n");

    xMutexData = xSemaphoreCreateMutex();

    THREAD_DATA_t* data;
    data = (THREAD_DATA_t*) pvPortMalloc(sizeof(THREAD_DATA_t));
    data->node = getStartNode();
    data->cost = 0;
    data->depth = 0;
    data->predecessors[0] = data->node->id;
    data->bestpath = (BESTPATH_t*) pvPortMalloc(sizeof(BESTPATH_t));
    data->bestpath->lowestCost = -1;
    data->bestpath->depth = 0;

    xTaskCreate(search, "startTask", configMINIMAL_STACK_SIZE, (void*) data, 1, 0);
    xTaskCreate(show, "showTask", configMINIMAL_STACK_SIZE, (void*) data, 0, 0);

    vTaskStartScheduler();

    return 0;
}


void search(void* arguments)
{

    THREAD_DATA_t *data = (THREAD_DATA_t*) arguments;

    printf("Node: %i reached\n", data->node->id);

#ifdef SIMULATE_COST
    vTaskDelay(((data->cost)*20)/ portTICK_PERIOD_MS); //receive data for route
#endif

    if(data->node->id == 8) //end node reached
    {
        printf("Route found! Cost: %i\n", data->cost);

        xSemaphoreTake( xMutexData, ( TickType_t ) 1 );

        if((data->bestpath->lowestCost == -1) || (data->bestpath->lowestCost > data->cost))
            //first route found or better route found
        {
            data->bestpath->lowestCost = data->cost;
            data->bestpath->depth = data->depth;
            for(int j = 0; j < data->depth; j++)
            {
                data->bestpath->predecessorsBestPath[j] = data->predecessors[j];
            }
            data->bestpath->predecessorsBestPath[(data->bestpath->depth)] = data->node->id;
        }

        xSemaphoreGive( xMutexData );
    }
    else
    {
        //create new tasks
        for(int i = 0; i < data->node->nodescount; i++)
        {
            THREAD_DATA_t* nextdata;
            nextdata = (THREAD_DATA_t*) pvPortMalloc(sizeof(THREAD_DATA_t));
            //nextdata = (THREAD_DATA_t*) pvPortMalloc(1000000000);
            nextdata->node = (data->node->nodesnext[i]);
            nextdata->bestpath = data->bestpath;
            nextdata->cost = data->node->nodescost[i] + data->cost; //sum up cost
            nextdata->depth = data->depth + 1;

            for(int j = 0; j < data->depth; j++) {
                nextdata->predecessors[j] = data->predecessors[j];
            }
            nextdata->predecessors[((nextdata->depth) - 1)] = data->node->id;

            xTaskCreate(search, "searchTask", configMINIMAL_STACK_SIZE, (void*) nextdata, 2, 0);
        }
    }
    printf("Node: %i exit\n", data->node->id);
    
    vTaskDelete( NULL );
}


void show(void* arguments)
{
    THREAD_DATA_t *data = (THREAD_DATA_t*) arguments;
    for(;;)
    {
        xSemaphoreTake( xMutexData, ( TickType_t ) 1 );
        if(data->bestpath->lowestCost > -1)
        {
            printf("\nBest route: %i", data->bestpath->predecessorsBestPath[0]);
            for(int i = 1; i <= data->bestpath->depth; i++) {
                printf(" --> %i", data->bestpath->predecessorsBestPath[i]);
            }
            printf("\nWith cost of: %i\n", data->bestpath->lowestCost);
        }
        xSemaphoreGive( xMutexData );
        vTaskDelay(1000 / portTICK_PERIOD_MS); //wait 1sec
    }
    vTaskDelete( NULL );
}


void vApplicationMallocFailedHook( void )
{
    printf("Malloc failed\n");
}


void vApplicationIdleHook( void )
{
//printf("No tasks to do\n");
}


void vApplicationTickHook( void )
{
//printf("TickHook\n");
}