#include <stdlib.h>
#include "rpccommands.h"

void rpc_Ping(rpc_t *rpc)
{
    rpc_respond(rpc, NULL);
}

void rpc_FreeMemory(rpc_t *rpc)
{
    FreeMemoryResponse response = {};
    response.free_core = chCoreStatus();
    size_t heap;
    response.heap_fragments = chHeapStatus(NULL, &heap);
    response.free_heap = heap;
    rpc_respond(rpc, &response);
}

extern unsigned long __heap_end__; // From linker script
void rpc_ThreadStatus(rpc_t *rpc)
{
    ThreadStatusResponse *response = calloc(1, sizeof(ThreadStatusResponse));
    
    if (response == NULL)
    {
        rpc_resources_error(rpc, "Out of memory");
        return;
    }
    
    Thread *thread = chRegFirstThread();
    
    while (thread != NULL && response->thread_count < 16)
    {
        ThreadInfo *p = &response->thread[response->thread_count];
        response->thread_count++;
        
        p->id = (uint32_t)thread;
        p->priority = thread->p_prio;
        p->state = thread->p_state;
        p->current_sp = (uint32_t)thread->p_ctx.r13;
        
        uint32_t stack_bottom;
        if (p->current_sp > (uint32_t)&__heap_end__)
            stack_bottom = (uint32_t)&__heap_end__;
        else
            stack_bottom = (uint32_t)(thread + 1);
        
        p->free_stack_now = p->current_sp - stack_bottom;
        
        uint32_t *stackentry = (uint32_t*)stack_bottom;
        uint32_t empty_val = *stackentry;
        while (*stackentry == empty_val) stackentry++;
        p->has_free_stack_min = true;
        p->free_stack_min = (uint32_t)stackentry - stack_bottom;
        
#ifdef CH_DBG_THREADS_PROFILING
        p->has_consumed_time = true;
        p->consumed_time = thread->p_time;
#endif
        thread = chRegNextThread(thread);
    }
    rpc_respond(rpc, &response);
    free(response);
}