/** 
 * @defgroup gfx gfx
 * raster-image routines
 * @{
 */

#include "main.h"
#include "prefs.h"
#include "sensor.h"
#include "source.h"
#include "gui.h"
#include "gfx.h"


/**
 * draw line
 */
void g_drawLine(unsigned char *buffer, int width, int height,
              unsigned char color, register int x1,
              register int y1, register int x2, register int y2)
{
    register int deltaX, deltaY;
    register int addX, addY;
    register int counter, error;
    // default step right 
    addX = 1;
    // ...and down
    addY = 1;
    // first calculate delta values
    deltaX = x2 - x1;
    deltaY = y2 - y1;
    // check for negative delta-value
    if(deltaX < 0) {
        addX = -addX;
        deltaX = -deltaX;
    }
    if(deltaY < 0) {
        addY = -addY;
        deltaY = -deltaY;
    }

    // special case for horizontal and vertical lines...
    // this will speed up things a bit in these cases
    if(deltaX == 0) {
        for(counter = deltaY; counter > 0; counter--) {
            if(y1 >= 0 && y1 <= height) {
                buffer[y1 * width * 3 + x1 * 3] = color;
                buffer[y1 * width * 3 + x1 * 3 + 1] = color;
                buffer[y1 * width * 3 + x1 * 3 + 2] = color;
            }
            y1 += addY;
        }
        return;
    }
    // horizontal line
    if(deltaY == 0) {
        for(counter = deltaX; counter > 0; counter--) {
            if(x1 <= width && x1 >= 0) {
                buffer[y1 * width * 3 + x1 * 3] = color;
                buffer[y1 * width * 3 + x1 * 3 + 1] = color;
                buffer[y1 * width * 3 + x1 * 3 + 2] = color;
            }
            x1 += addX;
        }
        return;
    }
    if(deltaX > deltaY) {
        // calc initial error-value
        error = deltaX >> 1;
        for(counter = deltaX; counter > 0; counter--) {
            // adjust error-value
            error += deltaY;
            // set pixel with clipping
            if(x1 <= width && x1 >= 0 && y1 <= height && y1 >= 0) {
                buffer[y1 * width * 3 + x1 * 3] = color;
                buffer[y1 * width * 3 + x1 * 3 + 1] = color;
                buffer[y1 * width * 3 + x1 * 3 + 2] = color;
            }
            // if error is out of range, go one line further up or down
            if(error >= deltaX) {
                error -= deltaX;
                y1 += addY;
            }
            x1 += addX;
        }
    }
    else {
        // calc initial error-value
        error = deltaY >> 1;
        for(counter = deltaY; counter >= 0; counter--) {
            // adjust error-value
            error += deltaX;
            // set pixel with clipping
            if(x1 <= width && x1 >= 0 && y1 <= height && y1 >= 0) {
                buffer[y1 * width * 3 + x1 * 3] = color;
                buffer[y1 * width * 3 + x1 * 3 + 1] = color;
                buffer[y1 * width * 3 + x1 * 3 + 2] = color;
            }
            // if error is out of range, go one line further left or right
            if(error >= deltaY) {
                error -= deltaY;
                x1 += addX;
            }
            y1 += addY;
        }
    }
}

/**
 * draw inverse line
 */
