#include "malloc.h"
#include "malloc_private.h"
#include "stdio.h"

heap_ll * _heap_head[NB_TASKS_MAX];

heap_ll * _remote_free_list[NB_TASKS_MAX] = { 0 };
giet_lock_t lock_rf_list[NB_TASKS_MAX] = { {"toto",0} };

unsigned int _heap_base[NB_TASKS_MAX] = { -1 };
unsigned int _heap_length[NB_TASKS_MAX] = { -1 };

int _first_malloc[NB_TASKS_MAX] = { 0 };


#if MALLOC_SELECTED == 1

#elif MALLOC_SELECTED == 2
heap_ll * _prev_next_last_allocted[NB_TASKS_MAX] = { 0 };
#else
#define MAX_SIZE_POW2_TAB 28 
#define GET_TAB_INDEX(size)                 (size == 0x00000008) ? 0 :\
                                            (size <= 0x00000010) ? 1 :\
                                            (size <= 0x00000020) ? 2 :\
                                            (size <= 0x00000040) ? 3 :\
                                            (size <= 0x00000080) ? 4 :\
                                            (size <= 0x00000100) ? 5 :\
                                            (size <= 0x00000200) ? 6 :\
                                            (size <= 0x00000400) ? 7 :\
                                            (size <= 0x00000800) ? 8 :\
                                            (size <= 0x00001000) ? 9 :\
                                            (size <= 0x00002000) ? 10 :\
                                            (size <= 0x00004000) ? 11 :\
                                            (size <= 0x00008000) ? 12 :\
                                            (size <= 0x00010000) ? 13 :\
                                            (size <= 0x00020000) ? 14 :\
                                            (size <= 0x00040000) ? 15 :\
                                            (size <= 0x00080000) ? 16 :\
                                            (size <= 0x00100000) ? 17 :\
                                            (size <= 0x00200000) ? 18 :\
                                            (size <= 0x00400000) ? 19 :\
                                            (size <= 0x00800000) ? 20 :\
                                            (size <= 0x01000000) ? 21 :\
                                            (size <= 0x02000000) ? 22 :\
                                            (size <= 0x04000000) ? 23 :\
                                            (size <= 0x08000000) ? 24 :\
                                            (size <= 0x10000000) ? 25 :\
                                            (size <= 0x20000000) ? 26 :\
                                                                   27

heap_ll * _pow2tab[NB_TASKS_MAX][MAX_SIZE_POW2_TAB] = {{ 0 }};
#endif


#if DEBUG_MALLOC == 1              
#define giet_printf(...)                \
    ({                                      \
     giet_tty_printf(__VA_ARGS__);       \
     })                                  
#else
#define giet_printf(...) ({   })
#endif



#if MALLOC_SELECTED == 1
/*########################################################################*/
/**************************************************************************/
/**********                     WORST-FIT                       ***********/
/**************************************************************************/
/*########################################################################*/
heap_ll *get_prev_fit_chunk(unsigned int size) {
    int task_id = giet_global_task_id();
    int check_remote_free_list = 0;
after_remote_list_check :
    if (_heap_head[task_id] == 0x0) {
        if(check_remote_free_list == 0)
        {
            heap_ll * poped_block;
            heap_ll * head_of_rf;
            check_remote_free_list = 1;

            head_of_rf = pop_ptr();

            while((poped_block = pop_remote_free_list(&head_of_rf)) != 0x0)
            {
                update_chunk_list((unsigned int)poped_block, poped_block->chunk_length);
            }
            goto after_remote_list_check; 
        }
        return ((heap_ll *) 0x01);
    }
    int found = 0;
    heap_ll * ptr_to_chunk_list = _heap_head[task_id];
    heap_ll * tmp = 0x0;
    heap_ll * prev = 0x0;
    heap_ll * prev_tmp = 0x0;
    while (ptr_to_chunk_list != 0x0) {
        if (size <= ptr_to_chunk_list->chunk_length) {
            if (tmp == 0x0 || ptr_to_chunk_list->chunk_length > tmp->chunk_length) {
                prev_tmp = prev;
                tmp = ptr_to_chunk_list;
                found = 1;
            }
        }
        prev = ptr_to_chunk_list;
        ptr_to_chunk_list = ptr_to_chunk_list->next;
    }
    if (found == 1)
    {
        return prev_tmp; // case when prev_tmp == 0x0 is handled in the calling function
    }
    else
    {
        if(check_remote_free_list == 0)
        {
            heap_ll * poped_block;
            heap_ll * head_of_rf;
            check_remote_free_list = 1;

            head_of_rf = pop_ptr();

            while((poped_block = pop_remote_free_list(&head_of_rf)) != 0x0)
            {
                update_chunk_list((unsigned int)poped_block, poped_block->chunk_length);
            }
            goto after_remote_list_check; 
        }
        return (heap_ll *) 0x1;
    }
}



