// ----------------------------------------------------------------------------------------
//
//  Minisumo Robot Project: The Thin Man
//  main.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//    
//  Copyright (C) 2007 Bob Cook
//
//      This program is free software; you can redistribute it and/or modify it
//      under the terms of the GNU General Public License as published by the
//      Free Software Foundation; either version 2 of the License, or (at your
//      option) any later version.
//    
//      This program is distributed in the hope that it will be useful, but 
//      WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
//      or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License 
//      for more details.
//    
//      You should have received a copy of the GNU General Public License along
//      with this program; if not, write to the Free Software Foundation, Inc., 
//      51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
//
// ----------------------------------------------------------------------------------------

#include <avr/interrupt.h>
#include <avr/io.h>
#include <avr/pgmspace.h>

#include <ctype.h>
#include <stdio.h>

#include "packages/avr/device/adc.h"
#include "packages/avr/device/int_helpers.h"
#include "packages/avr/device/spinwait.h"
#include "packages/avr/redir/redir.h"


// ----------------------------------------------------------------------------------------

const uint8_t led_status      = 0;
const uint8_t led_front_left  = 1;
const uint8_t led_front_right = 2;
const uint8_t led_rear_left   = 3;
const uint8_t led_rear_right  = 4;

static void led_on( uint8_t which )
{
    switch ( which )
    {
        case led_status:
            PORTD &= ~( 1 << PD2 );
            break;

        case led_front_left:
            PORTD &= ~( 1 << PD5 );
            break;

        case led_front_right:
            PORTD &= ~( 1 << PD3 );
            break;

        case led_rear_left:
            PORTD &= ~( 1 << PD4 );
            break;

        case led_rear_right:
            PORTB &= ~( 1 << PB4 );
            break;
    }
}

static void led_toggle( uint8_t which )
{
    switch ( which )
    {
        case led_status:
            PORTD ^= ( 1 << PD2 );
            break;

        case led_front_left:
            PORTD ^= ( 1 << PD5 );
            break;

        case led_front_right:
            PORTD ^= ( 1 << PD3 );
            break;

        case led_rear_left:
            PORTD ^= ( 1 << PD4 );
            break;

        case led_rear_right:
            PORTB ^= ( 1 << PB4 );
            break;
    }
}

static void led_off( uint8_t which )
{
    switch ( which )
    {
        case led_status:
            PORTD |= ( 1 << PD2 );
            break;

        case led_front_left:
            PORTD |= ( 1 << PD5 );
            break;

        case led_front_right:
            PORTD |= ( 1 << PD3 );
            break;

        case led_rear_left:
            PORTD |= ( 1 << PD4 );
            break;

        case led_rear_right:
            PORTB |= ( 1 << PB4 );
            break;
    }
}

static void led_off()
{
    PORTD |= ( 1 << PD2 ) | ( 1 << PD3 ) | ( 1 << PD4 ) | ( 1 << PD5 );
    PORTB |= ( 1 << PB4 );
}


// ----------------------------------------------------------------------------------------

static bool pushbutton_is_down()
{
    return ( PINC & ( 1 << PC0 ) ) == 0;
}


// ----------------------------------------------------------------------------------------

static uint16_t white_limit_left;
static uint16_t white_limit_right;

static void set_floor_limits()
{
    uint16_t limit = adc_read( 4 );

    for ( uint8_t i = 0; i < 20; i++ )
    {
        limit += adc_read( 4 );
        limit /= 2;
    }

    white_limit_left = limit + ( ( 1023 - limit ) / 2 );

    limit = adc_read( 2 );

    for ( uint8_t i = 0; i < 20; i++ )
    {
        limit += adc_read( 2 );
        limit /= 2;
    }

    white_limit_right = limit + ( ( 1023 - limit ) / 2 );
}


// ----------------------------------------------------------------------------------------

static bool s_floor_left;
static bool s_floor_right;