void g_drawLine_invert(unsigned char *buffer, int width, int height,
              register int x1, register int y1,
              register int x2, register int y2)
{
    register int deltaX, deltaY;
    register int addX, addY;
    register int counter, error;
    // default step right 
    addX = 1;
    // ...and down
    addY = 1;
    // first calculate delta values
    deltaX = x2 - x1;
    deltaY = y2 - y1;
    // check for negative delta-value
    if(deltaX < 0) {
        addX = -addX;
        deltaX = -deltaX;
    }
    if(deltaY < 0) {
        addY = -addY;
        deltaY = -deltaY;
    }

    // special case for horizontal and vertical lines...
    // this will speed up things a bit in these cases
    if(deltaX == 0) {
        for(counter = deltaY; counter > 0; counter--) {
            if(y1 >= 0 && y1 <= height) {
                buffer[y1 * width * 3 + x1 * 3] = 255-buffer[y1 * width * 3 + x1 * 3];
                buffer[y1 * width * 3 + x1 * 3 + 1] = 255-buffer[y1 * width * 3 + x1 * 3 + 1];
                buffer[y1 * width * 3 + x1 * 3 + 2] = 255-buffer[y1 * width * 3 + x1 * 3 + 2];
            }
            y1 += addY;
        }
        return;
    }
    // horizontal line
    if(deltaY == 0) {
        for(counter = deltaX; counter > 0; counter--) {
            if(x1 <= width && x1 >= 0) {
                buffer[y1 * width * 3 + x1 * 3] = 255-buffer[y1 * width * 3 + x1 * 3];
                buffer[y1 * width * 3 + x1 * 3 + 1] = 255-buffer[y1 * width * 3 + x1 * 3 + 1];
                buffer[y1 * width * 3 + x1 * 3 + 2] = 255-buffer[y1 * width * 3 + x1 * 3 + 2];
            }
            x1 += addX;
        }
        return;
    }
    if(deltaX > deltaY) {
        // calc initial error-value
        error = deltaX >> 1;
        for(counter = deltaX; counter > 0; counter--) {
            // adjust error-value
            error += deltaY;
            // set pixel with clipping
            if(x1 <= width && x1 >= 0 && y1 <= height && y1 >= 0) {
                buffer[y1 * width * 3 + x1 * 3] = 255-buffer[y1 * width * 3 + x1 * 3];
                buffer[y1 * width * 3 + x1 * 3 + 1] = 255-buffer[y1 * width * 3 + x1 * 3 + 1];
                buffer[y1 * width * 3 + x1 * 3 + 2] = 255-buffer[y1 * width * 3 + x1 * 3 + 2];
            }
            // if error is out of range, go one line further up or down
            if(error >= deltaX) {
                error -= deltaX;
                y1 += addY;
            }
            x1 += addX;
        }
    }
    else {
        // calc initial error-value
        error = deltaY >> 1;
        for(counter = deltaY; counter >= 0; counter--) {
            // adjust error-value
            error += deltaX;
            // set pixel with clipping
            if(x1 <= width && x1 >= 0 && y1 <= height && y1 >= 0) {
                buffer[y1 * width * 3 + x1 * 3] = 255-buffer[y1 * width * 3 + x1 * 3];
                buffer[y1 * width * 3 + x1 * 3 + 1] = 255-buffer[y1 * width * 3 + x1 * 3 + 1];
                buffer[y1 * width * 3 + x1 * 3 + 2] = 255-buffer[y1 * width * 3 + x1 * 3 + 2];
            }
            // if error is out of range, go one line further left or right
            if(error >= deltaY) {
                error -= deltaY;
                x1 += addX;
            }
            y1 += addY;
        }
    }
}


/**
 * find right border of a blob
 */
static int find_border(unsigned char *data, int width, unsigned char byte)
{
    int x;
    for(x = 0; x < width * 3; x += 3)
        if(data[x] == byte && data[x + 1] == byte && data[x + 2] == byte)
            break;
    if(x == width * 3)
        return -1;
    else
        return x / 3;
}

/**
 * draw a box
 */
void g_drawBox(unsigned char *data, int pix_width, int x, int y,
              int width, int height)
{
    unsigned char *tmp;
    int count;
    /*
     * upper border 
     */
    tmp = &data[y * 3 * pix_width + (x * 3)];
    for(count = 0; count < width; count++) {
        tmp[count * 3] = 0xFF;
        tmp[(count + 1) * 3] = 0xFF;
    }
    /*
     * lower border 
     */
    tmp = &data[(y + height) * pix_width * 3 + (x * 3)];
    for(count = 0; count <= width; count++) {
        tmp[count * 3] = 0xFF;
        tmp[(count - 1) * 3] = 0xFF;
    }
    /*
     * left border 
     */
    tmp = &data[y * pix_width * 3 + (x * 3)];
    for(count = 0; count < height; count++) {
        tmp[count * pix_width * 3] = 0xFF;
        tmp[count * pix_width * 3 + 3] = 0xFF;
    }
    /*
     * right border 
     */
    tmp = &data[y * 3 * pix_width + (x + width) * 3];
    for(count = 0; count < height; count++) {
        tmp[count * pix_width * 3] = 0xFF;
        tmp[count * pix_width * 3 - 3] = 0xFF;
    }
}

/**
 * draw inverted box
 */
void g_drawBox_invert(unsigned char *data, int pix_width, int x, int y,
              int width, int height)
{
    unsigned char *tmp;
    int count;
    /*
     * upper border 
     */
    tmp = &data[y * 3 * pix_width + (x * 3)];
    for(count = 0; count < width; count++) {
        tmp[count * 3] = 255-tmp[count * 3];
        tmp[count * 3+1] = 255-tmp[count * 3+1];
        tmp[count * 3+2] = 255-tmp[count * 3+2];
    }
    /*
     * lower border 
     */
    tmp = &data[(y + height) * pix_width * 3 + (x * 3)];
    for(count = 0; count <= width; count++) {
        tmp[count * 3] = 255-tmp[count * 3];
        tmp[count * 3+1] = 255-tmp[count * 3+1];
        tmp[count * 3+2] = 255-tmp[count * 3+2];
    }
    /*
     * left border 
     */
    tmp = &data[y * pix_width * 3 + (x * 3)];
    for(count = 0; count < height; count++) {
        tmp[count * pix_width * 3] = 0xFF-tmp[count * pix_width * 3];
        tmp[count * pix_width * 3+1] = 0xFF-tmp[count * pix_width * 3+1];
        tmp[count * pix_width * 3+2] = 0xFF-tmp[count * pix_width * 3+2];
    }
    /*
     * right border 
     */
    tmp = &data[y * 3 * pix_width + (x + width) * 3];
    for(count = 0; count < height; count++) {
        tmp[count * pix_width * 3] = 0xFF-tmp[count * pix_width * 3];
        tmp[count * pix_width * 3+1] = 0xFF-tmp[count * pix_width * 3+1];
        tmp[count * pix_width * 3+2] = 0xFF-tmp[count * pix_width * 3+2];
    }
}