void * malloc(unsigned int size) {
    int task_id = giet_global_task_id();

    giet_printf("############ MALLOC ############\n\n");
    if (size == 0) {

        giet_printf(" SIZE = 0 \n");
        return 0x0;
    }

    /****** First call to malloc ******/
    if (_first_malloc[task_id] == 0) {
        giet_heap_info(&_heap_base[task_id], &_heap_length[task_id]);

        if (_heap_length[task_id] == 0) {
            giet_printf("*** Error: Malloc called in task %d and no heap was defined.\n", task_id);
            return 0x0;
        }
        _heap_head[task_id] = (heap_ll *) _heap_base[task_id];
        _heap_head[task_id]->chunk_length = _heap_length[task_id];
        _heap_head[task_id]->next = 0x0;


        _first_malloc[task_id] = 1;
    }

    /****** Size align ******/
    int size_align = size;
    if (size % 4 != 0) {
        size_align = ((size / 4) + 1) * 4;
    }



    giet_printf("Looking for size : %d\n", size_align + 4);
    /****** Get prev victim from the chunks list ******/
    heap_ll *victim;
    heap_ll *prev_victim;
    //// WORST

    if ((prev_victim = get_prev_fit_chunk(size_align + 4)) == (heap_ll *) 0x1) {
        return 0x0;		// no chunk of this size avaiable, 0x1 : no suitable chunk found
    }
    else {
        if (prev_victim != 0x0)	{
            // victim != HEAD
            victim = prev_victim->next;
        }
        else {
            victim = _heap_head[task_id];
        }
    }
    /****** Get Victim Base ******/
    unsigned int victim_vbase = (unsigned int) victim;	// overhead

    /****** update the chunks list ******/
    if (victim->chunk_length == size_align + 4) {
        // if it is an exact match
        if (prev_victim == 0x0)	{
            // if victim == head
            _heap_head[task_id] = victim->next;
        }
        else {
            prev_victim->next = victim->next;
        }
    } 
    else {
        // if its not an exact match then update chunk base and length
        heap_ll * tmp_chunk = victim;
        victim = (heap_ll *) (((unsigned int) victim) + (size_align + 4));

        giet_printf("NEXT CHUNK is at @ : 0x%x + 0x%x = 0x%x\n",
                (unsigned int) tmp_chunk,
                (unsigned int) ((size_align + 4) / 4),
                (unsigned int) victim);

        victim->chunk_length = tmp_chunk->chunk_length - (size_align + 4);
        victim->next = tmp_chunk->next;
        if (prev_victim == 0x0) {
            _heap_head[task_id] = victim;
        }
        else {
            prev_victim->next = victim;
        }
    }

    /****** Write Overhead ******/
    *((unsigned int *) victim_vbase) = size_align;	// writing overhead on the first word

    giet_printf("BLOCK SUCCESSFULLY ALLOCATED at @ 0x%x user will write at @ 0x%x\n\n"\
            ,(unsigned int)(victim_vbase),(unsigned int) (victim_vbase + 4));
    return (unsigned int *) (victim_vbase + 4);
}



/* this function tries to merge the block to free with chunks in the list if
 * they are contiguous
 * the block can be merged two times.
 */
