// ----------------------------------------------------------------------------------------
//
//  avr/sensors/avrcam/avrcam.cpp
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
//    
//  This file implements support for the JROBOT AVRcam.
//
//  Copyright (C) 2009 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 "avrcam.h"

#include "packages/common/util/misc.h"

#include "packages/avr/device/spinwait.h"
#include "packages/avr/device/uart.h"

#include <stdio.h>
#include <avr/pgmspace.h>


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

#include "project_defs.h"


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

#if !defined( PRJ_AVRCAM_USE_UART0 ) && !defined( PRJ_AVRCAM_USE_UART1 )
#error One of PRJ_AVRCAM_USE_UART0 or PRJ_AVRCAM_USE_UART1 is required!
#endif

#if defined( PRJ_AVRCAM_USE_UART0 ) && defined( PRJ_AVRCAM_USE_UART1 )
#error Only one of PRJ_AVRCAM_USE_UART0 or PRJ_AVRCAM_USE_UART1 can be defined!
#endif

#if defined( PRJ_AVRCAM_USE_UART0 ) && defined( PRJ_REDIR_VIA_UART0 )
#error STDIO redirection conflicts with AVRcam use of UART0!
#endif

#if defined( PRJ_AVRCAM_USE_UART1 ) && defined( PRJ_REDIR_VIA_UART1 )
#error STDIO redirection conflicts with AVRcam use of UART1!
#endif


// ----------------------------------------------------------------------------------------
    
#if defined( PRJ_AVRCAM_ENABLE_TRACKING )

static volatile avrcam_track_data_t* g_tracking_data_ptr;
static volatile uint8_t              g_tracking_num_data;
static volatile uint8_t              g_tracking_slot_index;

static volatile uint8_t g_tracking_state;

static volatile uint8_t g_tracking_color;
static volatile uint8_t g_tracking_ulx;
static volatile uint8_t g_tracking_uly;
static volatile uint8_t g_tracking_lrx;

#if defined( PRJ_AVRCAM_ENABLE_TRACKING_CLIP )

static volatile uint8_t g_track_clip_ulx;
static volatile uint8_t g_track_clip_uly;
static volatile uint8_t g_track_clip_lrx;
static volatile uint8_t g_track_clip_lry;

#endif  // PRJ_AVRCAM_ENABLE_TRACKING_CLIP

#endif  // PRJ_AVRCAM_ENABLE_TRACKING


// ----------------------------------------------------------------------------------------
    
