#include <pic16f689.h>
#include <stdint.h>
#include <stdbool.h>
#include "delays.h"
#include "configuration.h"
#include "macros.h"
#include "main.h"
#include "rnd.h"

typedef enum {
    // Just going forward
    FORWARD = 0x00,
    
    // Lost the floor
    NOFLOOR = 0x10,
    
    // Obstacle avoidance
    OBSAVOID = 0x20,
    OA_STOP = 0x20, // Turn on brake lights, stop
    OA_REVERSE = 0x21, // Turn in reverse for a moment
    OA_FORWARD = 0x22, // Go forward and straighten after a moment
    
    GIVEUP = 0x30 // Give up after totally stuck
} states_t;

typedef enum {
    EV_STATECHANGED = 0x01, // State machine entered a new state, perform
                            // initialization for the new state.
    EV_NOFLOOR = 0x02, // Bottom sensor does not detect floor
    EV_FLOOR = 0x04, // Bottom sensor has detected floor for 500ms
    EV_OBSTACLE = 0x08,  // Front sensor detects an obstacle
    EV_NOOBSTACLE = 0x10, // Front sensor has not detected obstacle for 500ms
    EV_STUCK = 0x20, // Hall sensor has not detected a pulse for a while
} events_t;

states_t obsavoid_state_machine(states_t state, events_t events, uint16_t ticks_in_state)
{
    static uint16_t lastob_time;
    static BOOL turn_direction;

    switch (state)
    {
        case OA_STOP:
            // No events are handled during stopping, a bit dull state
            MOTOR_PWR = 0;
            g_brakes = true;
            delay_ticks(50);
            g_brakes = false;
            return OA_REVERSE;
        
        case OA_REVERSE:
            if (events & EV_STATECHANGED)
            {
                if (lastob_time + 1000 < get_tick_count())
                {
                    // If there has been atleast 10 seconds since previous
                    // obstacle, decide a new direction.
                    turn_direction = rnd_get();
                }
                lastob_time = get_tick_count();
            
                if (turn_direction)
                {
                    set_turnsignals(TURN_LEFT);
                    set_servo_pos(SERVO_FULLLEFT);
                }
                else
                {
                    set_turnsignals(TURN_RIGHT);
                    set_servo_pos(SERVO_FULLRIGHT);
                }
                
                delay_ticks(10);
                LED_REVERSE = LED_ON;
                delay_ticks(40);
                MOTOR_DIR = 0;
                MOTOR_PWR = 1;
                
                if (events & EV_STUCK)
                {
                    delay_ticks(100);
                }
            }
            
            if (ticks_in_state > 150 && (events & EV_NOOBSTACLE))
            {
                // Obstacle is not visible anymore, start going forward
                g_brakes = true;
                MOTOR_PWR = 0;
                delay_ticks(50);
                g_brakes = false;
                LED_REVERSE = LED_OFF;
                return OA_FORWARD;
            }
            
            break;
        
        case OA_FORWARD:
            if (events & EV_STATECHANGED)
            {
                if (!turn_direction)
                {
                    set_turnsignals(TURN_LEFT);
                    set_servo_pos(SERVO_FULLLEFT);
                }
                else
                {
                    set_turnsignals(TURN_RIGHT);
                    set_servo_pos(SERVO_FULLRIGHT);
                }
                
                delay_ticks(50);
                MOTOR_DIR = 1;
                MOTOR_PWR = 1;
            }
            
            if (events & EV_OBSTACLE)
                return OA_STOP;
            
            if (events & EV_STUCK)
                return OA_REVERSE;
            
            if (ticks_in_state > 200)
            {
                set_servo_pos(SERVO_CENTER);
                set_turnsignals(TURN_NONE);
                return FORWARD;
            }
            break;
    }
    return state;
}