static void update_floor_sensors()
{
    static uint8_t samples = 0;
    static uint8_t hits_left = 0;
    static uint8_t hits_right = 0;

    if ( ++samples > 5 )
    {
        if ( s_floor_left = ( hits_left > 3 ) )
        {
            led_on( led_front_left );
        }
        else
        {
            led_off( led_front_left );
        }

        if ( s_floor_right = ( hits_right > 3 ) )
        {
            led_on( led_front_right );
        }
        else
        {
            led_off( led_front_right );
        }

        samples = 0;
        hits_left = 0;
        hits_right = 0;
    }

    hits_left += ( adc_read( 4 ) > white_limit_left ) ? 1 : 0;
    hits_right += ( adc_read( 2 ) > white_limit_right ) ? 1 : 0;
}

static bool check_floor_left()
{
    return s_floor_left;
}

static bool check_floor_right()
{
    return s_floor_right;
}


// ----------------------------------------------------------------------------------------

static bool s_opponent_left;
static bool s_opponent_right;

static void update_opponent_sensors()
{
    static uint8_t samples = 0;
    static uint8_t hits_left = 0;
    static uint8_t hits_right = 0;

    if ( ++samples > 5 )
    {
        s_opponent_left = ( hits_left > 3 );

        if ( s_opponent_left )
        {
            led_on( led_rear_left );
        }
        else
        {
            led_off( led_rear_left );
        }
        
        s_opponent_right = ( hits_right > 3 );

        if ( s_opponent_right )
        {
            led_on( led_rear_right );
        }
        else
        {
            led_off( led_rear_right );
        }

        samples = 0;
        hits_left = 0;
        hits_right = 0;
    }

    // the GP2Y0D340K sensor reads low when it sees an object

    hits_left  += ( ( PINC & ( 1 << PC5 ) ) ? 0 : 1 );
    hits_right += ( ( PINC & ( 1 << PC1 ) ) ? 0 : 1 );
}

static bool check_forward_left()
{
    return s_opponent_left;
}

static bool check_forward_right()
{
    return s_opponent_right;
}


// ----------------------------------------------------------------------------------------

static void set_motors( int16_t left, int16_t right )
{
    if ( right == 0 )
    {
        PORTD |= ( 1 << PD6 );
        OCR1AL = 255;
    }
    else if ( right < 0 )
    {
        PORTD |= ( 1 << PD6 );
        OCR1AL = static_cast< uint8_t >( 255 + right );
    }
    else
    {
        PORTD &= ~( 1 << PD6 );
        OCR1AL = static_cast< uint8_t >( right );
    }
    
    if ( left == 0 )
    {
        PORTD |= ( 1 << PD7 );
        OCR1BL = 0;
    }
    else if ( left < 0 )
    {
        PORTD |= ( 1 << PD7 );
        OCR1BL = static_cast< uint8_t >( -left );
    }
    else
    {
        PORTD &= ~( 1 << PD7 );
        OCR1BL = static_cast< uint8_t >( 255 - left );
    }
}


// ----------------------------------------------------------------------------------------
//  check for the pushbutton within 2.5 seconds, return false if pressed.
//

static bool do_immediate_start()
{
    led_off( led_status );

    led_on( led_front_left );
    led_on( led_front_right );
    led_on( led_rear_left );
    led_on( led_rear_right );

    for ( uint8_t i = 0; i < 250; i++ )
    {
        if ( pushbutton_is_down() )
        {
            led_on( led_status );

            led_off( led_front_left );
            led_off( led_front_right );
            led_off( led_rear_left );
            led_off( led_rear_right );

            spinwait_delay_ms( 1 ); // de-bounce
            while ( pushbutton_is_down() ) ;
            spinwait_delay_ms( 1 ); // de-bounce

            return false;
        }

        spinwait_delay_ms( 10 );
    }

    return true;
}


// ----------------------------------------------------------------------------------------
//  select an initial mode. use the pushbutton as both a toggle switch and an "enter" key;
//  if pressed quickly (less than 750ms) its only a toggle
//

const uint8_t start_mode_go_straight = 0;
const uint8_t start_mode_go_left     = 1;
const uint8_t start_mode_go_right    = 2;
const uint8_t start_mode_pause_first = 3;

const uint8_t display_seg_ticks = 250;
const uint8_t display_on_ticks  = 75;

