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

static inline void adc_setchannel(uint8_t channel) {
    ADCON0 = (ADCON0 & 0b11000011) | (channel << 2);
}

enum adc_state_t {ADC_FRONT, ADC_LEFT, ADC_RIGHT};

volatile uint16_t g_servo_timing;
volatile uint8_t g_servo_enable_time = 0;

volatile int16_t g_adc_result_dark;
volatile int16_t g_adc_result_bright;
volatile int16_t g_adc_result_bottom;
volatile int16_t g_adc_prev_bottom;

static volatile uint8_t g_turnsignals_postscaler = 0;
static volatile enum turnsignals_t g_turnsignals = TURN_NONE;

volatile BOOL g_brakes = false;

// Running counter that increments every 10 milliseconds.
static volatile uint16_t g_tick_count = 0;

static volatile uint8_t g_ticks_since_hall_change = 0;
static volatile BOOL g_has_had_hall_pulse = false;
volatile BOOL g_stuck = false;

// Periodic interrupt for servo control.
// Returns the number of microseconds to the next event.
// For example, if isr_servo returns 8000, it will be called again after 8 ms.
static uint16_t isr_servo()
{
    // Provide power to the servo only when turning
    if (g_servo_enable_time > 0)
    {
        g_servo_enable_time--;
        SERVO_PWR = 1;
        
        if (!SERVO_CTRL)
        {
            // Start the pulse
            SERVO_CTRL = 1;
            return g_servo_timing;
        } else {
            // Pulse is complete
            SERVO_CTRL = 0;
            return 8000; // 8 ms
        }
    } else {
        SERVO_PWR = 0;
        SERVO_CTRL = 1;
        return 65535;
    }
}

static uint16_t isr_adc()
{
    static enum adc_state_t current_state = ADC_FRONT;
    static uint8_t substate = 0;
    
    static uint16_t result_temp1;
    static uint16_t result_temp2;
    
    if (current_state == ADC_FRONT)
    {
        // The measurement period is 70ms to eliminate any light noise from
        // AC frequency. This is split to 32 measurements of each channel.
        if (substate == 0)
        {
            substate++;
            result_temp1 = 0; // Sum of bright results
            result_temp2 = 0; // Sum of dark results
            adc_setchannel(0);
            LED_HEADLIGHTS = LED_ON;
            return 1090;
        }
        else if (substate == 1)
        {
            // Begin measuring bright state
            ADCON0_bits.GO_DONE = 1;
            
            // Turn led off (the value is already in sample and hold)
            substate++;
            LED_HEADLIGHTS = LED_OFF;
            return 1090;
        }
        else if (substate <= 64)
        {
            if (substate & 1)
            {
                // Collect result for dark state
                uint16_t result = ((uint16_t)ADRESH << 8) | ADRESL;
                result_temp2 += result;
                
                // Begin measuring bright state
                ADCON0_bits.GO_DONE = 1;
                
                // Turn led off (the value is already in sample and hold)
                substate++;
                LED_HEADLIGHTS = LED_OFF;
                return 1090;
            }
            else
            {
                // Collect result for bright state
                uint16_t result = ((uint16_t)ADRESH << 8) | ADRESL;
                result_temp1 += result;
                
                // Begin measuring dark state
                ADCON0_bits.GO_DONE = 1;
                
                // Turn led on
                substate++;
                LED_HEADLIGHTS = LED_ON;
                return 1090;
            }
        }
        else
        {
            // Collect result for dark state
            uint16_t result = ((uint16_t)ADRESH << 8) | ADRESL;
            result_temp2 += result;
            
            // Store results
            g_adc_result_bright = result_temp1 >> 5;
            g_adc_result_dark = result_temp2 >> 5;
            
            current_state++;
            substate = 0;
            return 100;
        }
    }
    else if (substate == 0)
    {
        substate++;
        
        LED_HEADLIGHTS = LED_ON;
        
        if (current_state == ADC_LEFT)
            adc_setchannel(2);
        else if (current_state == ADC_RIGHT)
            adc_setchannel(1);
        
        // Wait for ADC acquisition
        return 1000;
    } else if (substate == 1) {
        substate++;
        
        // Begin measurement
        ADCON0_bits.GO_DONE = 1;
        
        LED_HEADLIGHTS = LED_OFF;
        
        return 1000;
    } else if (substate == 2) {
        substate++;
        
        // Collect result for bright state
        result_temp1 = ((uint16_t)ADRESH << 8) | ADRESL;
        
        // Start measuring dark state
        ADCON0_bits.GO_DONE = 1;
        
        LED_HEADLIGHTS = LED_OFF;
        return 100;
    } else {
        uint16_t result = ((uint16_t)ADRESH << 8) | ADRESL;
        
        if (current_state == ADC_LEFT)
        {
            result_temp2 = result - result_temp1;
        } else {
            result = result - result_temp1;
            
            g_adc_prev_bottom = g_adc_result_bottom;
            
            // It is enough if atleast one of the sensors detects floor.
            if (result > result_temp2)
                g_adc_result_bottom = result;
            else
                g_adc_result_bottom = result_temp2;
        }
        
        if (current_state == ADC_RIGHT)
            current_state = ADC_FRONT;
        else
            current_state++;
        
        substate = 0;
        return 100;
    }
}