void update_chunk_list(unsigned int block_base, unsigned int block_length) {
    /* entire heap is allocated : create a new block of size block_length and give it to the head  */
    int task_id = giet_global_task_id();
    if (_heap_head[task_id] == 0x0) {
        ((heap_ll *) block_base)->chunk_length = block_length;
        ((heap_ll *) block_base)->next = 0x0;
        _heap_head[task_id] = ((heap_ll *) block_base);

        giet_printf("****** NEW BLOCK ******\n"
                "@ : %x\n"
                "Length : %d o\n"
                "***********************\n",
                block_base,
                (unsigned int) (((heap_ll *) block_base)->chunk_length));
        return;
    }


    /* Test for each Chunk in my list if the block can be merged with */
    heap_ll *ptr_to_chunk_list = _heap_head[task_id]; // move through the list
    heap_ll *prev = 0x0; // pointer on the previous visited chunk in the list
    heap_ll *prev_block_base = 0x0; // pointer on the previous chunk of the chunk that contains the merged block in the list
    unsigned int chunk_length;
    int block_merged = 0; // =1 when the block has already been merged with a chunk in the list
    while (ptr_to_chunk_list != 0x0) {
        chunk_length = ptr_to_chunk_list->chunk_length;
        /*  [.....|ptr|block|.....] */
        if ((unsigned int) (ptr_to_chunk_list) + chunk_length == block_base) {

            giet_printf("****** BLOCK MERGED ******\n"
                    "CHUNK : %x of size %d o\n"
                    "merged with \n"
                    "BLOCK : %x of size %d o\n"
                    "***********************\n",
                    (unsigned int) ptr_to_chunk_list,
                    ptr_to_chunk_list->chunk_length, block_base,
                    block_length);

            /* update the size */
            ptr_to_chunk_list->chunk_length = chunk_length + block_length;

            /* [.....|ptr|[block|old_ptr]|.....]
             *  remove ptr from the list
             *  update next pointer
             *  update the previous chunk of block base to point to ptr
             */
            if (block_merged == 1) {
                prev->next = ptr_to_chunk_list->next;
                ptr_to_chunk_list->next = ((heap_ll *) block_base)->next;
                if (prev_block_base != 0x0) {
                    prev_block_base->next = ptr_to_chunk_list;
                }
                else {
                    _heap_head[task_id] = ptr_to_chunk_list;
                }
            }
            /****** for next turn ******/
            block_base = (unsigned int) ptr_to_chunk_list;
            block_length = ptr_to_chunk_list->chunk_length;

            giet_printf("****** NEW BLOCK ******\n"
                    "@ : %x\n"
                    "Length : %d o\n"
                    "***********************\n",
                    (unsigned int) ptr_to_chunk_list,
                    (unsigned int) (ptr_to_chunk_list->chunk_length));
            prev_block_base = prev;
            prev = ptr_to_chunk_list;
            ptr_to_chunk_list = ptr_to_chunk_list->next;
            block_merged = 1;
            continue;

        } // end [.....|ptr|block|.....]
        /*  [......|block|ptr|.....] */
        if (block_base + block_length == (unsigned int) ptr_to_chunk_list) {

            giet_printf("****** BLOCK MERGED ******\n"
                    "BLOCK : %x of size %d o\n"
                    "merged with \n"
                    "CHUNK : %x of size %d o\n"
                    "***********************\n",
                    block_base, block_length,
                    (unsigned int) ptr_to_chunk_list,
                    ptr_to_chunk_list->chunk_length);

            /* update the size */
            ((heap_ll *) block_base)->chunk_length = block_length + chunk_length;

            /* [.....|block|ptr|......] */
            if (block_merged == 0) {
                /* update next pointer
                 *  update the previous chunk of ptr to point to block_base
                 */
                ((heap_ll *) block_base)->next = ptr_to_chunk_list->next;
                if (prev == 0x0) {
                    _heap_head[task_id] = ((heap_ll *) block_base);
                }
                else {
                    prev->next = ((heap_ll *) block_base);
                }
            }
            /* [.....[old_ptr|block]|ptr|......] */
            else {
                /* remove ptr from the list */
                prev->next = ptr_to_chunk_list->next;
            }

            ptr_to_chunk_list = ((heap_ll *) block_base);
            block_length = ptr_to_chunk_list->chunk_length;

            giet_printf("****** NEW BLOCK ******\n"
                    "@ : %x\n"
                    "Length : %d o\n"
                    "***********************\n",
                    (unsigned int) ptr_to_chunk_list,
                    (unsigned int) (ptr_to_chunk_list->chunk_length));
            prev_block_base = prev;
            block_merged = 1;
        } // end [......|block|ptr|.....]
        prev = ptr_to_chunk_list;
        ptr_to_chunk_list = ptr_to_chunk_list->next;
    } // end while
    if (block_merged == 1) {
        return;
    }
    /* if the block cant be merge
     *  create new block and add it to the end of the list 
     */
    ((heap_ll *) block_base)->chunk_length = block_length;
    ((heap_ll *) block_base)->next = 0x0;
    prev->next = ((heap_ll *) block_base);

    giet_printf("****** NEW BLOCK ******\n"
            "@ : %x\n"
            "Length : %d o\n"
            "***********************\n",
            block_base,
            (unsigned int) (((heap_ll *) block_base)->chunk_length));
    return;

}



/*########################################################################*/
/**************************************************************************/
/**********                 END WORST-FIT                       ***********/
/**************************************************************************/
/*########################################################################*/
#elif MALLOC_SELECTED == 2 // Next Fit 
/*########################################################################*/
/**************************************************************************/
/**********                   NEXT-FIT                          ***********/
/**************************************************************************/
/*########################################################################*/

heap_ll * get_prev_fit_chunk(unsigned int size) {
    int task_id = giet_global_task_id();
    int check_remote_free_list = 0;
after_remote_list_check_n : //n for next_fit
    if (_heap_head[task_id] == 0x0) {
        if(check_remote_free_list == 0)
        {
            heap_ll * poped_block;
            heap_ll * head_of_rf;
            check_remote_free_list = 1;

            head_of_rf = pop_ptr();

            while((poped_block = pop_remote_free_list(&head_of_rf)) != 0x0)
            {
                update_chunk_list((unsigned int)poped_block, poped_block->chunk_length);
            }
            goto after_remote_list_check_n; 
        }
        return ((heap_ll *) 0x1);
    }

    heap_ll * ptr_to_chunk_list;
    heap_ll * prev = 0x0;
    heap_ll * tmp_turn = 0x0;

    if (_prev_next_last_allocted[task_id] == 0x0 || _prev_next_last_allocted[task_id]->next == 0x0) {
        ptr_to_chunk_list = _heap_head[task_id];
    }
    else
    {
        ptr_to_chunk_list = _prev_next_last_allocted[task_id]->next;
    }

    tmp_turn = (ptr_to_chunk_list == 0x0)?_heap_head[task_id]:ptr_to_chunk_list;
    do {
        if (ptr_to_chunk_list == 0x0) {
            // simulate circular list
            ptr_to_chunk_list = _heap_head[task_id];
            if (ptr_to_chunk_list == tmp_turn) break;
        }

        if (size <= ptr_to_chunk_list->chunk_length) {
            return prev;
        }
        prev = ptr_to_chunk_list;
        ptr_to_chunk_list = ptr_to_chunk_list->next;
    } while (ptr_to_chunk_list != tmp_turn);
    if(check_remote_free_list == 0)
    {
        heap_ll * poped_block;
        heap_ll * head_of_rf;
        check_remote_free_list = 1;

        head_of_rf = pop_ptr();

        while((poped_block = pop_remote_free_list(&head_of_rf)) != 0x0)
        {
            update_chunk_list((unsigned int)poped_block, poped_block->chunk_length);
        }
        goto after_remote_list_check_n; 
    }

    return (heap_ll *) 0x1;
}


