/*
    ANTLand simulation
    (C) 2004 justin_sane

    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: justinsanemsu@msu.edu

    FILE: antunit.cpp
    Authors: justin_sane
*/

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

#pragma hdrstop

#include "world.h"
#include "ant.h"
#include "draw.h"
#include "utility.h"
#include "collide.h"
#include "network.h"

#pragma package(smart_init)


extern world_class world;

/* Adds a random ant to the simulation */
ant_class::ant_class(  vector<obstacle_class> &obstacles, vector<ant_class *> &ants )
{
    // setup bogus directions, so it will find a random place to land
    pos.x = -1;
    pos.y = -1;
    y_dir = 0;
    dir = 0;

    //look for a landing site
    while (!okay_to_land(pos, dir, obstacles))
    {
        pos.x = rvalplus(sim_x_size);   // random x
        pos.y = rvalplus(sim_y_size);   // random y
        dir = rand() % 360;             // direction
    }

    // clear movement
    forward = 0;
    turn = 0;

    // create the default sensors
    setup_sensors();

    // default is automatic control
    self_control = 1;

    // clear learning counts
    stuck_count = 0;
    pain_count = 0;
    mutate_count = 0;

    forward = f_speed;

    // not selected
    selected = false;

    // fill the last_move buffer
    memset(last_moves, 1, last_move_size);

    // configure the inital neural network
    setup_network();

    // ensure sane vectors
    update_vectors();

    // get inital sensor values
    update_sensor_values(obstacles, ants);

}
/* end of random ant */


/* decontructor for ant */
ant_class::~ant_class(void)
{
    net.inputs.clear();
    net.nodes.clear();
    net.outputs.clear();
};


void __fastcall ant_class::setup_sensors( void )
{
    for (int i = 0; i < 6; i++)
    {
        int ang = (i * 20) - 50; // crates {-50, -30, -10, 10, 30, 50}

        f_warning[i].set(4, ang, clGreen);
        f_pain[i].set(3, ang, clRed);

        r_warning[i].set(-4, ang, clGreen);
        r_pain[i].set(-3, ang, clRed);
    }
}



void __fastcall ant_class::get_stat(char *stat)
{
    sprintf(stat, "Pos: (%3.2f, %3.2f, %3.0f) " \
                  "Forward: %1.4f Turn: %1.4f " \
                  "Stuck: %d Pain %d Mutate: %d",
                   pos.x, pos.y, dir,
                   forward, turn,
                   stuck_count, pain_count,  mutate_count);
}



/* toggles between manual and self control */
void __fastcall ant_class::toggle_self( void )
{
    if (self_control)
    {
        self_control = false;
        forward = 0;
        turn = 0;
    }
    else
    {
        self_control = true;
        stuck_count = 0;
        pain_count = 0;
        forward = 0.25;
    }
}
/* end of toggle self */



/* returns true id the point is inside of the ant */
bool __fastcall ant_class::point_inside( point p )
{
    return(point_in_box(bound_rear_left,
                        bound_forward_left,
                        bound_forward_right,
                        bound_rear_right,
                        p.x, p.y));
}
/*end of point inside */


/* probe the sim area are update the sensor variables */
void __fastcall ant_class::update_sensor_values( vector<obstacle_class> &obstacles, vector<ant_class *> &ants  )
{
    // see if the sensor vectors are valid
//    if (!data_valid) return;

    // update the sensors from the current grid
    for (int i = 0; i < 6; i++)
    {
        f_warning[i].update_value(obstacles, ants);
        f_pain[i].update_value(obstacles, ants);

        r_warning[i].update_value(obstacles, ants);
        r_pain[i].update_value(obstacles, ants);
    }
}
/* end of update_sensors */




/* this takes the forward and turn variables and applies them to the ant */
void __fastcall ant_class::update_vectors( void )
{
    // calculate the new bounding box
    bound_forward_right = forward_vector(pos, 2.5, dir - 25);
    bound_forward_left = forward_vector(pos, 2.5, dir + 25);
    bound_rear_right = forward_vector(pos, -2.5, dir + 25);
    bound_rear_left = forward_vector(pos, -2.5, dir - 25);

    // calculate the arrow vectors
    arrow_front = forward_vector(pos, 2, dir);
    arrow_rear =  forward_vector(pos, -2, dir);
    arrow_left =  forward_vector(arrow_front, 2, dir + 150);
    arrow_right = forward_vector(arrow_front, 2, dir - 150);

    // update the sensor vectors (front)
    for (int i = 0; i < 6; i++)
    {
        f_warning[i].update_vector(pos, dir);
        f_pain[i].update_vector(pos, dir);

        r_warning[i].update_vector(pos, dir);
        r_pain[i].update_vector(pos, dir);
    }
}
/* end of update vectors */



/* mutates node if we havent move far enough */
void __fastcall ant_class::check_dead( void )
{
    if (stuck_count > 3) // did not more for 3 cycles
    {
        stuck_count = 0;
        mutate_node();
    }
}
/* end of check_dead */


