#include "util.h"

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

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

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

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

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

    // [Step 1] Gap Filling (구멍 메우기)
    // 센서 불량이나 바닥 상태로 인해 라인 중간이 끊겨 보이는 경우(101)를 보정
    // 예: 01011000 -> 01111000 (중간의 0을 1로 채움)
    uint8_t filled_raw = raw;
    int k;
    for (k = 1; k < 7; k++) {
        // 현재 비트가 0이고, 좌우 비트가 모두 1인 경우
        if ( !((raw >> k) & 1) && ((raw >> (k+1)) & 1) && ((raw >> (k-1)) & 1) ) {
            filled_raw |= (1 << k);
        }
    }

    // [Step 2] Largest Segment Selection (가장 큰 덩어리 선택)
    // 보정된 데이터(filled_raw)를 기준으로 가장 큰 덩어리만 남김
    
    uint8_t filtered_raw = 0;
    int max_consecutive = -1;
    int best_mask = 0;
    
    int current_consecutive = 0;
    int current_mask = 0;
    int i;

    for (i = 0; i < 8; i++) {
        // 7번 비트(왼쪽)부터 0번 비트(오른쪽) 순으로 검사
        if ((filled_raw >> (7 - i)) & 1) {
            current_consecutive++;
            current_mask |= (1 << (7 - i));
        } else {
            // 덩어리가 끊긴 경우
            if (current_consecutive > 0) {
                if (current_consecutive > max_consecutive) {
                    max_consecutive = current_consecutive;
                    best_mask = current_mask;
                } else if (current_consecutive == max_consecutive) {
                    // 덩어리 크기가 같다면, 이전 진행 방향(last_valid_error)과 가까운 쪽 선택
                    if (last_valid_error > 0) best_mask = current_mask;
                }
                current_consecutive = 0;
                current_mask = 0;
            }
        }
    }
    
    // 마지막 비트까지 1이었던 경우 처리
    if (current_consecutive > 0) {
        if (current_consecutive > max_consecutive) {
            best_mask = current_mask;
        } else if (current_consecutive == max_consecutive) {
            if (last_valid_error > 0) best_mask = current_mask;
        }
    }

    filtered_raw = best_mask;
    if (filtered_raw == 0x00) filtered_raw = filled_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(HistoryBuffer *sb) {
    int crossroad_count = 0;
    int total_count = sb->size;
    int i;
    if (total_count < 3) {
        return 0; // 기록이 충분하지 않음
    }
    for (i = 0; i < 3; i++) {
        HistoryEntry entry;
        hisbuf_get(sb, &entry, i);
        if (!is_crossroad(entry.sensor)) {
            goto fail;
        }
    }
    return 1;
fail:
    return 0;
}

void print_binary(const uint8_t n) {
    int i;
    for (i = 7; i >= 0; i--) printf("%d", (n >> i) & 0b1);
}

uint8_t extract_ir_value(const uint8_t mask, const uint8_t ir_value) {
    return mask & ir_value;
}