void * malloc(unsigned int size) {

    int task_id = giet_global_task_id();

    giet_printf("############ MALLOC ############\n\n");
    if (size == 0) {

        giet_printf(" SIZE = 0 \n");
        return 0x0;
    }

    /****** First call to malloc ******/
    if (_first_malloc[task_id] == 0) {
        giet_heap_info(&_heap_base[task_id], &_heap_length[task_id]);

        if (_heap_length[task_id] == 0) {
            giet_printf("*** Error: Malloc called in task %d while no heap was defined.\n", task_id);
            return 0x0;
        }

        _heap_head[task_id] = (heap_ll *) _heap_base[task_id];
        _heap_head[task_id]->chunk_length = _heap_length[task_id];
        _heap_head[task_id]->next = 0x0;

        _prev_next_last_allocted[task_id] = 0x0;

        _first_malloc[task_id] = 1;
    }

    /****** Size align ******/
    int size_align = size;
    if (size % 4 != 0) {
        size_align = ((size / 4) + 1) * 4;
    }



    giet_printf("Looking for size : %d\n", size_align + 4);
    /****** Get prev victim from the chunks list ******/
    heap_ll *victim;
    heap_ll *prev_victim;

    if ((prev_victim = get_prev_fit_chunk(size_align + 4)) == (heap_ll *) 0x1) {
        return 0x0;		// no chunk of this size avaiable, 0x1 : no suitable chunk found
    }
    else {
        if (prev_victim != 0x0) {
            // victim != _next_last_allocted
            if (prev_victim->next == 0x0) {
                // victim == HEAD
                victim = _heap_head[task_id];
            }
            else {
                victim = prev_victim->next;
            }
        }
        else
        {
            if (_prev_next_last_allocted[task_id] == 0x0 || _prev_next_last_allocted[task_id]->next == 0x0) {
                victim =_heap_head[task_id];
            }
            else {
                victim = _prev_next_last_allocted[task_id]->next;
            }
        }
    }

    /****** Get Victim Base ******/
    unsigned int victim_vbase = (unsigned int) victim; // overhead

    /****** update the chunks list ******/
    if (victim->chunk_length - (size_align + 4) < 8) {
        // if it is an exact match
        size_align = size_align + (victim->chunk_length - (size_align + 4));

        if (prev_victim == 0x0)	{
            // if victim == head
            if (_prev_next_last_allocted[task_id] == 0x0 || _prev_next_last_allocted[task_id]->next == 0x0) {
                _heap_head[task_id] = victim->next;
            }
            else {
                _prev_next_last_allocted[task_id]->next = victim->next;
            }
            _prev_next_last_allocted[task_id] = victim->next;
        }
        else {
            if (prev_victim->next == 0x0) {
                _heap_head[task_id] = victim->next;
            }
            else {
                prev_victim->next = victim->next;
            }
            _prev_next_last_allocted[task_id] = prev_victim;
        }
    } 
    else {
        // if its not an exact match then update chunk base and length
        heap_ll * tmp_chunk = victim;
        victim = (heap_ll *) (((unsigned int) victim) + (size_align + 4));

        giet_printf("NEXT CHUNK is at @ : 0x%x + 0x%x = 0x%x\n",
                (unsigned int) tmp_chunk,
                (unsigned int) ((size_align + 4) / 4),
                (unsigned int) victim);

        victim->chunk_length = tmp_chunk->chunk_length - (size_align + 4);
        victim->next = tmp_chunk->next;
        if (prev_victim == 0x0) {
            if (_prev_next_last_allocted[task_id] == 0x0 || _prev_next_last_allocted[task_id]->next == 0x0) {
                _heap_head[task_id] = victim;
            }
            else {
                _prev_next_last_allocted[task_id]->next = victim;
            }
            _prev_next_last_allocted[task_id] = victim->next;
        }
        else {
            if (prev_victim->next == 0x0) {
                _heap_head[task_id] = victim;
            }
            else {
                prev_victim->next = victim;
            }
            _prev_next_last_allocted[task_id] = prev_victim;
        }
    }

    /****** Write Overhead ******/
    *((unsigned int *) victim_vbase) = size_align;	// writing overhead on the first word

    giet_printf("BLOCK SUCCESSFULLY ALLOCATED at @ 0x%x user will write at @ 0x%x\n\n"\
            ,(unsigned int)(victim_vbase),(unsigned int )(victim_vbase + 4));
    return (unsigned int *) (victim_vbase + 4);
}



/* this function tries to merge the block to free with chunks in the list if they are contiguous
 * the block can be merged two times.
 */

