#include "util.h"

#include <math.h>
#include <stdint.h>

// 가중치 설정 (-4000 ~ +4000)
// 중앙(3번과 4번 사이)이 0이 되도록 설정
// [0] [1] [2] [3] | [4] [5] [6] [7]
const int SENSOR_WEIGHTS[8] = {-4000, -3000, -2000, -1000, 1000, 2000, 3000, 4000};

// 선을 완전히 벗어났을 때 반환할 최대 에러 값
const int MAX_ERROR = 5000;

int get_error(uint8_t raw) {
    // static 변수: 함수가 끝나도 값이 사라지지 않고 기억됨 (로봇의 마지막 위치)
    static int last_valid_error = 0;

    // 1. 선이 아예 안 보이는 경우 (Line Lost)
    if (raw == 0x00) {
        // 마지막에 왼쪽(-값)에 있었다면 -> 계속 왼쪽으로 돌아라 (-MAX_ERROR)
        if (last_valid_error < 0) return -MAX_ERROR;
        // 마지막에 오른쪽(+값)에 있었다면 -> 계속 오른쪽으로 돌아라 (+MAX_ERROR)
        else
            return MAX_ERROR;
    }

    // 2. 노이즈/갈림길 처리 (Masking) - 사용자가 걱정했던 '1 0 0 0 1 1 0 0' 상황
    // 간단한 로직: 마지막 위치가 양수(오른쪽)였다면, 왼쪽 끝(0,1번) 센서는 노이즈일 확률이 높음 -> 무시
    // 반대의 경우도 마찬가지.

    uint8_t filtered_raw = raw;

    // 로봇이 확실히 오른쪽(>1000)에 있었는데, 갑자기 왼쪽 끝(0,1번 비트)이 켜짐?
    if (last_valid_error > 1000 && (raw & 0b00000011)) {
        filtered_raw &= ~0b00000011;// 0, 1번 비트 강제 삭제 (Masking)
    }
    // 로봇이 확실히 왼쪽(<-1000)에 있었는데, 갑자기 오른쪽 끝(6,7번 비트)이 켜짐?
    else if (last_valid_error < -1000 && (raw & 0b11000000)) {
        filtered_raw &= ~0b11000000;// 6, 7번 비트 강제 삭제
    }

    // 만약 필터링했더니 남는 게 없다면? (노이즈만 떴던 경우) -> 원본 사용 or 이전 값 리턴
    if (filtered_raw == 0x00) filtered_raw = raw;

    // 3. 가중 평균 계산 (Weighted Average)
    long sum_weighted = 0;
    int sum_active = 0;
    int i;
    for (i = 0; i < 8; i++) {
        // i번째 비트가 1인지 확인
        if ((filtered_raw >> (7 - i)) & 1) {
            sum_weighted += SENSOR_WEIGHTS[i];
            sum_active++;
        }
    }

    // 예외 처리: 계산 중 비트가 다 사라진 경우 (혹시 모를 상황)
    if (sum_active == 0) return last_valid_error;

    // 최종 에러 값 계산
    int current_error = sum_weighted / sum_active;

    // 4. 다음 계산을 위해 현재 위치 기억
    last_valid_error = current_error;

    return current_error;
}

// 센서 비트 중 1의 개수를 세서 T자/십자 여부 판단
int is_crossroad(uint8_t raw) {
    int count = 0;
    int i;
    for (i = 0; i < 8; i++) {
        if ((raw >> i) & 1) {// i번째 비트가 1이면 카운트
            count++;
        }
    }
    // 8개 중 6개 이상이 검은색이면 교차로(T자)로 인정
    if (count >= 7) return 1;
    else
        return 0;
}

int is_crossroad_robust(SensorBuffer *sb) {
    int crossroad_count = 0;
    int total_count = sb->size;
    int i;
    for (i = 0; i < 3; i++) {
        uint8_t sensor_value;
        senbuf_get(sb, &sensor_value, i);
        if (!is_crossroad(sensor_value)) {
            goto fail;
        }
    }
    return 1;
fail:
    return 0;
}