#include "block_detect.h"

#include "bluetooth.h"
#include "buzzer.h"
#include "hcsr04.h"
#include "infrared.h"
#include "syscalls.h"
#include "timer.h"

int has_block_front;
int has_block_left;
int has_block_right;
static uint8_t timer_event_id = -1;
static uint8_t timer_event_id2 = -1;

void INFRARED_Detect();
void Sonar_Detect();

// 避障检测初始化
void BlockDetect_Init()
{
    timer_event_id = TIMER_AddInfiniteLoopEvent(timer_event_id, INFRARED_Detect, 20);
    timer_event_id2 = TIMER_AddInfiniteLoopEvent(timer_event_id2, Sonar_Detect, 150);
}

// 每20ms红外检测一次障碍物
void INFRARED_Detect()
{
    has_block_left = INFRARED_GetL() == INFRARED_MEET;
    has_block_right = INFRARED_GetR() == INFRARED_MEET;
}

// 每100ms超声波检测一次障碍物
void Sonar_Detect()
{
    if (sonor_state == 0) sonar_mm();
}

// 超声波测距完毕回调
void Sonar_CP_Callback()
{
    // BLUETOOTH_Send("f: %d, l: %d, r: %d\n sonar distance = %d (mm)\n", has_block_front, has_block_left, has_block_right, sonor_distance);
    has_block_front = sonor_distance < 200;
}

int Has_Block(BlockDirection direction)
{
    switch (direction)
    {
        case FRONT:
            BUZZER_StartTimed(10);
            return has_block_front;
            break;
        case LEFT_FRONT:
            BUZZER_StartTimed(2);
            return has_block_left;
            break;
        case RIGHT_FRONT:
            BUZZER_StartTimed(2);
            return has_block_right;
            break;
    }
    return 0;
}