#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>

#include "Clock.h"
#include "msp.h"

#include "ir.h"
#include "motor.h"
#include "state.h"
#include "util.h"

#define LOOP_PERIOD_MS 20
#define REC_INTERVAL 5
#define MAX_REC_SIZE 1000

// PID gains tuned for speed range [-20, 20]
const float Kp = 0.005f;   // proportional gain
const float Kd = 0.02f;    // derivative gain
#define BASE_SPEED 10       // nominal forward speed (0~20)
#define MAX_DRV 20          // absolute max speed

#define Practice1


#ifdef Practice1

static int pid_history[MAX_REC_SIZE];
static int history_idx = 0;

void LineTracerPrac1(void) {
    // ------------------------------------------------
    // 1. 초기화
    // ------------------------------------------------
    motor_init();
    ir_init();
    
    SensorBuffer sb;
    senbuf_init(&sb);

    int error = 0;
    int prev_error = 0;
    float pid_out = 0;
    int speed_l, speed_r;
    int loop_cnt = 0; // 다운샘플링용 카운터

    // ------------------------------------------------
    // 2. [PHASE 1] 출발 및 기록 (Forward & Record)
    // ------------------------------------------------
    while (1) {
        // A. 센서 값 읽기
        uint8_t raw_val;
        ir_read_ready();       // 적외선 발광 시작
        delay_ms_simple(1);    // 안정화 대기
        ir_read_value(&raw_val);
        ir_read_off();         // 절전

        // B. 버퍼에 저장 및 T자 감지
        senbuf_push(&sb, raw_val);
        
        // T자형 교차로(끝)를 만나면 루프 탈출
        if (is_crossroad_robust(&sb)) {
            motor_move_stop();
            delay_ms_simple(1000); // 1초 정지
            break;
        }

        // C. PID 제어 계산
        error = get_error(raw_val);
        pid_out = (Kp * error) + (Kd * (error - prev_error));
        prev_error = error;

        // D. 모터 구동
        // 보정값이 양수(우측 쏠림) -> 우회전 필요 -> 왼쪽 가속, 오른쪽 감속
        speed_l = BASE_SPEED + (int)pid_out;
        speed_r = BASE_SPEED - (int)pid_out;

        // 속도 범위 제한 (0 ~ 20) - motor.h의 MAX_SPEED 참조
        if (speed_l > MAX_SPEED) speed_l = MAX_SPEED;
        if (speed_l < 0) speed_l = 0;
        if (speed_r > MAX_SPEED) speed_r = MAX_SPEED;
        if (speed_r < 0) speed_r = 0;

        motor_move_custom(speed_l, speed_r);

        // E. 경로 기록 (Downsampling)
        loop_cnt++;
        if (loop_cnt >= REC_INTERVAL) {
            if (history_idx < MAX_REC_SIZE) {
                // 현재 적용된 PID 보정값을 저장
                pid_history[history_idx++] = (int)pid_out;
            }
            loop_cnt = 0;
        }

        delay_ms_simple(2); // 루프 주기
    }

    // ------------------------------------------------
    // 3. [PHASE 2] U턴 (180도 회전)
    // ------------------------------------------------
    // T자 라인을 벗어나기 위해 살짝 후진 (선택 사항)
    motor_move_backward(10);
    delay_ms_simple(200);
    
    // 제자리 회전 (Pivot Turn)
    // 왼쪽: 20, 오른쪽: -20 (motor_rotate_clockwise 함수 활용)
    motor_rotate_clockwise(15);
    
    // 회전 시간 제어: 라인을 찾을 때까지 돔
    // (처음 0.5초는 무조건 돌아서 기존 라인을 벗어남)
    delay_ms_simple(500); 
    
    while(1) {
        uint8_t raw_turn;
        ir_read_ready();
        ir_read_value(&raw_turn);
        ir_read_off();

        // 중앙 센서(00011000)가 감지되면 정지
        if (raw_turn & 0b00011000) {
            motor_move_stop();
            delay_ms_simple(500); // 안정화
            break;
        }
    }

    // ------------------------------------------------
    // 4. [PHASE 3] 되돌아가기 (Replay)
    // ------------------------------------------------
    // 기록된 인덱스의 끝부터 역순으로 재생
    loop_cnt = 0;

    while (history_idx >= 0) {
        // 타이밍 맞추기 (기록할 때와 같은 속도로 읽기)
        loop_cnt++;
        if (loop_cnt >= REC_INTERVAL) {
            history_idx--; // 인덱스 감소 (Stack Pop)
            loop_cnt = 0;
        }
        
        // 데이터가 끝났으면 종료
        if (history_idx < 0) break;

        // ★ 핵심 로직: 부호 반전
        // 갈 때: 왼쪽으로 5만큼 꺾음 (PID = +5)
        // 올 때: 로봇이 뒤집혔으므로 오른쪽으로 5만큼 꺾어야 함 (Replay PID = -5)
        int replay_pid =    pid_history[history_idx];

        speed_l = BASE_SPEED + replay_pid;
        speed_r = BASE_SPEED - replay_pid;

        // 속도 제한
        if (speed_l > MAX_SPEED) speed_l = MAX_SPEED;
        if (speed_l < 0) speed_l = 0;
        if (speed_r > MAX_SPEED) speed_r = MAX_SPEED;
        if (speed_r < 0) speed_r = 0;

        motor_move_custom(speed_l, speed_r);
        
        delay_ms_simple(2); // 갈 때와 동일한 루프 딜레이
    }

    // ------------------------------------------------
    // 5. 종료
    // ------------------------------------------------
    motor_move_stop();
    while(1); // 무한 대기
}

#endif

#ifdef Practice2
void LineTracerPrac(void) {
    Clock_Init48MHz();
    printf("Init Phase Begin\n");

    ir_init();
    motor_init();

    SensorBuffer sb;
    senbuf_init(&sb);

    printf("Init Phase End\n");

    uint8_t ir_value;

    int error = 888481557;
    int prev_error = 0;

    int output_pid = 0;

    int speed_left;
    int speed_right;

    while (1) {
        ir_read_ready();
        ir_read_value(ir_value);
        ir_read_off();
        int i, j;

        senbuf_push(&sb, ir_value);
        if (is_crossroad_robust(&sb)) {
            // 교차로 처리 로직
            motor_move_stop();
            while(1) {} // succ
        }



        error = get_error(ir_value);
        output_pid = (int)(Kp * error + Kd * (error - prev_error));
        prev_error = error;


        // Differential drive speeds based on PID correction
        // Left slows down when error positive (line to right), right speeds up
        speed_left  = BASE_SPEED - output_pid;
        speed_right = BASE_SPEED + output_pid;

        // Clamp to allowed range [-20, 20]
        if (speed_left  >  MAX_DRV) speed_left  =  MAX_DRV;
        if (speed_left  < -MAX_DRV) speed_left  = -MAX_DRV;
        if (speed_right >  MAX_DRV) speed_right =  MAX_DRV;
        if (speed_right < -MAX_DRV) speed_right = -MAX_DRV;

        // Move motors with custom differential speeds
        motor_move_custom(speed_left, speed_right);

        Clock_Delay1ms(LOOP_PERIOD_MS);

    }
}

#endif

int main(void) {
    LineTracerPrac();
}