/* mutate a node if we have triggeret the pain sensor */
void  __fastcall  ant_class::check_pain( void )
{
    int pain_found = 0;

    for (int i = 0; i < 6; i++)
        if (f_pain[i].get_value() || r_pain[i].get_value())
        pain_found = 1;

    if (pain_found)
        pain_count++;
    else
        pain_count = 0;

    if (pain_count > 3) // did not more for 3 cycles
    {
        mutate_node();
        pain_count = 0;
    }
}
/* end of chech_pain */


/* update the occilation check buffer */
float __fastcall ant_class::next_move_check(float m)
{
    float sum = 0;

    for (int i = 0; i < last_move_size - 1; i++)
    {
        last_moves[i] = last_moves[i+1]; // move the data forward
        sum += last_moves[i];
    }

    sum += m;
    sum /= last_move_size;

    last_moves[last_move_size - 1] = m;

    return(sum);
}
/* end of nex_more_check */




/* this takes the forward and turn variables and applies them to the ant */
void __fastcall ant_class::update_movement(vector<obstacle_class> &obstacles )
{
    double next_direction = dir + turn;
    point next = forward_vector(pos, forward, next_direction);

    float check = next_move_check(forward);

    if (okay_to_land(next, next_direction, obstacles))
    {
        if (fabs(check) > 0.2)  // account for jitter, or getting stuck
            stuck_count = 0;
        else
            stuck_count++;

        pos = next;
        dir = next_direction;
    }
    else
    {
        forward = 0;
        stuck_count++;
    }

    // wrap angle around
    if (dir > 360) dir -= 360;
    if (dir  < 0)  dir += 360;
}
/* end uf update movement */


/* update all aspect of the ant */
void __fastcall ant_class::update(vector<obstacle_class> &obstacles, vector<ant_class *> &ants)
{
    update_sensor_values(obstacles, ants);

    // force full forward proegation per update
    for (unsigned int i = 0; i < net_layers + 1; i++)
        process_network();

    update_movement(obstacles);

    update_trig_data();

    update_vectors();

    check_dead();
    check_pain();
}
/* end of update */



/* draw the entire ant */
void __fastcall ant_class::draw(Graphics::TBitmap *buffer)
{
    // draw the bounding box
    grid_box(bound_forward_right, bound_forward_left,
             bound_rear_left, bound_rear_right, buffer);

    // draw the arrow
    grid_line(arrow_rear, arrow_front, buffer, selected);
    grid_line(arrow_front, arrow_left, buffer, selected);
    grid_line(arrow_front, arrow_right, buffer, selected);

    // draw the sensors
    for (int i = 0; i < 6; i++)
    {
        f_warning[i].draw(buffer);
        f_pain[i].draw(buffer);

        r_warning[i].draw(buffer);
        r_pain[i].draw(buffer);
    }

    // draw the nerual network display
    if (selected)
        net.draw();
}
/* end of draw ant */




/*******************
** Sensor related **
*******************/


void sensor_class::update_vector( point ant_pos, int ant_dir )
{
    pos = forward_vector(ant_pos, dist, ant_dir + dir);  // far right
//    value = grid_area_blocked(pos);
}

void sensor_class::update_value(vector<obstacle_class> &obstacles, vector<ant_class *> &ants)
{
    value = grid_area_blocked(pos, obstacles, ants);
}

void sensor_class::set(float the_dist, int the_dir, TColor the_act_color)
{
    dist = the_dist; dir = the_dir;
    value = false;
    act_color = the_act_color;
}

void sensor_class::draw(Graphics::TBitmap *buffer)
{
    draw_sensor_dot(pos, value, act_color, buffer);
}


/**************************
** End of sensor related **
**************************/



void ant_class::move(move_type move)
{
    if (self_control) return;



    if (move == forw)

        debug("for \n");



    if (move == ang_up)
    {
        y_dir += 1;
        if (y_dir >= 30) y_dir = 30;
    } else
    if (move == ang_down)
    {
        y_dir -= 1;
        if (y_dir < -30) y_dir = -30;
    } else
    if (move == turn_left) turn = (2 + 2 * fabs(forward)); else
    if (move == turn_right) turn = -(2 + 2 * fabs(forward)); else
    if (move == forw)
    {
        if (forward < 0.10) forward = 0.10; else
            if (forward < 0.5) forward *= 1.05;
    } else
    if (move == back)
    {
        if (forward > -0.10) forward =- 0.10; else
            if (forward > -0.5) forward *= 1.05;
    } else
    if (move == stop_forw) forward /= 2.0; else
    if (move == stop_turn) turn = 0;
}



void ant_class::update_trig_data( void )
{
    trig_data.dir_cos = fast_cos(-dir);
    trig_data.dir_sin = fast_sin(-dir);

    trig_data.y_dir_cos = fast_cos(-y_dir);
    trig_data.y_dir_sin = fast_sin(-y_dir);
}