/**
 * find the middle of a found blob
 */
static void find_spot_middle(unsigned char *data, int y, int width, int height)
{

    int line, tmp_left, tmp_right, b_x, b_y, b_x2, b_height;
    b_x2 = b_x = b_y = 0;
    for(line = y; line < height; line++) {
        tmp_left = find_border(&data[width * 3 * line], width, 0xFF);
        if(tmp_left < 0)
            break;
        if(tmp_left < b_x || b_x == 0)
            b_x = tmp_left;
        tmp_right =
            find_border(&data[width * 3 * line + b_x * 3], width - tmp_left,
                        0x00) + tmp_left;
        if(tmp_right > b_x2)
            b_x2 = tmp_right;
        /*
         * no right border found 
         */
        if(tmp_right < 0)
            break;
    }

    b_y = y;
    b_height = line - y;
    if(b_y + b_height >= height)
        b_height = (height - b_y) - 1;
    if(b_x2 >= width)
        b_x2 = width - 1;
    
    
    
    /*
     * size threshold 
     */
    if((b_x2 - b_x) > SPOT_MIN_WIDTH && b_height > SPOT_MIN_HEIGHT) 
    {
        
        //g_print("X: %d, Y: %d, Width: %d, Height: %d\n", b_x, b_y, b_x2-b_x, b_height);
        //g_print("X: %d, Y: %d\n", b_x+(b_x2-b_x)/2, b_y+b_height/2);
        
        /*
         * draw red box 
         */
        g_drawBox(data, width, b_x, b_y, b_x2 - b_x, b_height);
           
        /*
         * save coords 
         */
        sensor_get()->x = b_x + (b_x2 - b_x) / 2;
        sensor_get()->y = b_y + b_height / 2;
    }
    else {
        sensor_get()->x = 0;
        sensor_get()->y = 0;
    }
}



/**
 * find X/Y position of laser
 */
void g_track_xy(unsigned char *data)
{
#define TREMBLE 0

    int line;
    int old_x, old_y;
    int width, height, screen_width, screen_height;
    width = src_get()->width;
    height = src_get()->height;
    screen_width = sensor_get()->out_width;
    screen_height = sensor_get()->out_height;
    old_x = sensor_get()->x;
    old_y = sensor_get()->y;
    
    
    for(line = 0; line < height; line++) 
    {
        
        /*
         * find first white pixel 
         */
        if(find_border(&data[width * 3 * line], width, 0xFF) > 0) 
        {
                
            find_spot_middle(data, line, width, height);
                
            /*
             * respect spot-size threshold 
             */
            if(sensor_get()->x != 0
               && sensor_get()->y != 0) 
            {
                   
                /*
                 * adjust coords 
                 */
                sensor_get()->x =
                    (int) ((double) sensor_get()->x /
                           (double)width * (double)screen_width);
                sensor_get()->y =
                    (int) ((double) sensor_get()->y /
                           (double)height * (double)screen_height);
                
                /*
                 * flatten movement 
                 */
                if((sensor_get()->x > old_x + TREMBLE
                    || sensor_get()->x <
                    old_x - TREMBLE) &&(sensor_get()->y >
                                        old_y + TREMBLE
                                        || sensor_get()->y <
                                        old_y - TREMBLE)) 
                {
                    
                    if(sensor_get()->ondelay <= src_get_framecount())
                    {
                        
                        if(src_get_laserDetected() == FALSE &&
                            sensor_get()->repeat_delay <= src_get_framecount())
                        {

                            src_set_framecount(0);
                            src_set_laserDetected(TRUE);
                        }
                        
                        
                    }     

                    
                    
                }
                

            }
            
            goto txy_end;
        }
        

    }
    
    sensor_get()->x = old_x;
    sensor_get()->y = old_y;
    
    if(sensor_get()->offdelay <= src_get_framecount())
    {
        
        
        if(src_get_laserDetected() == TRUE)
        {
            src_set_framecount(0);
            src_set_laserDetected(FALSE);
        }
        
    }
    
    

txy_end:
    return;
}
