/*
    ORPP Client

    (C) 2005 Jason Hunt

    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., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

    Contact: huntjas2@msu.edu

    FILE: target.cpp
    
    Simple target tracking system

*/


#include <windows.h>
#include <stdio.h>
#include <math.h>

#pragma hdrstop

#include "display.h"
#include "target.h"
#include "network.h"
#include "debug.h"

// buffer for image processing
color_type last_frame[max_image_x * max_image_y];

color_type processed_frame[max_image_x * max_image_y];


unsigned int update = 0;    // used to keep track of camera sweep

bool targeting = false;     // targeting enabled
bool d_frame = false;       // display difference frame

/* compute change between frames */
unsigned char get_change(unsigned int x, unsigned int y)
{
    unsigned int index = x + y * image_x;

    unsigned int change = (int)(fabs(camera_buffer[index].r - last_frame[index].r) + 
                                fabs(camera_buffer[index].g - last_frame[index].g) +
                                fabs(camera_buffer[index].b - last_frame[index].b)) / 3;

    if (change < 50) // 50 works
        change = 0;

    return((unsigned char )fabs(change));
}
/* end of get_change */
 

/* draw a box */
void draw_box(unsigned int x1, unsigned int y1, 
              unsigned int x2, unsigned int y2, 
              color_type *m)
{
    // range check
    if (x1 < 0) x1 = 0;
    if (x1 > image_x-1) x1 = image_x - 1;

    if (x2 < 0) x2 = 0;
    if (x2 > image_x-1) x2 = image_x - 1;

    if (y1 < 0) y1 = 0;
    if (y1 > image_y-1) y1 = image_y - 1;

    if (y2 < 0) y2 = 0;
    if (y2 > image_y-1) y2 = image_y - 1;
            
    for (unsigned int y = y1; y < y2 ; y++) 
    {
        unsigned int index = x1 + y * image_x;

        for (unsigned int x = x1; x < x2 ; x++)
        {
            m[index].r = 255;
            m[index].g = 0;
            m[index].b = 0;
            index++;
        }
    }
}
/* end of draw box */



/* update pan and tilt based on camera data */
void process_frame( void )
{
    unsigned int sum_x = 0, sum_y = 0, num = 0, index = 0;

    // compute centroid of difference frame
    // with weighted averages
    for (unsigned int y = 0; y < image_y; y++)
        for (unsigned int x = 0; x < image_x; x++)
        {
            unsigned int  v = get_change(x, y);

            if (d_frame) // if we need to display the difference frame
            {
                unsigned int tmp = v;
                if (tmp > 255) tmp = 255;

                processed_frame[index].g = (unsigned char)tmp;
                processed_frame[index].r = 0;        
                processed_frame[index].b = 0;

                index++;
            }

            sum_x += x*v;
            sum_y += y*v;
            num+=v;       
        }

    // waiting for camera to stabilize
    if (update++ < 10) 
    {
        // save the frame
        memcpy(last_frame, camera_buffer, image_x*image_y * sizeof(color_type));  
        
        if (d_frame)
            memcpy(camera_buffer, processed_frame, sizeof(color_type)*image_x*image_y);

        return;
    }

    // center of image
    unsigned int center_x, center_y;
    bool found = false;

    // make sure we have enough change to even bother moving
   if (num > 1500)
//    if (num > 10000)
    {
        center_x = sum_x / num;
        center_y = sum_y / num;
        found = true;
    } 
    else
    {
        center_x = image_x / 2;
        center_y = image_y / 2;
    }

    // display for debugging
//    char b[300];
//    sprintf(b, "num: %d ", num);
//    add_line(b);
    
    // done with image, save to old image
    memcpy(last_frame, camera_buffer, image_x*image_y * sizeof(color_type));   

    if (found)
    {
        unsigned int index = center_x;

        // draw lines
        for (unsigned int y = 0; y < image_y; y++, index+=image_x)
            camera_buffer[index].g = 255;        

        index = center_y * image_x;

        for (unsigned int x = 0; x < image_x; x++)
            camera_buffer[index++].g = 255;        


        if (center_x < image_x / 2 - 50) {add_line("move left"); send_value(0x62);} else
        if (center_x >  image_x / 2 + 50) {add_line("move right"); send_value(0x63);} else
        
        if (center_y < image_y / 2 - 50) {add_line("move down"); send_value(0x61);} else
        if (center_y >  image_y / 2 + 50) {add_line("move up"); send_value(0x60);} else
        
        
        
        {add_line("centered");
        send_value(0x64);}

        // draw targetting box
        draw_box(center_x-5,center_y-5, center_x+5, center_y+5, camera_buffer);
        
       // fire_on();
    }
    else
    {
      //  fire_off();
       send_value(0x64);   
    }    
        

 /*   int new_pan = pan;
    int diff = center_x - image_x/2;

    // calculate new servo offset
    new_pan -= (int)((diff / (double)(image_x / 2)) * 30);

    // range check        
    if (new_pan < 0) new_pan = 0;
    if (new_pan > 255) new_pan = 255;

    // tilt has not been tested, no tilt servo installed at the time
    int new_tilt = tilt;
    diff = center_y - image_y/2;

    // calculate new servo offset
    new_tilt -= (int)((diff / (double)(image_y / 2)) * 30);
        
    // range check
    if (new_tilt < 0) new_tilt = 0;
    if (new_tilt> 255) new_tilt = 255;

    // did any value change?
    if (new_pan != pan || new_tilt != tilt)
    {
        pan = new_pan;
        tilt = new_tilt;

        send_controller_values();
        update = 0;                 // we moved the camera, is has to stablize again
    }*/
        
    if (d_frame)
        memcpy(camera_buffer, processed_frame, sizeof(color_type)*image_x*image_y);
}
/* end of process frame */