void update_chunk_list(unsigned int block_base, unsigned int block_length) {
    int task_id = giet_global_task_id();
    /* entire heap is allocated : create a new block of size block_length and give it to the head  */
    if (_heap_head[task_id] == 0x0) {
        ((heap_ll *) block_base)->chunk_length = block_length;
        ((heap_ll *) block_base)->next = 0x0;
        _heap_head[task_id] = ((heap_ll *) block_base);

        giet_printf("****** NEW BLOCK ******\n"
                "@ : %x\n"
                "Length : %d o\n"
                "***********************\n",
                block_base,
                (unsigned int) (((heap_ll *) block_base)->chunk_length));
        return;
    }

    /* Test for each Chunk in my list if the block can be merged with */
    heap_ll * ptr_to_chunk_list = _heap_head[task_id]; // move through the list
    heap_ll * prev = 0x0; // pointer on the previous visited chunk in the list
    heap_ll * prev_block_base = 0x0; // pointer on the previous chunk of the chunk that contains the merged block in the list
    unsigned int chunk_length;
    int block_merged = 0; // =1 when the block has already been merged with a chunk in the list
    while (ptr_to_chunk_list != 0x0) {
        chunk_length = ptr_to_chunk_list->chunk_length;
        /*  [.....|ptr|block|.....] */
        if ((unsigned int) (ptr_to_chunk_list) + chunk_length == block_base) {

            giet_printf("****** BLOCK MERGED ******\n"
                    "CHUNK : %x of size %d o\n"
                    "merged with \n"
                    "BLOCK : %x of size %d o\n"
                    "***********************\n",
                    (unsigned int) ptr_to_chunk_list,
                    ptr_to_chunk_list->chunk_length, block_base,
                    block_length);
            if (_prev_next_last_allocted[task_id] == ptr_to_chunk_list) {
                _prev_next_last_allocted[task_id] = ptr_to_chunk_list->next;
            }

            /* update the size */
            ptr_to_chunk_list->chunk_length = chunk_length + block_length;

            /* [.....|ptr|[block|old_ptr]|.....]
             *  remove ptr from the list
             *  update next pointer
             *  update the previous chunk of block base to point to ptr
             */
            if (block_merged == 1) {
                prev->next = ptr_to_chunk_list->next;
                ptr_to_chunk_list->next = ((heap_ll *) block_base)->next;

                if (prev_block_base != 0x0) {
                    prev_block_base->next = ptr_to_chunk_list;
                }
                else {
                    _heap_head[task_id] = ptr_to_chunk_list;
                }
            }
            /****** for next turn ******/
            block_base = (unsigned int) ptr_to_chunk_list;
            block_length = ptr_to_chunk_list->chunk_length;

            giet_printf("****** NEW BLOCK ******\n"
                    "@ : %x\n"
                    "Length : %d o\n"
                    "***********************\n",
                    (unsigned int) ptr_to_chunk_list,
                    (unsigned int) (ptr_to_chunk_list->chunk_length));
            prev_block_base = prev;
            prev = ptr_to_chunk_list;
            ptr_to_chunk_list = ptr_to_chunk_list->next;
            block_merged = 1;
            continue;

        } // end [.....|ptr|block|.....]
        /*  [......|block|ptr|.....] */
        if (block_base + block_length == (unsigned int) ptr_to_chunk_list) {

            giet_printf("****** BLOCK MERGED ******\n"
                    "BLOCK : %x of size %d o\n"
                    "merged with \n"
                    "CHUNK : %x of size %d o\n"
                    "***********************\n",
                    block_base, block_length,
                    (unsigned int) ptr_to_chunk_list,
                    ptr_to_chunk_list->chunk_length);

            if (_prev_next_last_allocted[task_id] == ptr_to_chunk_list) {
                _prev_next_last_allocted[task_id] = ptr_to_chunk_list->next;
            }

            /* update the size */
            ((heap_ll *) block_base)->chunk_length = block_length + chunk_length;

            /* [.....|block|ptr|......] */
            if (block_merged == 0) {
                /* update next pointer
                 *  update the previous chunk of ptr to point to block_base
                 */
                ((heap_ll *) block_base)->next = ptr_to_chunk_list->next;
                if (prev == 0x0) {
                    _heap_head[task_id] = ((heap_ll *) block_base);
                }
                else {
                    prev->next = ((heap_ll *) block_base);
                }
            }
            /* [.....[old_ptr|block]|ptr|......] */
            else {
                /* remove ptr from the list */
                prev->next = ptr_to_chunk_list->next;
                if (prev_block_base != 0x0) {
                    prev_block_base->next = ((heap_ll *)block_base);
                }
                else {
                    _heap_head[task_id] = ((heap_ll *)block_base);
                }
            }

            ptr_to_chunk_list = ((heap_ll *) block_base);
            block_length = ptr_to_chunk_list->chunk_length;

            giet_printf("****** NEW BLOCK ******\n"
                    "@ : %x\n"
                    "Length : %d o\n"
                    "***********************\n",
                    (unsigned int) ptr_to_chunk_list,
                    (unsigned int) (ptr_to_chunk_list->chunk_length));
            prev_block_base = prev;
            block_merged = 1;
        } // end [......|block|ptr|.....]

        prev = ptr_to_chunk_list;
        ptr_to_chunk_list = ptr_to_chunk_list->next;
    } // end while

    if (block_merged == 1) {
        return;
    }

    /* if the block cant be merge
     *  create new block and add it to the end of the list 
     */
    ((heap_ll *) block_base)->chunk_length = block_length;
    ((heap_ll *) block_base)->next = 0x0;
    prev->next = ((heap_ll *) block_base);


    giet_printf("****** NEW BLOCK ******\n"
            "@ : %x\n"
            "Length : %d o\n"
            "***********************\n",
            block_base,
            (unsigned int) (((heap_ll *) block_base)->chunk_length));
    return;
}



