#include <stdio.h>
#ifdef __MSP432P401R__
#else
#define __MSP432P401R__
#endif

//#define DEBUG_PRINT


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

#define LOOP_PERIOD_MS 20

#define BASE_SPEED_PERCENT 30
#define TURN_SPEED_PERCENT 25

#define BASE_ON_TIME (LOOP_PERIOD_MS * BASE_SPEED_PERCENT / 100)
#define BASE_OFF_TIME (LOOP_PERIOD_MS - BASE_ON_TIME)

#define TURN_ON_TIME (LOOP_PERIOD_MS * TURN_SPEED_PERCENT / 100)
#define TURN_OFF_TIME (LOOP_PERIOD_MS - TURN_ON_TIME)

// IR
void ir_init(void) {
    P5->SEL0 &= ~0x08;
    P5->SEL1 &= ~0x08;
    P5->DIR |= 0x08;
    P5->OUT &= ~0x08;
    P9->SEL0 &= ~0x04;
    P9->SEL1 &= ~0x04;
    P9->DIR |= 0x04;
    P9->OUT &= ~0x04;
    P7->SEL0 &= ~0xff;
    P7->SEL1 &= ~0xff;
    P7->DIR &= ~0xff;
}

// Motor
void motor_init(void) {
    P3->SEL0 &= ~0xc0;
    P3->SEL1 &= ~0xc0;
    P3->DIR |= 0xc0;
    P3->OUT &= ~0xc0;

    P5->SEL0 &= ~0x30;
    P5->SEL1 &= ~0x30;
    P5->DIR |= 0x30;
    P5->OUT &= ~0x30;

    P2->SEL0 &= ~0xc0;
    P2->SEL1 &= ~0xc0;
    P2->DIR |= 0xc0;
    P2->OUT &= ~0xc0;
}

void motor_move_forward() {
    P5->OUT &= ~0b00110000;
    P3->OUT |= 0b11000000;
    P2->OUT |= 0b11000000;
}

void motor_move_biases_left() {
    P5->OUT = (P5->OUT & ~0x30) | 0x10;
    P3->OUT |= 0b11000000;
    P2->OUT |= 0b11000000;
}

void motor_move_biases_right() {
    P5->OUT = (P5->OUT & ~0x30) | 0x20;
    P3->OUT |= 0b11000000;
    P2->OUT |= 0b11000000;
}

void motor_move_stop() {
    P2->OUT &= ~0xc0;
}

void ir_read_ready() {
    P5->OUT |= 0x08;
    P9->OUT |= 0x04;
    P7->DIR = 0xff;
    P7->OUT = 0xff;
    Clock_Delay1us(10);
    P7->DIR = 0x00;
    Clock_Delay1us(1000);
}

void ir_read_value(char *value) {
    int i;
    uint8_t port_value = P7->IN;
    for (i = 0; i < 8; i++) {
        value[i] = !!(port_value & (0x01 << i));
    }
}

void ir_read_off() {
    P5->OUT &= ~0x08;
    P9->OUT &= ~0x04;
}

void main(void) {
    Clock_Init48MHz();
    printf("start init\n");

    //led_init();
    ir_init();
    motor_init();

    printf("end init\n");

    char ir_value[8];

    uint32_t current_on_time = BASE_ON_TIME;
    uint32_t current_off_time = BASE_OFF_TIME;

    while (1) {
        ir_read_ready();
        ir_read_value(ir_value);
        ir_read_off();
        int i;
#ifdef DEBUG_PRINT

        for (i = 7; i >= 0; i--) { printf("%d", ir_value[i]); }
        printf("\n");
#endif
        int cnt = 0;
        for (i = 0; i < 8; i++) {
            if (ir_value[i]) cnt++;
        }

        if (cnt >= 7) {
            motor_move_stop();
#ifdef DEBUG_PRINT
            printf("T-detected.\n");
#endif
        } else if (ir_value[3] && ir_value[4]) {
            motor_move_forward();
            current_on_time = BASE_ON_TIME;
            current_off_time = BASE_OFF_TIME;
        } else if (ir_value[5] || ir_value[6] || ir_value[7]) {
            motor_move_biases_left();
            current_on_time = TURN_ON_TIME;
            current_off_time = TURN_OFF_TIME;
        } else if (ir_value[0] || ir_value[1] || ir_value[2]) {
            motor_move_biases_right();
            current_on_time = TURN_ON_TIME;
            current_off_time = TURN_OFF_TIME;
        } else {
            motor_move_stop();
            current_on_time = 0;
            current_off_time = LOOP_PERIOD_MS;
        }

        if (current_on_time > 0) {
            Clock_Delay1ms(current_on_time);
            motor_move_stop();
        }

        Clock_Delay1ms(current_off_time);
    }
}