#include <stdio.h>

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

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

#define LOOP_PERIOD_MS 20

const float Kp = 0.8f;
const float Kd = 4.5f;

void main(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(check_crossroad_robust(&sb)) {
            // 교차로 처리 로직
            move_stop();
            while(1) {} // succ
        }



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


        // clamp;

        // motor driverev_error = error;

        Clock_Delay1ms(LOOP_PERIOD_MS);

        
    }
}