#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"

enum State {
    STATE_ALIGNING,
    STATE_ALIGNED,
    STATE_SHOOTING,
    STATE_STOP,
};



// 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
#define MAX_DRV 3000

#define PERIOD_MS 20

enum State current_state = STATE_ALIGNING;

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

    // 초기 안정화 대기
    Clock_Delay1ms(500);

    int align_phase = 0; // 0: 출발 라인 벗어나기, 1: 목표 라인 찾기

    while(1) {
        switch(current_state) {
            case STATE_ALIGNING:
                {
                    // 1. 센서 값 읽기
                    uint8_t raw_val;
                    ir_read_ready();
                    Clock_Delay1us(1000);
                    ir_read_value(&raw_val);
                    ir_read_off();

                    // 2. 에러 계산
                    int error = get_error(raw_val);
                    
                    // 3. 정렬 완료 조건 확인 (에러가 거의 0인 상태 유지)
                    // util.c의 가중치: {-14, ..., 14}
                    // error 0: 완전 중앙, error +/- 2: 약간 벗어남
                    static int aligned_count = 0;
                    if (abs(error) <= 2) { 
                        aligned_count++;
                    } else {
                        aligned_count = 0;
                    }

                    if (aligned_count > 25) { 
                        motor_stop(); // 잠시 대기
                        current_state = STATE_ALIGNED; // 다음 단계(직진/발사)로 이동
                        break;
                    }

                    // 4. 제자리 회전 (Pivot Turn)
                    // BASE_SPEED 없이 오차만큼만 회전하여 제자리에서 방향을 맞춤
                    float pid_out = Kp * error; 
                    
                    int speed_l = (int)pid_out;
                    int speed_r = -(int)pid_out;

                    // 최소 기동 토크 보정 (Deadzone Compensation)
                    // 모터가 돌기 시작하는 최소 PWM 값을 더해줌
                    int min_speed = 1000; 
                    if (speed_l > 0) speed_l += min_speed;
                    else if (speed_l < 0) speed_l -= min_speed;
                    
                    if (speed_r > 0) speed_r += min_speed;
                    else if (speed_r < 0) speed_r -= min_speed;

                    // 최대 속도 제한
                    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);
                    
                    Clock_Delay1ms(PERIOD_MS);
                }
                break;

            case STATE_ALIGNED:
                motor_stop();
                Clock_Delay1ms(500); // 잠시 대기
                current_state = STATE_SHOOTING;
                break;

            case STATE_SHOOTING:
                {
                    uint8_t raw_val;
                    ir_read_ready();
                    Clock_Delay1us(1000);
                    ir_read_value(&raw_val);
                    ir_read_off();

                    // 2. 직진 (PID 없이 등속 주행)
                    motor_move(BASE_SPEED, BASE_SPEED);

                    // 3. 왼쪽 라인(검은색) 감지 시 정지
                    // 출발 직후(오른쪽 라인)에는 감지될 수 있으므로, 
                    // 일단 흰색(0x00)이 된 후 다시 검은색이 나올 때 멈추는 로직 필요
                    static int left_line_phase = 0; 
                    
                    if (left_line_phase == 0) {
                        if (raw_val == 0x00) left_line_phase = 1; // 출발 라인 벗어남
                    } else if (left_line_phase == 1) {
                        if (raw_val != 0x00) { // 목표 라인 도착
                            motor_stop();
                            current_state = STATE_STOP;
                        }
                    }
                    Clock_Delay1ms(PERIOD_MS);
                }
                break;

            case STATE_STOP:
                motor_stop();
                break;
        }
    }
}