static uint8_t get_start_mode()
{
    uint8_t  mode         = start_mode_go_straight;
    uint16_t mode_display = 0;

    for ( ;; )
    {
        switch ( mode_display++ )
        {
            case ( start_mode_go_straight * display_seg_ticks ):
                led_on( led_status );
                break;

            case ( start_mode_go_straight * display_seg_ticks + display_on_ticks ):
                led_off( led_status );
                break;

            case ( start_mode_go_left * display_seg_ticks ):
                if ( mode >= start_mode_go_left )
                {
                    led_on( led_status );
                }
                break;

            case ( start_mode_go_left * display_seg_ticks + display_on_ticks ):
                led_off( led_status );
                break;

            case ( start_mode_go_right * display_seg_ticks ):
                if ( mode >= start_mode_go_right )
                {
                    led_on( led_status );
                }
                break;

            case ( start_mode_go_right * display_seg_ticks + display_on_ticks ):
                led_off( led_status );
                break;

            case ( start_mode_pause_first * display_seg_ticks ):
                if ( mode >= start_mode_pause_first )
                {
                    led_on( led_status );
                }
                break;

            case ( start_mode_pause_first * display_seg_ticks + display_on_ticks ):
                led_off( led_status );
                break;

            case ( ( start_mode_pause_first + 2 ) * display_seg_ticks ):
                mode_display = 0;
                break;
        }

        if ( pushbutton_is_down() )
        {
            led_off( led_status );
            led_on( led_front_right );
            led_on( led_front_left );

            spinwait_delay_ms( 1 ); // de-bounce

            uint8_t counts = 0;

            while ( pushbutton_is_down() )
            {
                if ( counts < 255 )
                {
                    counts++;
                }
            
                if ( counts > 30 )
                {
                    led_on( led_status );
                }
                else
                {
                    led_off( led_status );
                }

                spinwait_delay_ms( 25 );
            }

            led_off( led_front_right );
            led_off( led_front_left );
            
            spinwait_delay_ms( 1 ); // de-bounce

            if ( ( counts > 0 ) && ( counts < 30 ) )
            {
                mode++;

                if ( mode > start_mode_pause_first )
                {
                    mode = start_mode_go_straight;
                }
            }
            else if ( counts > 30 )
            {
                break;  // held down for > 750 ms
            }
        }

        spinwait_delay_ms( 1 );
    }

    return mode;
}


// ----------------------------------------------------------------------------------------
//  blink the leds for five seconds
//

static void do_start_delay()
{
    for ( uint8_t i = 0; i < 5; i++ )
    {
        for ( uint8_t j = 0; j < 10; j++ )
        {
            led_toggle( led_front_right );
            led_toggle( led_front_left );
            led_toggle( led_rear_left );
            led_toggle( led_rear_right );

            spinwait_delay_ms( 100 );
        }
    }
}


// ----------------------------------------------------------------------------------------

static void respond_to_floor( bool triggered_right_side )
{
    set_motors( -255, -255 );

    uint16_t state = 0;

    while ( state < 500 )
    {
        update_floor_sensors();
        update_opponent_sensors();

        bool floor_left = check_floor_left();
        bool floor_right = check_floor_right();

        if ( floor_left || floor_right )
        {
            state = 0; // get away from the edge
            set_motors( -255, -255 );
            spinwait_delay_ms( 1 );
            continue;
        }

        state++;

        if ( state > 300 )
        {
            if ( check_forward_left() || check_forward_right() )
            {
                return;
            }
        }

        if ( state > 100 )
        {
            // away from the edge, now rotate left or right

            if ( triggered_right_side )
            {
                set_motors( -255, 255 );
            }
            else
            {
                set_motors( 255, -255 );
            }
        }

        // still backing up from the edge

        spinwait_delay_ms( 1 );
    }
}


// ----------------------------------------------------------------------------------------