static uint16_t isr_tickcounter()
{
    BOOL state = (g_turnsignals_postscaler > 25) ? LED_OFF : LED_ON;
    
    switch(g_turnsignals)
    {
        case TURN_NONE:
            LED_LEFTTURN = LED_RIGHTTURN = LED_OFF;
            break;
        
        case TURN_LEFT:
            LED_LEFTTURN = state;
            LED_RIGHTTURN = LED_OFF;
            break;
        
        case TURN_RIGHT:
            LED_LEFTTURN = LED_OFF;
            LED_RIGHTTURN = state;
            break;
        
        case TURN_EMERGENCY:
            LED_LEFTTURN = LED_RIGHTTURN = state;
            break;
    }

    g_turnsignals_postscaler++;
    if (g_turnsignals_postscaler > 40)
    {
        g_turnsignals_postscaler = 0;
    }
    
    g_tick_count++;
    
    if (g_has_had_hall_pulse && MOTOR_PWR)
    {
        if (g_ticks_since_hall_change < 100)
        {
            g_ticks_since_hall_change++;
            g_stuck = false;
        } else {
            g_stuck = true;
        }
    }
    
    return 10000;
}

uint8_t STK00_save;
void isr() __interrupt 0
{
    // Each variable stores the number of microseconds remaining
    // until the next event.
    static uint16_t next_servo_event = 0;
    static uint16_t next_adc_event = 0;
    static uint16_t next_tickcounter_event = 0;
    uint16_t next_event;
    uint16_t time_now;
    static uint8_t brake_pwm = 0;
    
    // Work around bug #2840515 in SDCC
    __asm
    banksel _STK00_save
    movf STK00, W
    movwf _STK00_save
    __endasm;

    if (INTCON_bits.RABIF)
    {
        // Re-read porta to clear interrupt
        __asm
        banksel PORTA
        movf PORTA, W
        __endasm;
        INTCON_bits.RABIF = 0;
        g_ticks_since_hall_change = 0;
        g_has_had_hall_pulse = true;
    }
    
    if (INTCON_bits.T0IF)
    {
        brake_pwm++;
        if (g_brakes)
            LED_BRAKES = LED_ON;
        else
            LED_BRAKES = ((brake_pwm & 15) == 15) ? LED_ON : LED_OFF;
        
        INTCON_bits.T0IF = 0;
    }

    if (PIR1_bits.T1IF)
    {
        // Run all events
        if (next_servo_event < 50)
            next_servo_event += isr_servo();
        
        if (next_adc_event < 100)
            next_adc_event += isr_adc();
        
        if (next_tickcounter_event < 500)
            next_tickcounter_event += isr_tickcounter();
        
        // Calculate the time until next event (minimum of wait times)
        next_event = next_servo_event;
        
        if (next_adc_event < next_event)
            next_event = next_adc_event;
        
        if (next_tickcounter_event < next_event)
            next_event = next_tickcounter_event;
        
        // Compensate for the time spent in event handlers
        T1CON_bits.TMR1ON = 0;
        time_now = ((uint16_t)TMR1H << 8) | TMR1L;
        time_now = ADDS(next_event,time_now);
        
        // Set up the timer to trigger after next_event microseconds.
        next_event = 0xffffu - next_event;
        TMR1H = HIGH(next_event);
        TMR1L = LOW(next_event);
        PIR1_bits.T1IF = 0;
        T1CON_bits.TMR1ON = 1;
        
        // Update all event times
        next_servo_event = SUBS(next_servo_event, time_now);
        next_adc_event = SUBS(next_adc_event, time_now);
        next_tickcounter_event = SUBS(next_tickcounter_event, time_now);
    }
    
    __asm
    banksel _STK00_save
    movf _STK00_save, W
    movwf STK00
    __endasm;
}

