#include "wait_queue.h"
#include "os/os_task.h"
#include "libs/utils.h"
#include "bsp/bsp_driver.h"

void wait_queue_init(wait_queue_t *queue, int max_size){
	queue->keys = os_alloc(max_size * sizeof(response_t));
	memset(queue->keys, 0, sizeof(max_size * sizeof(response_t)));
	queue->max_key_count = max_size;
}


s32 wait_queue_add_buffer(wait_queue_t *queue, u16 key, u8*buffer, u16 len) {
	s32 q_index = -1;
	for(int i = 0; i < queue->max_key_count; i++){
		if (key == queue->keys[i].key){
			q_index = -1;
			break;
		}
		if ((q_index == -1) && (queue->keys[i].key== 0)){
			q_index = i;
		}
	}
	if (q_index != -1) {
		queue->keys[q_index].key = key;
		queue->keys[q_index].data_len= len;
		queue->keys[q_index].resp_data= buffer;
	}
	return q_index;
}


s32 wait_queue_add(wait_queue_t *queue, u16 key) {
	return wait_queue_add_buffer(queue, key, NULL, 0);
}

bool wait_queue_key_acked_response(wait_queue_t *queue, u16 key, u8 *data, u16 len){
	bool stop_to_next = false;
	for(int i = 0; i < queue->max_key_count; i++){
		if (key == queue->keys[i].key){
			queue->keys[i].key = ACKED_KEY;
			if (queue->keys[i].resp_data != NULL && data != NULL) {
				memcpy(queue->keys[i].resp_data, data, min(queue->keys[i].data_len, len));
				queue->keys[i].data_len = len;
				stop_to_next = true;
			}else {
				queue->keys[i].data_len = 0;
			}
			break;
		}
	}
	return stop_to_next;
}


bool wait_queue_key_acked(wait_queue_t *queue, u16 key){
	return wait_queue_key_acked_response(queue, key, NULL, 0);
}

s32 wait_queue_wait_key(wait_queue_t *queue, s32 index, s32 timeout) {
	volatile response_t *resp_key = queue->keys + index;
	while((resp_key->key != ACKED_KEY) && (timeout-- >= 0)){
		can_rx_poll();
		delay_ms(1);
	}
	if (resp_key->key == ACKED_KEY){
		return resp_key->data_len;
	}
	return -1;
}

void wait_queue_delete_key(wait_queue_t *queue, s32 index) {
	queue->keys[index].key = 0;
	queue->keys[index].data_len = 0;
	queue->keys[index].resp_data = NULL;
}