#define TOS_MAX_TASK 32 // 最大任务数（优先级数）

// 任务控制块（TCB）
struct tos_tcb
{
    unsigned int *stack_top_point; // 任务栈顶指针
    unsigned int delay; // 任务延时时钟
};
typedef struct tos_tcb tos_tcb_t;

tos_tcb_t tos_tcb_list[TOS_MAX_TASK]; // 任务控制块列表

/**
 * 刚开始的时候，每个任务都还没有自己的栈数据，所以我们就先手工写出一些数据。
 * 其中LR寄存器时每个子函数返回时的寄存器，由于我们任务里是死循环，不会返回，所以这个地方随便写。
 */
static void __task_end(void)
{
    while (1);
}

unsigned char tos_prio_current; // 记录当前运行的任务优先级
unsigned char tos_prio_high_ready; // 
volatile unsigned int tos_ready_table; // 任务就绪表

// 设置优先级
inline void tos_set_prio_ready(unsigned char _prio)
{
    tos_ready_table |= 0x01 << _prio;
}

// 从任务就绪表中删除
inline void tos_del_prio_ready(unsigned char _prio)
{
    tos_ready_table &= ~(0x01 << _prio);
}

// 寻找最高优先级
inline void tos_get_high_ready(void)
{
    unsigned char next_prio;
    for (next_prio = 0; (next_prio < TOS_MAX_TASK) && (!(tos_ready_table&(0x01 << next_prio))); next_prio++)
    {
        tos_prio_high_ready = next_prio;
    }
}

// 任务创建
void tos_task_create(void (*_task)(void), unsigned int *_stack, unsigned char _prio)
{
    unsigned int *p_stack;

    p_stack = _stack;
    p_stack = (unsigned int *)((unsigned int)(p_stack)&0xFFFFFFF8U);
    // 以下寄存器顺序和PendSV退出时寄存器恢复顺序一致
    *(--p_stack) = (unsigned int)0x01000000UL; // xPSR状态寄存器，第24位THUMB模式必须置位1
    *(--p_stack) = (unsigned int)_task; // entry point 任务函数入口
    *(--p_stack) = (unsigned int)__task_end; // R14(LR)
    *(--p_stack) = (unsigned int)0x12121212UL; // R12
    *(--p_stack) = (unsigned int)0x03030303UL; // R3
    *(--p_stack) = (unsigned int)0x02020202UL; // R2
    *(--p_stack) = (unsigned int)0x01010101UL; // R1
    *(--p_stack) = (unsigned int)0x00000000UL; // R0
    // PendSV发生时未自动保存的内核寄存器：R4~R11
    *(--p_stack) = (unsigned int)0x11111111UL; // R11
    *(--p_stack) = (unsigned int)0x10101010UL; // R10
    *(--p_stack) = (unsigned int)0x09090909UL; // R9
    *(--p_stack) = (unsigned int)0x08080808UL; // R8
    *(--p_stack) = (unsigned int)0x07070707UL; // R7
    *(--p_stack) = (unsigned int)0x06060606UL; // R6
    *(--p_stack) = (unsigned int)0x05050505UL; // R5
    *(--p_stack) = (unsigned int)0x04040404UL; // R4
    tos_tcb_list[_prio].stack_top_point = p_stack; // 将该任务控制块中应当指向栈顶的指针，指向了该任务的新栈顶
    tos_tcb_list[_prio].delay = 0;
    tos_set_prio_ready(_prio);
}

#define TOS_EXCEPT_STACK_SIZE 256
#define TOS_IDLE_STACK_SIZE 256
unsigned int tos_cpu_except_stack[TOS_EXCEPT_STACK_SIZE]; // 主任务堆栈
unsigned int *tos_cpu_except_stack_base; // 指向的是数组最后一个元素
unsigned int tos_idle_stack[TOS_IDLE_STACK_SIZE]; // 空闲任务堆栈
tos_tcb_t *p_tcb_current; // 指向当前任务的tcb
tos_tcb_t *p_tcb_high_ready; // 指向最高级任务的tcb

/**
 * 
 *
 */
static void __idle_task(void)
{
    unsigned int idle_count = 0;
    while (1)
    {
        idle_count++;
    }
}

/**
 * 这里面，会初始化系统时钟，设置主堆栈，同时创建一个空闲任务，为防止某一时刻都没有任务为就绪态时，PC运行这个空闲任务
 *
 */
void tos_start(void)
{
    system_init();
    tos_cpu_except_stack_base = tos_cpu_except_stack + TOS_EXCEPT_STACK_SIZE - 1; // cortex-m3栈向下增长
    tos_task_create(__idle_task, &tos_idle_stack[TOS_IDLE_STACK_SIZE - 1], TOS_MAX_TASK - 1); // 空闲任务
    tos_get_high_ready(); // 获取最高级的就绪任务
    tos_prio_current = tos_prio_high_ready;
    p_tcb_high_ready = &tos_tcb_list[tos_prio_high_ready];
    tos_start_high_ready(); // 
}

void tos_schedule(void)
{
    tos_get_high_ready();
    if (tos_prio_high_ready != tos_prio_current)
    {
        p_tcb_high_ready = &tos_tcb_list[tos_prio_high_ready];
        tos_prio_current = tos_prio_high_ready;
        tos_context_switch();
    }
}

void system_init(void)
{
    unsigned int reload;
    SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8);
    reload = SystemCoreClock / 8000000;
    reload *= 1000000 / System_Ticks;

    Systick->CTRL |= SysTick_CTRL_TICKINT_Msk;
    SysTick->LOAD = reload;
    SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk;
}

void SysTick_Handler(void)
{
    unsigned char i = 0;

    for (; i < TOS_MAX_TASK; i++)
    {
        if (tos_tcb_list[i].delay)
        {
            tos_tcb_list[i].delay -= 1000 / System_Ticks;
            if (tos_tcb_list[i].delay == 0)
            {
                tos_set_prio_ready(i);
            }
        }
    }

    tos_schedule();
}

void tos_time_delay(unsigned int _ticks)
{
    if (_ticks)
    {
        tos_del_prio_ready(tos_prio_current);
        tos_tcb_list[tos_prio_current].delay = _ticks;
        tos_schedule();
    }
}