#include "block_detect.h"

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

#define MIN_BLOCK_TIME 500

int has_block_front;
int has_block_left;
int has_block_right;
int block_time;

void INFRARED_Detect();
void Sonar_Detect();

void Block_Time_Inc()
{
    block_time++;
}

// 避障检测初始化
void BlockDetect_Init()
{
    TIMER_AddLoopEvent(EVENT_INFRARED, INFRARED_Detect, 5);
    TIMER_AddLoopEvent(EVENT_SONAR, Sonar_Detect, 150);
    TIMER_AddLoopEvent(EVENT_BLOCK, Block_Time_Inc, 1);
}

// 每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);
            if (has_block_front)
            {
                if (block_time > MIN_BLOCK_TIME)
                {
                    block_time = 0;
                    return 1;
                }
            }
            break;
        case LEFT_FRONT:
            // BUZZER_StartTimed(2);           
             if (has_block_left)
            {
                if (block_time > MIN_BLOCK_TIME)
                {
                    block_time = 0;
                    return 1;
                }
            }
            break;
        case RIGHT_FRONT:
            // BUZZER_StartTimed(2);           
             if (has_block_right)
            {
                if (block_time > MIN_BLOCK_TIME)
                {
                    block_time = 0;
                    return 1;
                }
            }
            break;
    }
    return 0;
}