/*########################################################################*/
/**************************************************************************/
/**********                 END NEXT-FIT                        ***********/
/**************************************************************************/
/*########################################################################*/

#else // Default case : New Fit

/*########################################################################*/
/**************************************************************************/
/**********                    CLOSE-FIT                        ***********/
/**************************************************************************/
/*########################################################################*/


int get_prev_fit_chunk(unsigned int size) {
    int task_id = giet_global_task_id();
    int check_remote_free_list = 0;
    int index;
after_remote_list_check_c : //c for close fit
    index = GET_TAB_INDEX(size);
    while (index != MAX_SIZE_POW2_TAB) {
        if (_pow2tab[task_id][index] != 0x0) {
            return index;
        }
        index++;
    }

    if(check_remote_free_list == 0)
    {
        heap_ll * poped_block;
        heap_ll * head_of_rf;
        check_remote_free_list = 1;

        head_of_rf = pop_ptr();

        while((poped_block = pop_remote_free_list(&head_of_rf)) != 0x0)
        {
            update_chunk_list((unsigned int)poped_block, poped_block->chunk_length);
        }
        goto after_remote_list_check_c; 
    }

    return -1;
}



void * malloc(unsigned int size) {
    int task_id = giet_global_task_id();

    giet_printf("############ MALLOC ############\n\n");
    if(size == 0) {

        giet_printf(" SIZE = 0 \n");
        return 0x0;
    }
    if (size > 0x20000000) {

        giet_printf("ERROR max size = %d\n",0x20000000); 
        return 0x0;
    }
    if (size < 8) {
        size = 8;
    }

    /****** First call to malloc ******/
    if (_first_malloc[task_id] == 0) {
        giet_heap_info(&_heap_base[task_id], &_heap_length[task_id]);

        if (_heap_length[task_id] == 0) {
            giet_printf("*** Error: Malloc called in task %d while no heap was defined.\n", task_id);
            return 0x0;
        }

        int index = GET_TAB_INDEX(_heap_length[task_id]);

        _pow2tab[task_id][index] = (heap_ll *) _heap_base[task_id];
        _pow2tab[task_id][index]->chunk_length = _heap_length[task_id];
        _pow2tab[task_id][index]->next = 0x0;

        *((unsigned int *) ((_heap_base[task_id] +_heap_length[task_id]) - 4))     = _heap_length[task_id];

        _first_malloc[task_id] = 1;
    }

    /****** Size align ******/
    int size_align = size;
    if (size % 4 != 0) {
        size_align = ((size / 4) + 1) * 4;
    }


    giet_printf("Looking for size : %d %d = %d\n", size_align, 4,size_align+4);


    /****** Get tab index of the victim from the chunks list ******/
    heap_ll * victim;
    int index;
    if ((index = get_prev_fit_chunk(size_align + 4)) == -1) {
        return 0x0;
    }
    victim = _pow2tab[task_id][index];

    /****** Get Victim Base ******/
    unsigned int victim_vbase = (unsigned int) victim;	// overhead


    /****** update the chunks list ******/
    _pow2tab[task_id][index] = _pow2tab[task_id][index]->next;

    if ((victim->chunk_length - (size_align + 4)) < 12) {
        size_align = size_align + (victim->chunk_length - (size_align + 4)); 
    } 
    else {
        // if its not an exact match then update chunk base and length
        unsigned int new_size = victim->chunk_length - (size_align + 4); // get the new size;
        unsigned int new_index = GET_TAB_INDEX(new_size);                 // get the new index;
        heap_ll * new_chunk = (heap_ll *) (((unsigned int) victim) + (size_align + 4)); //update new_chunk @
        new_chunk->chunk_length = new_size;                                // write new size
        *((unsigned int *) ((unsigned int) new_chunk + new_size - 4)) = new_size;                                // on both side of the block
        new_chunk->next = _pow2tab[task_id][new_index];                     // insert new_chunk @ the new index
        _pow2tab[task_id][new_index] = new_chunk;


        giet_printf("NEXT CHUNK is at @ : 0x%x + 0x%x = 0x%x : @ of size over the block : %x\n",
                (unsigned int) victim,
                (unsigned int) ((size_align + 4) / 4),
                (unsigned int) new_chunk,
                ((unsigned int) new_chunk + new_chunk->chunk_length) - 4);
    }

    /****** Write Overhead ******/
    *((unsigned int *) victim_vbase) = size_align;	// writing overhead on the first word

    giet_printf("BLOCK SUCCESSFULLY ALLOCATED at @ 0x%x user will write at @ 0x%x\n\n",\
            (unsigned int)(victim_vbase), (unsigned int ) (victim_vbase + 4));

    return (unsigned int *) (victim_vbase + 4);
}