void set_servo_pos(uint8_t servo_pos)
{
    // Basic formula:
    // Pulse time = (MAXPULSE - MINPULSE) * servo_pos / 256 + MINPULSE
    
    const uint16_t range = SERVO_MAXPULSE - SERVO_MINPULSE;
    uint16_t pulsetime;
    pulsetime = (uint16_t)HIGH(range) * servo_pos;
    pulsetime += HIGH((uint16_t)LOW(range) * servo_pos);
    pulsetime += SERVO_MINPULSE;
    
    INTCON_bits.GIE = 0;
    g_servo_timing = pulsetime;
    g_servo_enable_time = 100;
    INTCON_bits.GIE = 1;
}

void set_turnsignals(enum turnsignals_t value)
{
    g_turnsignals = value;
    g_turnsignals_postscaler = 0;
}

static void adc_init() {
    // Phototransistor channels as analog inputs, other pins as digital.
    ANSEL = 0b00000111;
    ANSELH = 0;
    
    ADCON0 = 0b10000001; // VDD as reference voltage
    ADCON1 = 0b01010000; // Fosc/16 clock for ADC
}

static int adc_convert() {
    ADCON0_bits.GO_DONE = 1;
    while (ADCON0_bits.GO_DONE) {}
    return ((int)ADRESH << 8) | ADRESL;
}

static uint8_t check_battery_voltage() {
    int16_t adc_result;
    uint8_t vbatt;
    adc_setchannel(0b01101);
    delay_100ms(1);
    adc_result = adc_convert();
    
    // ADC result = 1024 * (0.6V / Vbatt)
    // Therefore:
    // Vbatt = 0.6V * 1024 / ADC result
    // The result unit is chosen to be 0.1V
    // Result is rounded down.
    
    vbatt = (uint8_t)(6 * 1024 / adc_result);
    return vbatt;
}

static void rs232_init() {
    SPBRG = 12; // 38400bps
    SPBRGH = 0;
    
    TXSTA = 0b00100110; // BRGH = 1
    BAUDCTL = 0; // BRG16 = 0
    RCSTA = 0b10000000; // Only transmitter
}

void rs232_send(uint8_t byte) {
    if (!TXSTA_bits.TXEN)
        return; // Serial port disabled
    
    while (!TXSTA_bits.TRMT) {}
    TXREG = byte;
}

uint16_t get_tick_count()
{
    uint16_t result;
    INTCON_bits.GIE = 0;
    result = g_tick_count;
    INTCON_bits.GIE = 1;
    return result;
}

void delay_ticks(uint16_t tick_count)
{
    uint16_t start = get_tick_count();
    uint16_t end = start + tick_count;
    if (end < start)
    {
        while (end < get_tick_count()); // Handle overflow
    }
    
    while (end > get_tick_count());
}

static BOOL g_was_sleeping;
void go_to_sleep()
{
    INTCON = 0;
    PIE1 = 0;
    g_was_sleeping = true;
    MOTOR_PWR = 0;
    SERVO_PWR = 0;
    SENSORS_PWR = 0;
    ADCON0_bits.ADON = 0;
    OPTION_REG_bits.NOT_RABPU = 1;
    TRISA = 0b101111;
    TRISB = 0b01111111;
    TRISC = 0b01011111;
    __asm
    sleep
    __endasm;
}

void main() {
    delay_100us(200);
    delay_100us(200);

    if (PCON_bits.NOT_POR == 0 || PCON_bits.NOT_BOR == 0)
    {
        g_was_sleeping = false;
    }
    PCON_bits.NOT_POR = 1;
    PCON_bits.NOT_BOR = 1;
    
    if (!g_was_sleeping)
    {
        go_to_sleep();
    }
    g_was_sleeping = false;
    
    OSCCON = 0b01110001; // 8 MHz internal osc
    
    adc_init();
//     rs232_init();
    
    T1CON = 0b00010001; // Internal osc / 2 for Timer 1
    OPTION_REG = 0b00000010; // Internal osc / 8 for Timer 0
    PIE1_bits.T1IE = 1;
    INTCON = 0b01101000; // Enable peripheral interrupts, IOCA and TMR0
    
    PORTA = PORTB = PORTC = 0;
    TRISC = 0;
    TRISA_bits.TRISA4 = 0;
    TRISB = 0;
    
    WPUA = 0b00100000; // Pull-up for HALL input
    WPUB = 0;
    
    IOCA = 0b00100000; // Interrupt on HALL_IN change
    
    SENSORS_PWR = 1;
    
    delay_100ms(5);
    if (check_battery_voltage() < 30)
    {
        go_to_sleep();
    }
    
    INTCON_bits.GIE = 1;
    
    set_servo_pos(SERVO_CENTER);
    
    delay_100ms(2);
    
    while (true)
    {
        control_iteration();
        
        if (get_tick_count() > 60000) // 600 seconds
            go_to_sleep();
    }
}