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

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

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

// PID Control Constants
// Error range: -14 ~ +14 (Max 20)
// PWM Period: 15000
const float Kp = 150.0f;// P gain: 150 * 14 = 2100 (Significant correction)
const float Kd = 600.0f;// D gain

#define BASE_SPEED 1500// Slower speed (approx 10% duty of 15000)
#define MAX_DRV 3000   // Max speed limit (20% duty)

void main() {
    Clock_Init48MHz();
    ir_init();
    motor_init();

    Clock_Delay1ms(1000);// 휴식
    motor_stop();

    int error = 0;
    int prev_error = 0;
    float pid_out = 0;
    int speed_l, speed_r;

    HistoryEntry entry;
    HistoryBuffer hisbuf;
    hisbuf_init(&hisbuf);

    int black_cnt = 0;

    int trace_mode = 0;// 0: Normal, 1: Traceback
    // 2. Main Loop
    while (1) {
        // A. Read Sensor
        uint8_t raw_val;
        ir_read_ready();
        Clock_Delay1us(1000);// Wait for IR LED
        ir_read_value(&raw_val);
        ir_read_off();

        // B. Line Lost Handling (White Area) -> Replay History from Beginning (FIFO)
        if (raw_val == 0x00 || trace_mode == 1) {
            trace_mode = 1;// Enter Traceback Mode
            motor_stop();
            Clock_Delay1ms(1000);// Wait a bit before replay
            
            if (raw_val != 0x00) {
                black_cnt ++;
            } else {
                black_cnt = 0;
            }
            if (black_cnt >= 3) {
                goto finish;
            }
            
            // Replay entire history from the beginning (FIFO)
            static int replay_prev_error = 0;

            if (hisbuf_pop_data(&hisbuf, &entry)) {
                // Reconstruct PID
                float rec_pid = (Kp * entry.error) + (Kd * (entry.error - replay_prev_error));
                replay_prev_error = entry.error;

                int rec_speed_l = BASE_SPEED + (int) rec_pid;
                int rec_speed_r = BASE_SPEED - (int) rec_pid;

                // Limit
                if (rec_speed_l > MAX_DRV) rec_speed_l = MAX_DRV;
                if (rec_speed_l < -MAX_DRV) rec_speed_l = -MAX_DRV;
                if (rec_speed_r > MAX_DRV) rec_speed_r = MAX_DRV;
                if (rec_speed_r < -MAX_DRV) rec_speed_r = -MAX_DRV;

                // Move Forward (Replay)
                motor_move(rec_speed_l, rec_speed_r);

                Clock_Delay1ms(2);
            }

        } else {
            if (is_crossroad_robust(&hisbuf)) {
                goto finish;
            }
            // C. Normal PID Control
            error = get_error(raw_val);
            pid_out = (Kp * error) + (Kd * (error - prev_error));
            prev_error = error;

            // D. Motor Control
            speed_l = BASE_SPEED + (int) pid_out;
            speed_r = BASE_SPEED - (int) pid_out;

            // E. Speed Limiting
            if (speed_l > MAX_DRV) speed_l = MAX_DRV;
            if (speed_l < -MAX_DRV) speed_l = -MAX_DRV;
            if (speed_r > MAX_DRV) speed_r = MAX_DRV;
            if (speed_r < -MAX_DRV) speed_r = -MAX_DRV;

            motor_move(speed_l, speed_r);

            // Save to History
            HistoryEntry entry = {raw_val, error};
            hisbuf_push(&hisbuf, entry);
        }

        // F. Loop Delay (Sampling Time)
        Clock_Delay1ms(2);
    }

finish:
    motor_stop();
    while(1);
}