/* this function tries to merge the block to free with chunks in the list if they are contiguous
 * the block can be merged two times.
 */
void update_chunk_list(unsigned int block_base, unsigned int block_length) {
    int task_id = giet_global_task_id();

    int left_merging               = 0;
    unsigned int left_block_size   = 0;
    unsigned int left_block_index  = 0;
    heap_ll * left_block_addr      = 0x0;

    int right_merging              = 0;
    unsigned int right_block_size  = 0; 
    unsigned int right_block_index = 0;
    heap_ll * right_block_addr     = 0x0;

    if (block_base - 4 >= _heap_base[task_id]) {


        giet_printf("############# LEFT MERGE #############\n\n");
        left_block_size = *((unsigned int *) (block_base - 4));
        left_block_index = GET_TAB_INDEX(left_block_size);
        left_block_addr = (heap_ll *)(block_base - left_block_size);
        left_merging = 1;
    }

    if ((block_base + block_length) <= (_heap_base[task_id] + _heap_length[task_id]) - 8) {

        giet_printf("############# RIGHT MERGE #############\n\n");
        right_block_size = ((heap_ll *)(block_base + block_length))->chunk_length;
        right_block_index = GET_TAB_INDEX(right_block_size);
        right_block_addr = (heap_ll *) (block_base + block_length);
        right_merging = 1;
    }


    giet_printf("############# END PREP MERGE #############\n\n");

    heap_ll * ptr_to_chunk = 0x0;
    heap_ll * prev_ptr_to_chunk = 0x0;

    heap_ll * ptr_to_chunk_right = 0x0;
    heap_ll * prev_ptr_to_chunk_right = 0x0;

    unsigned int new_size = block_length;
    unsigned int new_index = 0;
    int merged_left = 0;
    heap_ll * new_addr = 0x0;



    /* try to merge with left block */
    if (left_merging == 1 &&
            (unsigned int) left_block_addr >= _heap_base[task_id] &&
            (unsigned int) left_block_addr <= block_base - 12) {
        ptr_to_chunk = _pow2tab[task_id][left_block_index];
        while (ptr_to_chunk != 0x0) {
            if (ptr_to_chunk == left_block_addr && ptr_to_chunk->chunk_length == left_block_size) {
                new_size = new_size + left_block_size;
                if (prev_ptr_to_chunk == 0x0) {
                    _pow2tab[task_id][left_block_index] = ptr_to_chunk->next;
                }
                else {
                    prev_ptr_to_chunk->next = ptr_to_chunk->next;
                }
                merged_left = 1;
                break;
            }
            else if (ptr_to_chunk == right_block_addr && ptr_to_chunk->chunk_length == right_block_size) {
                ptr_to_chunk_right = ptr_to_chunk;
                prev_ptr_to_chunk_right = prev_ptr_to_chunk;
            }
            prev_ptr_to_chunk = ptr_to_chunk;
            ptr_to_chunk = ptr_to_chunk->next;
        }
    }

    if (right_block_index != left_block_index) {
        prev_ptr_to_chunk = 0x0;
        ptr_to_chunk = _pow2tab[task_id][right_block_index];
    }
    else {
        ptr_to_chunk = ptr_to_chunk_right;
        prev_ptr_to_chunk = prev_ptr_to_chunk_right;
    }

    /* try to merge with right block */
    if (right_merging == 1 &&
            (unsigned int) right_block_addr >= block_base + block_length &&
            (unsigned int) right_block_addr <= (_heap_base[task_id] + _heap_length[task_id]) - 12) {
        while (ptr_to_chunk != 0x0) {
            if (ptr_to_chunk == right_block_addr && ptr_to_chunk->chunk_length == right_block_size) {
                new_size = new_size + right_block_size;
                if (prev_ptr_to_chunk == 0x0) {
                    _pow2tab[task_id][right_block_index] = ptr_to_chunk->next;
                }
                else {
                    prev_ptr_to_chunk->next = ptr_to_chunk->next;
                }
                break;
            }
            prev_ptr_to_chunk = ptr_to_chunk;
            ptr_to_chunk = ptr_to_chunk->next;
        }
    }
    new_index = GET_TAB_INDEX(new_size);
    if (merged_left == 1) {
        new_addr = (heap_ll *) (block_base - left_block_size);
    }
    else {
        new_addr = (heap_ll *) block_base;
    }
    new_addr->chunk_length = new_size;
    *((unsigned int *) ((unsigned int) new_addr + (new_addr->chunk_length - 4))) = new_size;
    new_addr->next = _pow2tab[task_id][new_index];
    _pow2tab[task_id][new_index] = new_addr;

    giet_printf("########################  UPDT SUCCESS ######################\n");

    return;
}




/*########################################################################*/
/**************************************************************************/
/**********                 END CLOSE-FIT                       ***********/
/**************************************************************************/
/*########################################################################*/

#endif // New Fit