static bool receive_line( char* str, uint8_t str_size )
{
    uint8_t idle_count   = 0;
    bool    data_overrun = false;

    for ( ;; )
    {
        char c;

#if defined( PRJ_AVRCAM_USE_UART0 )

        if ( uart0_is_char_available() )
        {
            c = static_cast< char >( uart0_read() );

#elif defined( PRJ_AVRCAM_USE_UART1 )

        if ( uart1_is_char_available() )
        {
            c = static_cast< char >( uart1_read() );

#endif
            
//if ( idle_count > 0 )
//{
//    printf_P( PSTR("...idle_count = %d\n"), idle_count );
//}
            idle_count = 0;

            if ( ( c == '\015' ) || ( c == '\012' ) )
            {
                *str = '\0';
                return data_overrun ? false : true;
            }

            if ( str_size > 1 )
            {
                *str = c;
                str++;
                str_size--;
            }
            else
            {
                data_overrun = true;
            }
        }
        else
        {
            if ( ++idle_count > 125 )
            {
                return false;
            }

            spinwait_delay_ms( 5 );
        }
    }

    return false;
}


// ----------------------------------------------------------------------------------------
    
static void flush_input()
{
    uint8_t idle_count = 0;

    for ( ;; )
    {

#if defined( PRJ_AVRCAM_USE_UART0 )

        if ( uart0_is_char_available() )
        {
            uart0_read();

#elif defined( PRJ_AVRCAM_USE_UART1 )

        if ( uart1_is_char_available() )
        {
            uart1_read();

#endif
            
            idle_count = 0;
        }
        else
        {
            if ( ++idle_count > 125 )
            {
                return;
            }
        }
    }
}


// ----------------------------------------------------------------------------------------
    
#if 0
static bool read_data( uint8_t* buf, uint16_t buf_size )
{
    uint8_t idle_count = 0;

    for ( uint16_t i = 0; i < buf_size; i++ )
    {
        uint8_t c;

#if defined( PRJ_AVRCAM_USE_UART0 )

        if ( uart0_is_char_available() )
        {
            c = uart0_read();

#elif defined( PRJ_AVRCAM_USE_UART1 )

        if ( uart1_is_char_available() )
        {
            c = uart1_read();

#endif

            idle_count = 0;

            *buf = c;
            buf++;
        }
        else
        {
            if ( ++idle_count > 125 )
            {
                return false;
            }

            spinwait_delay_us( 10 );
        }
    }

    return true;
}
#endif


// ----------------------------------------------------------------------------------------
    
static bool send_command( char cmdchar0, char cmdchar1 )
{

#if defined( PRJ_AVRCAM_USE_UART0 )

    uart0_write( cmdchar0 );
    uart0_write( cmdchar1 );
    uart0_write( '\015' );

#elif defined( PRJ_AVRCAM_USE_UART1 )
    
    uart1_write( cmdchar0 );
    uart1_write( cmdchar1 );
    uart1_write( '\015' );

#endif

    char ack_buffer[ 6 ] = { 0 };

    if ( ! receive_line( ack_buffer, sizeof( ack_buffer ) ) )
    {
        return false;
    }

    if ( ( ack_buffer[ 0 ] != 'A'  ) || 
         ( ack_buffer[ 1 ] != 'C'  ) ||
         ( ack_buffer[ 2 ] != 'K'  ) ||
         ( ack_buffer[ 3 ] != '\0' ) )
    {
        return false;
    }

    return true;
}


// ----------------------------------------------------------------------------------------
    
bool avrcam_init()
{

#if defined( PRJ_AVRCAM_USE_UART0 )

    uart0_init();
    uart0_set_baudrate( 115200 );

#elif defined( PRJ_AVRCAM_USE_UART1 )
    
    uart1_init();
    uart1_set_baudrate( 115200 );

#endif

    UCSR1A = ( 1 << U2X1 );
    UCSR1B = ( 1 << RXCIE1 ) | ( 1 << RXEN1 ) | ( 1 << TXEN1 ) | ( 0 << UCSZ12 );
    UCSR1C = ( 0 << UMSEL1 ) | ( 0 << UPM11  ) | ( 0 << UPM10  )
           | ( 0 << USBS1  ) | ( 1 << UCSZ11 ) | ( 1 << UCSZ10 );
    UBRR1H =  0;
    UBRR1L = 16;

#if defined( PRJ_AVRCAM_ENABLE_TRACKING )

    g_tracking_data_ptr = 0;

#endif  // PRJ_AVRCAM_ENABLE_TRACKING

#if defined( PRJ_AVRCAM_ENABLE_TRACKING_CLIP )

    g_track_clip_ulx = 0;
    g_track_clip_uly = 0;
    g_track_clip_lrx = 255;
    g_track_clip_lry = 255;
    
#endif  // PRJ_AVRCAM_ENABLE_TRACKING_CLIP

    return avrcam_ping();
}


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

bool avrcam_ping()
{
    send_command( 'D', 'T' );
    send_command( 'R', 'S' );
    flush_input();
    return send_command( 'P', 'G' );
}


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

#if defined( PRJ_AVRCAM_ENABLE_GETVERSION )

bool avrcam_get_version(  char* verstr, uint8_t str_size )
{
    if ( ! send_command( 'G', 'V' ) )
    {
        return false;
    }

    return receive_line( verstr, str_size );
}

#endif  // PRJ_AVRCAM_ENABLE_GETVERSION


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

#if defined( PRJ_AVRCAM_ENABLE_TRACKING_CLIP )

void avrcam_set_tracking_clip( uint8_t ulx, uint8_t uly, uint8_t lrx, uint8_t lry )
{
    g_track_clip_ulx = ulx;
    g_track_clip_uly = uly;
    g_track_clip_lrx = lrx;
    g_track_clip_lry = lry;
}

#endif  // PRJ_AVRCAM_ENABLE_TRACKING_CLIP


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

#if defined( PRJ_AVRCAM_ENABLE_TRACKING )

bool avrcam_start_tracking( avrcam_track_data_t* data, uint8_t num_data )
{
    if ( data == 0 || num_data == 0 )
    {
        g_tracking_data_ptr = 0;
        return false;
    }

    if ( ! send_command( 'E', 'T' ) )
    {
        return false;
    }

    g_tracking_data_ptr = data;
    g_tracking_num_data = num_data;
    g_tracking_state    = 0;

    return true;
}

#endif  // PRJ_AVRCAM_ENABLE_TRACKING


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

#if defined( PRJ_AVRCAM_ENABLE_TRACKING )

bool avrcam_update_tracking()
{
    if ( g_tracking_data_ptr == 0 )
    {
        return false;
    }

    bool result = false;

#if defined( PRJ_AVRCAM_USE_UART0 )

    while ( uart0_is_char_available() )
    {
        uint8_t c = uart0_read();

#elif defined( PRJ_AVRCAM_USE_UART1 )

    while ( uart1_is_char_available() )
    {
        uint8_t c = uart1_read();

#endif
            
//printf_P( PSTR("%02x "), c );

        switch ( g_tracking_state )
        {
            case 0: // looking for the start of the frame

                if ( c == 0x0a )
                {
                    g_tracking_slot_index = 0;
                    g_tracking_state = 1;
                }
                break;

            case 1: // read the tracked object count

                g_tracking_state = 2; // we ignore this value
                break;

            case 2: // read the color index

                if ( c == 0xff ) // end of tracked objects
                {
                    volatile avrcam_track_data_t* slot_ptr = g_tracking_data_ptr;
                    slot_ptr += g_tracking_slot_index;

                    while ( g_tracking_slot_index < g_tracking_num_data )
                    {
                        slot_ptr->id = 0;
                        slot_ptr++;
                        g_tracking_slot_index++;
                    }

//            for ( uint8_t i = 0; i < g_tracking_num_data; i++ )
//            {
//                printf_P( PSTR("index: %d color: %d ul(%d,%d) lr(%d,%d)\n"), i, g_tracking_data_ptr[ i ].id,
//                      g_tracking_data_ptr[ i ].ul_x, g_tracking_data_ptr[ i ].ul_y, g_tracking_data_ptr[ i ].lr_x, g_tracking_data_ptr[ i ].lr_y );
//            }
                    result = true;

                    g_tracking_state = 0; // wait for the start of the next frame
                }
                else
                {
                    g_tracking_color = c;
                    g_tracking_state = 3;
                }
                break;

            case 3: // read the ulx value

#if defined( PRJ_AVRCAM_ENABLE_TRACKING_CLIP )

                if ( c > g_track_clip_lrx )
                {
                    // completely outside the frame
                    g_tracking_color = 0;
                }
                else
                {
                    // top edge clipped to frame
                    g_tracking_ulx = util_max( c, g_track_clip_ulx );
                }

#else
                
                g_tracking_ulx = c;

#endif  // PRJ_AVRCAM_ENABLE_TRACKING_CLIP

                g_tracking_state = 4;
                break;

            case 4: // read the uly value

#if defined( PRJ_AVRCAM_ENABLE_TRACKING_CLIP )

                if ( c > g_track_clip_lry )
                {
                    // completely outside the frame
                    g_tracking_color = 0;
                }
                else
                {
                    // left edge clipped to frame
                    g_tracking_uly = util_max( c, g_track_clip_uly );
                }

#else

                g_tracking_uly = c;

#endif  // PRJ_AVRCAM_ENABLE_TRACKING_CLIP

                g_tracking_state = 5;
                break;

            case 5: // read the lrx value

#if defined( PRJ_AVRCAM_ENABLE_TRACKING_CLIP )

                // bottom edge clipped to frame
                g_tracking_lrx = util_min( c, g_track_clip_lrx );

#else
                
                g_tracking_lrx = c;

#endif  // PRJ_AVRCAM_ENABLE_TRACKING_CLIP

                g_tracking_state = 6;
                break;

            case 6: // read the lry value, save color info

                if ( g_tracking_color > 0 && g_tracking_slot_index < g_tracking_num_data )
                {
                    volatile avrcam_track_data_t* slot_ptr = g_tracking_data_ptr;
                    slot_ptr += g_tracking_slot_index;

                    slot_ptr->id   = g_tracking_color;
                    slot_ptr->ul_x = g_tracking_ulx;
                    slot_ptr->ul_y = g_tracking_uly;
                    slot_ptr->lr_x = g_tracking_lrx;

#if defined( PRJ_AVRCAM_ENABLE_TRACKING_CLIP )

                    // right edge clipped to frame
                    slot_ptr->lr_y = util_min( c, g_track_clip_lry );

#else

                    slot_ptr->lr_y = c;

#endif

                    g_tracking_slot_index++;
                }

                g_tracking_state = 2; // handle the next target
                break;

            default:

                g_tracking_state = 0; // wait for the start of the next frame
                break;
        }
    }

    return result;
}

#endif  // PRJ_AVRCAM_ENABLE_TRACKING


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

#if defined( PRJ_AVRCAM_ENABLE_TRACKING )

void avrcam_stop_tracking()
{
    g_tracking_data_ptr = 0;

    send_command( 'D', 'T' );
    flush_input();
}

#endif  // PRJ_AVRCAM_ENABLE_TRACKING


// ----------------------------------------------------------------------------------------
//  end of avrcam.cpp
// ----------------------------------------------------------------------------------------