BOOL has_obstacle() {
    int16_t dark, bright;
    INTCON_bits.GIE = 0;
    dark = g_adc_result_dark;
    bright = g_adc_result_bright;
    INTCON_bits.GIE = 1;
    
    if (dark > 1015)
    {
        // Not much ambient light, use small treshold
        return g_adc_result_dark - g_adc_result_bright > 1;
    }
    else
    {
        // Bright ambient light
        return g_adc_result_dark - g_adc_result_bright > 5;
    }
}

events_t get_events() {
    events_t result = 0;
    
    INTCON_bits.GIE = 0;
    if (g_adc_result_bottom < 3 && g_adc_prev_bottom < 3)
        result |= EV_NOFLOOR;
    INTCON_bits.GIE = 1;
    
    if (g_stuck)
        result |= EV_STUCK;
    
    if (has_obstacle())
        result |= EV_OBSTACLE;
    
    return result;
}

// SDCC fails to initialize local static variables, therefore these are global.
static states_t g_state = FORWARD;
static states_t g_old_state = 0xFF;
static BOOL g_has_had_floor = false;

void control_iteration() {
    static uint16_t last_tick_count;
    static uint16_t ticks_in_state;
    static uint16_t ticks_no_obstacle;
    static uint16_t ticks_floor;
    static uint16_t ticks_stuck;
    uint16_t current_tick_count;
    uint16_t delta;
    events_t events = get_events();
    
    current_tick_count = get_tick_count();
    delta = current_tick_count - last_tick_count;
    last_tick_count = current_tick_count;
    
    ticks_in_state += delta;
    
    if (!(events & EV_NOFLOOR))
        g_has_had_floor = true;
    else if (!g_has_had_floor)
        events &= ~EV_NOFLOOR; // Automatically work around broken sensors
    
    if (events & EV_OBSTACLE)
        ticks_no_obstacle = 0;
    else if (ticks_no_obstacle > 50)
        events |= EV_NOOBSTACLE;
    else
        ticks_no_obstacle += delta;
    
    if (events & EV_NOFLOOR)
        ticks_floor = 0;
    else if (ticks_floor > 50)
        events |= EV_FLOOR;
    else
        ticks_floor += delta;
    
    if (!(events & EV_STUCK))
        ticks_stuck = 0;
    else if (ticks_stuck > 500)
        g_state = GIVEUP;
    else
        ticks_stuck += delta;
    
    if (g_old_state != g_state)
    {
        events |= EV_STATECHANGED;
        ticks_in_state = 0;
    }
    g_old_state = g_state;
    
    rs232_send(0xAA);
    rs232_send(0xAA);
    rs232_send(g_state);
    rs232_send(events);

    switch (g_state & 0xF0)
    {
        case FORWARD:
            if (events & EV_STATECHANGED)
            {
                set_servo_pos(SERVO_CENTER);
                g_brakes = false;
                LED_REVERSE = LED_OFF;
                MOTOR_DIR = 1;
                MOTOR_PWR = 1;
                set_turnsignals(TURN_NONE);
            }
            
            if (events & EV_OBSTACLE)
                g_state = OBSAVOID;
            
            if (events & EV_STUCK)
                g_state = OA_REVERSE;
            
            if (events & EV_NOFLOOR)
                g_state = NOFLOOR;
            break;
        
        case NOFLOOR:
            if (events & EV_STATECHANGED)
            {
                LED_REVERSE = LED_OFF;
                set_turnsignals(TURN_NONE);
                g_brakes = true;
                MOTOR_PWR = 0;
            }
            
            if (events & EV_FLOOR)
            {
                g_state = FORWARD;
            }
            break;
        
        case OBSAVOID:
            if (events & EV_NOFLOOR)
                g_state = NOFLOOR;
            else
                g_state = obsavoid_state_machine(g_state, events, ticks_in_state);
            break;
        
        case GIVEUP:
            MOTOR_PWR = 0;
            LED_REVERSE = LED_OFF;
            g_brakes = false;
            set_servo_pos(SERVO_CENTER);
            set_turnsignals(TURN_EMERGENCY);
            delay_ticks(500);
            go_to_sleep();
            break;
    }
}