heap_ll * pop_remote_free_list(heap_ll ** head)
{

    heap_ll * poped_block;

    if (*head == 0x0)
    {
        giet_printf(" no block to pop\n");
        return 0x0;
    }
    else
    {
        poped_block = *head;
        *head = (*head)->next;

        giet_printf(" : @%x, size : %x\n", poped_block, poped_block->chunk_length);
    }

    return poped_block;
}

heap_ll * pop_ptr(void)
{
    int task_id = giet_global_task_id();
    heap_ll * head_of_rf_list;
    giet_printf(" -- POP lock acquire -- for %d : @ : %x\n", task_id, &lock_rf_list[task_id]);
    lock_acquire(&lock_rf_list[task_id]);
    giet_printf(" -- POP lock acquired --\n");

    head_of_rf_list = _remote_free_list[task_id];
    _remote_free_list[task_id] = 0x0;

    lock_release(&lock_rf_list[task_id]);
    return head_of_rf_list;

}

void insert_in_remote_free_list(unsigned int remote_owner_id, unsigned int block_base, unsigned int block_length)
{

    giet_printf("############### INSERT \n");
    ((heap_ll *)block_base)->chunk_length = block_length;

    giet_printf(" -- INSERT lock acquire -- for %d : @ : %x\n", remote_owner_id, &lock_rf_list[remote_owner_id]);
    lock_acquire(&lock_rf_list[remote_owner_id]);

    giet_printf(" -- INSERT lock acquired -- \n");

    // insertion en début de liste
    giet_printf("  in remote_owner %d ...\n", remote_owner_id);
    if (_remote_free_list[remote_owner_id] == 0x0)
    {
        ((heap_ll *)block_base)->next = 0x0;
    } 
    else
    {
        ((heap_ll *)block_base)->next = _remote_free_list[remote_owner_id];
    }
    _remote_free_list[remote_owner_id] = ((heap_ll *)block_base);

    giet_printf("  : OK\n\n");
    giet_printf(" new head : %x : of size : %x, with next : %x\n", _remote_free_list[remote_owner_id], _remote_free_list[remote_owner_id]->chunk_length, _remote_free_list[remote_owner_id]->next);

    lock_release(&lock_rf_list[remote_owner_id]);

    return;
}

void free(void * ptr) {
    int i = 0;
    while (i != NB_TASKS_MAX)
    {
        if( _heap_base[i] == -1)
        {
            continue;
        }
        if ((unsigned int)ptr > _heap_base[i] && (unsigned int)ptr < _heap_base[i] + _heap_length[i])
        {
            break;
        }
        i++;
    }

    int task_id = giet_global_task_id();
    unsigned int * to_free = ptr;
    unsigned int heapB;
    unsigned int heapL;
    unsigned int overhead;
    unsigned int block_base;
    unsigned int block_length;
    unsigned int remote_free_needed;
    unsigned int remote_owner_id;
    if (i == task_id)
    {

        giet_printf("############# FREE  #############\n\n");
        remote_free_needed = 0;
        remote_owner_id = -1;
        heapB = _heap_base[task_id];
        heapL = _heap_length[task_id];
    }
    else
    {

        giet_printf("############# REMOTE FREE  #############\n\n");
        remote_free_needed = 1;
        remote_owner_id = i;
        heapB = _heap_base[remote_owner_id];
        heapL = _heap_length[remote_owner_id];
    }


    /****** CHECK PTR ******/
    if (to_free == 0x0) {

        giet_printf("free == 0x0\n");
        return;
    }
    // check alignement of ptr
    if (((unsigned int) to_free) % 4 != 0) {

        giet_printf("allignement of ptr  %x = %d\n", (unsigned int) to_free, (unsigned int) to_free % 4);
        return;
    }
    // check if the @ of ptr matches the range of the heap
    if (!((unsigned int) to_free >= heapB && (unsigned int) to_free < heapB + heapL)) {

        giet_printf("check if it matches the range of the heap\n");
        return;
    }

    // get the 4 bytes before the ptr that contains the size of the block 
    overhead = *(to_free - 1);
    // check alignement of overhead
    if (overhead % 4 != 0) {

        giet_printf("check alignement of overhead\n");
        return;
    }
    // check if (overhead + to_free) matches the range of the heap
    if ((((unsigned int) (to_free) + overhead) - 4) > heapB + heapL) {

        giet_printf ("check if (overhead + to_free) matches the range of the heap\n");
        return;
    }

    block_base = (unsigned int) (to_free - 1);
    block_length = overhead + 4;

    if (remote_free_needed == 0)
    {

        giet_printf("############# UPDATE CL :  FREE %x of size %d #############\n\n",block_base, block_length);
        update_chunk_list(block_base, block_length);
    }
    else
    {

        giet_printf("############# INSERT IN RF %d :  FREE %x of size %d #############\n\n", remote_owner_id, block_base, block_length);
        insert_in_remote_free_list(remote_owner_id, block_base, block_length);
    }


    giet_printf("BLOCK SUCCESSFULLY FREED\n\n");

    return;
}






// Local Variables:
// tab-width: 4
// c-basic-offset: 4
// c-file-offsets:((innamespace . 0)(inline-open . 0))
// indent-tabs-mode: nil
// End:
// vim: filetype=c:expandtab:shiftwidth=4:tabstop=4:softtabstop=4