int main()
{
    //--    Hardware initialization.
    //
    //      Indicator LEDs:
    //
    //          PORTD.2 - status led
    //          PORTD.3 - front right led
    //          PORTD.5 - front left led
    //          PORTD.4 - rear left led
    //          PORTB.4 - rear right led
    //
    //      Sensors:
    //
    //          PORTC.0 - pushbutton
    //          PORTC.2 - left floor sensor
    //          PORTC.3 - center floor sensor
    //          PORTC.4 - right floor sensor
    //          PORTC.1 - right opponent sensor
    //          PORTC.5 - left opponent sensor
    //
    //      Set up Timer1 for the two-channel PWM mode to drive the motor H-bridges and
    //      the associated direction pins:
    //
    //          PORTB.1 (OC1A) - right pwm
    //          PORTD.6        - right direction
    //          PORTB.2 (OC1B) - left pwm
    //          PORTD.7        - left direction

    cli();
    interrupts_clear_all();

    DDRB = ( 1 << PB1 ) | ( 1 << PB2 ) | ( 1 << PB4 );
    
    DDRC = 0;
    PORTC = ( 1 << PC0 ); // pull-up resistors off except for PC0
    
    DDRD = ( 1 << PD2 ) | ( 1 << PD3 ) | ( 1 << PD4 ) | ( 1 << PD5 )
         | ( 1 << PD6 ) | ( 1 << PD7 );

    TCCR1A = ( 1 << COM1A1 ) | ( 0 << COM1A0 )      // clear OC1A on compare match
           | ( 1 << COM1B1 ) | ( 1 << COM1B0 )      // set OC1B on compare match
           | ( 0 << WGM11  ) | ( 1 << WGM10  );     // mode 5: fast PWM, 8-bit
    
    TCCR1B = ( 0 << WGM13 ) | ( 1 << WGM12 )        // mode 5: fast PWM, 8-bit
           | ( 0 << CS12  ) | ( 1 << CS11  )
           | ( 0 << CS10  );                        // CLK/8

    TCNT1H = 0x00;
    TCNT1L = 0x00;

    OCR1AH = 0x00;
    OCR1AL = 0x00;
    OCR1BH = 0x00;
    OCR1BL = 0x00;

    adc_init();
    redir_initialize();

    led_off();

    set_motors( 0, 0 );

    sei();

    printf_P( PSTR( "The Thin Man\n" ) );

    //--    Start the minisumo program.

    uint16_t recent_opponent_left = 0;
    uint16_t recent_opponent_right = 0;

    uint8_t start_mode = start_mode_go_straight;

    if ( ! do_immediate_start() )
    {
        start_mode = get_start_mode();
        
        led_off( led_status );

        do_start_delay();
    }

    set_floor_limits();

    switch ( start_mode )
    {
        case start_mode_go_straight:
            break;

        case start_mode_go_left:
            recent_opponent_left = 500;
            break;

        case start_mode_go_right:
            recent_opponent_right = 500;
            break;

        case start_mode_pause_first:

            for ( uint16_t i = 0; i < 3000; i++ )
            {
                update_floor_sensors();
                update_opponent_sensors();

                if ( check_forward_left() || check_forward_right() )
                {
                    break;
                }

                spinwait_delay_ms( 1 );
            }
            break;
    }

    for ( ;; )
    {
        update_floor_sensors();
        update_opponent_sensors();

        // reached the edge?

        bool floor_left = check_floor_left();
        bool floor_right = check_floor_right();

        if ( floor_left || floor_right )
        {
            set_motors( -255, -255 );
            respond_to_floor( floor_right );

            recent_opponent_left = 0;
            recent_opponent_right = 0;
        }

        // seen the opponent?

        bool opponent_left = check_forward_left();
        bool opponent_right = check_forward_right();

        if ( opponent_left || opponent_right )
        {
            set_motors( 255, 255 );

            recent_opponent_left  = ( opponent_left  ? 800 : 0 );
            recent_opponent_right = ( opponent_right ? 800 : 0 );
        }
        else if ( recent_opponent_left > 0 )
        {
            set_motors( 16, 255 );
            --recent_opponent_left;
        }
        else if ( recent_opponent_right > 0 )
        {
            set_motors( 255, 16 );
            --recent_opponent_right;
        }
        else
        {
            set_motors( 255, 255 );
        }

        // and do it all over again

        spinwait_delay_ms( 1 );
    }

    return 0;
}

