/* 
    Robot Simulator

    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.

    (C) 2006 Jason Hunt
    nulluser@gmail.gom 
*/


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


#include "utility.h"
#include "console.h"
#include "graphics.h"
#include "automode.h"
#include "robot.h"
#include "world.h"
#include "main.h"
#include "map.h"

extern world_type world;
extern bool mode_manual;

double max_animal_speed = 0.25;  // The animals cal move no faster than this


robot_type robot;


/* Draw line in world cords */
void draw_line(HDC dc, line l)
{   
    MoveToEx(dc, get_screen_x(l.x1), get_screen_y(l.y1), NULL);
    LineTo(dc, get_screen_x(l.x2), get_screen_y(l.y2));    
}
/* End of draw line */



/* Draw a sensor for an animal */
void draw_sensor(HDC dc, double x, double y, double dir, double dist, double size, COLORREF c)
{
    double sx = x + dist * cos(dir);
    double sy = y + dist * sin(dir);
    
    RECT r;
    
    r.left = get_screen_x(sx - size / 2.0);
    r.top = get_screen_y(sy - size / 2.0);
    r.right = get_screen_x(sx + size / 2.0);
    r.bottom = get_screen_y(sy + size / 2.0);
    
    HBRUSH b = CreateSolidBrush(c);

    HBRUSH hold = (HBRUSH) SelectObject(dc, b);

    Ellipse(dc, r.left, r.top, r.right, r.bottom);    
    
    SelectObject(dc, hold);    
        
    DeleteObject(b);
}
/* End of draw sensor */


/* Decode the sensor into a color */
COLORREF get_sensor_color( bool val )
{
    if (val) return(RGB(255, 0, 0));     // Red for well

    return(RGB(255, 255, 255));                 // No hit, return white
}
/* End of get_sensor_color */


/* Draw an animal to a device context */
void robot_draw(HDC dc)
{
    
    SelectObject(dc, (HPEN)world.black_pen);    
    RECT r;
    get_bounding_rect(robot.x, robot.y, robot.size, robot.size, r);

    SelectObject(dc, (HBRUSH)world.green_brush);

    // Draw robot body
    Ellipse(dc, r.left, r.top, r.right, r.bottom);

    SelectObject(dc, (HBRUSH)world.white_brush);

    // Get distance from center and size of the sensors
    double size = robot.sensor_size;
    double dist = robot.sensor_dist;

    // Draw the direction arrow
    draw_arrow(dc, robot.x, robot.y, robot.dir, size);
 
 
 
    // Draw each sensor
    // The sensor index increases in a clockwise direction

    double angle = - M_PI / 3.0;
    double delta = 2 * M_PI / 6.0;

    for (int i = 0; i < 6; i++, angle += delta)
        draw_sensor(dc,  robot.x, robot.y, robot.dir + angle, dist, size, 
                       get_sensor_color(robot.touch[i]));
                       
                       
  /*  for (int i = 0; i < robot.num_vision_points * 2; i++)
    {
        if (robot.vision[i/ 2].food > 0)
        {

            MoveToEx(dc, get_screen_x(robot.x),get_screen_y(robot.y),NULL);
            LineTo(dc, get_screen_x(robot.end_x[i]),get_screen_y(robot.end_y[i]));
        }
    }*/
        
        
    if (robot.draw_vision)
    // Draw vision lines
    for (int i = 0; i < 6; i++)
        if (robot.wall_found[i])
        {
            line l;
            l.x1 = robot.x;
            l.y1 = robot.y;
            l.x2 = robot.wall_points[i].x;
            l.y2 =robot.wall_points[i].y;
        
            SelectObject(dc, (HPEN)world.green_pen);
    
            draw_line(dc, l);
        }        
                
                
    /// Draw nav vector                
 
    double mag = robot.nav_mag;
    
    if (mag > 0.5) mag = 0.5;
 
   SelectObject(dc, (HPEN)world.blue_pen); 
    draw_arrow(dc, robot.x, robot.y, robot.nav_dir, 0.1);
                
                                
}
/* End of draw sensors */



/* Check the spac that the sensor tip occupies, and set values to match */
bool check_sensor(double x, double y, double dir, double dist) 
{
    // Determine location of sensor tip
    double sx = x + dist * cos(dir);
    double sy = y + dist * sin(dir);
    
    if (world_check_wall_point(sx, sy)) 
    {
        //world_add_impact(sx, sy);
        return(true);        
    }

    if (sx < 0 || sx > world.x_size) 
    {
      //  world_add_impact(sx, sy);
        return(true);
    }

    if (sy < 0 || sy > world.y_size)
    { 
      // world_add_impact(sx, sy);
        return(true);
    }
    
    return(false);
}
/* End of check sensor */





/* compute intersection between two lines */
bool check_intersect(line line1, line line2, point &i)
{
    // Constants for line 1
    // a1*x + b1*y + c1 = 0 
    double a1 = line1.y2 - line1.y1;
    double b1 = line1.x1 - line1.x2;
    double c1 = line1.x2*line1.y1 - line1.x1*line1.y2;  

    // Constants for line 2
    // a2*x + b2*y + c2 = 0 
    double a2 = line2.y2 - line2.y1;
    double b2 = line2.x1 - line2.x2;
    double c2 = line2.x2*line2.y1 - line2.x1*line2.y2;  

    // Descriment
    double  d = a1*b2 - a2*b1;
    
    if (d == 0) return(false);

    i.x = (b1*c2 - b2*c1)/d;
    i.y = (a2*c1 - a1*c2)/d;

    if (line1.x1 != line1.x2 && i.x < line1.x1 && i.x < line1.x2) return(false);
    if (line1.x1 != line1.x2 && i.x > line1.x1 && i.x > line1.x2) return(false);

    if (line1.y1 != line1.y2 && i.y < line1.y1 && i.y < line1.y2) return(false);
    if (line1.y1 != line1.y2 && i.y > line1.y1 && i.y > line1.y2) return(false);

    if (line2.x1 != line2.x2 && i.x < line2.x1 && i.x < line2.x2) return(false);
    if (line2.x1 != line2.x2 && i.x > line2.x1 && i.x > line2.x2) return(false);

    if (line2.y1 != line2.y2 && i.y < line2.y1 && i.y < line2.y2) return(false);
    if (line2.y1 != line2.y2 && i.y > line2.y1 && i.y > line2.y2) return(false);

    return(true);    
}
/* End of check_intersect */


/* Check intersection between line and box */
double check_box_intersect(line main, line box, point &intersect)
{
    line test_line;
    
    bool hit[4] = {false};
    point points[4];
    double dist[4] = {MAX_IR_DIST, MAX_IR_DIST, MAX_IR_DIST, MAX_IR_DIST};
        
    // Check upper line
    test_line.x1 = box.x1;   test_line.y1 = box.y1;
    test_line.x2 = box.x2;   test_line.y2 = box.y1;

    if (check_intersect(main, test_line, points[0]))
    {
        dist[0] = sqrt((points[0].x - main.x1) * (points[0].x - main.x1)+   
                       (points[0].y - main.y1) * (points[0].y - main.y1)); 

    }

    // Check lower line
    test_line.x1 = box.x1;   test_line.y1 = box.y2;
    test_line.x2 = box.x2;   test_line.y2 = box.y2;

    if (check_intersect(main, test_line, points[1]))
        dist[1] = sqrt((points[1].x - main.x1) * (points[1].x - main.x1)+   
                       (points[1].y - main.y1) * (points[1].y - main.y1));   

    // Check left line
    test_line.x1 = box.x1;   test_line.y1 = box.y1;
    test_line.x2 = box.x1;   test_line.y2 = box.y2;

    if (check_intersect(main, test_line, points[2]))
        dist[2] = sqrt((points[2].x - main.x1) * (points[2].x - main.x1)+   
                       (points[2].y - main.y1) * (points[2].y - main.y1));   
    // Check right line
    test_line.x1 = box.x2;   test_line.y1 = box.y1;
    test_line.x2 = box.x2;   test_line.y2 = box.y2;

    if (check_intersect(main, test_line, points[3]))
        dist[3] = sqrt((points[3].x - main.x1) * (points[3].x - main.x1)+   
                       (points[3].y - main.y1) * (points[3].y - main.y1));   


    // Calculate and return distance to closest line in box
    double smallest_dist = world.x_size + world.y_size;
    int smallest_index = 0;
    
    char b[400];
    
    for (int i = 0; i < 4; i++)
    {
        if (dist[i] > 0 && dist[i] <= smallest_dist)
        {
            smallest_dist = dist[i];
            smallest_index = i;
        }
    }
   
    if (dist[smallest_index] < 1)
   {
        
            world_add_impact(points[smallest_index].x, points[smallest_index].y);
    }
   
    intersect = points[smallest_index];    
    


    return(dist[smallest_index]);
}



/* Check the space that the sensor tip occupies, and set values to match */
double check_vision(double x, double y, double dir, bool &found, point &p) 
{
    // Determine location of sensor tip
    double max_size = world.x_size * world.y_size;

    line main_line;
    
    main_line.x1 = x;
    main_line.y1 = y;
    
    main_line.x2 = x + max_size * cos(dir);
    main_line.y2 = y + max_size * sin(dir);
        
    wall_type *w = world.walls;    
   
    found = false;
   
    point points[world.num_walls];
    double dist[world.num_walls];

    
    point intersect;
    line box;    
    
    unsigned int i = 0;
    
    // Check against all walls
    while (w != NULL)
    {
        // The wall is checked using a line point to opposing corners
        box.x1 = w->x;
        box.y1 = w->y;
        
        box.x2 = w->x + w->x_size;
        box.y2 = w->y + w->y_size;
            
            
        dist[i] = check_box_intersect(main_line, box, intersect);
            
       // if (dist[i] > MAX_IR_DIST || dist[i] == -1)
//        dist[i] = MAX_IR_DIST;
            
        // check Intersection
        if  (dist[i]  > 0 && dist[i]  < MAX_IR_DIST)
        {
            points[i] = intersect;

            p = intersect;
            found = true;            
        }

        w=w->next;
        
        i++;
    }
    
    
      // Calculate and return distance */
    double smallest_dist = world.x_size + world.y_size;
    int smallest_index = -1;
    
    for (i = 0; i < world.num_walls; i++)
    {
        if (dist[i] > 0 && dist[i] <= smallest_dist)
        {
            smallest_dist = dist[i];
            smallest_index = i;
        }
    }
  
  
  if (smallest_index == -1) return(-1);
  
    p = points[smallest_index];

         //   world_add_impact(points[smallest_index].x, points[smallest_index].y);    
    
    return(dist[smallest_index]);

}
/* End of check sensor */



/* Update all values for the aminal */
void robot_update(void)
{
    double x_diff = robot.x - world.target_x;
    double y_diff = robot.y - world.target_y;
    
    double target_dist = sqrt(x_diff * x_diff + y_diff * y_diff);
    
    if (target_dist < 0.3)
    {
        world_set_target(f_rand(1, world.x_size-1), f_rand(1, world.y_size-1));
    }
    
    
    
    compute_grad_point(robot.x, robot.y, robot.nav_dir, robot.nav_mag);


    point v;
    
    compute_waypoint_vector(robot.x, robot.y, v);

    // Compute distance between waypoints
    double waypoint_seperation =  robot.size;

    double dist = sqrt(v.x * v.x + v.y * v.y);


//    char b[400];    
//    sprintf(b, "Cur waypoint: %d  Dist: %f  sep_dist: %f \n", world.cur_waypoint, dist, waypoint_seperation);
//    write_console(b);
    static double last_waypoint_time = 0;

    // Point to next waypoint if close enough
    if (dist < waypoint_seperation && world.cur_waypoint > 0)
    {
        last_waypoint_time = get_time();
          world.cur_waypoint--;
    }


    if (get_time() - last_waypoint_time  > 1.0)
        world_compute_map_grid();

    
    double d = 0.5;


    // Read all sensors    
    
    // Claculate distance to tip of sensor
     dist = robot.sensor_dist + robot.sensor_size / 2.0;    

    // Calculate angle and delta for sensor check
    double angle = - M_PI / 3.0;
    double delta = 2 * M_PI / 6.0;

    // Check the sensors moving clockwise around the animal
    for (int i = 0; i < 6; i++, angle += delta)
    {
        robot.touch[i] = check_sensor(robot.x, robot.y, robot.dir + angle, dist);
        
        robot.ir[i] =  MAX_IR_DIST - check_vision(robot.x, robot.y, robot.dir + angle, robot.wall_found[i], robot.wall_points[i]);
    }


  
    //robot.nav_dir = atan2(sum_y, sum_x);

    
    double nav_off = robot.nav_dir - robot.dir;
    
//    if (nav_off > M_PI) nav_off -= 2*M_PI;
  
    if (nav_off > 2*M_PI) nav_off -= 2*M_PI;
    if (nav_off < 0) nav_off += 2*M_PI;
    
    if (nav_off > M_PI) nav_off = nav_off - 2*M_PI;
    
        
    robot.nav_off = nav_off;
  
    
    //char b[400];
    //sprintf(b, "nav mag  %f \n",  robot.nav_mag);
  //  write_console(b);




//    angle = - M_PI / 5.0;
//    delta = M_PI / 5.0;    

   
//    check_vision(robot.x, robot.y, robot.dir, robot.num_vision_points, robot.vision,
//        robot.end_x, robot.end_y);

    if (!manual_mode)
        robot_auto_mode();





    // Direction change rate is the difference of drive speeds
  /*  a->ddir = a->l_drive - a->r_drive;

    // Speed is the sum of drive speeds
    a->speed = (a->l_drive + a->r_drive) / 2.0;    
*/
    // Add delta to direction
    robot.dir += robot.ddir;

    if (robot.dir > M_PI ) robot.dir -= 2*M_PI; 
    if (robot.dir < -M_PI ) robot.dir += 2*M_PI; 


//    if (robot.dir < 0 ) robot.dir += 2*M_PI;



    // Calulate x and y deltas
    robot.dx = robot.speed * cos(robot.dir);
    robot.dy = robot.speed * sin(robot.dir);


    double test_x = robot.x + robot.dx;
    double test_y = robot.y + robot.dy;



    point p;
    bool found;

    bool hit = false;


    unsigned int check_points = 12;

    // Calculate angle and delta for sensor check
     angle = 0;
    delta =  (2 * M_PI) / (double)check_points;

   // Check the sensors moving clockwise around the animal
    for (int i = 0; i < check_points; i++, angle += delta)
    { 
        if ((d = check_vision(test_x, test_y, robot.dir + angle, found, p)) < robot.size/2.0)
        {
            robot.dx -= (robot.size/2.0 - d) * cos(robot.dir + angle);
            robot.dy -= (robot.size/2.0 - d) * sin(robot.dir + angle);    
        } 
    }
    
   // } hit = true;
    
/*    if (check_vision(test_x, test_y, robot.dir + M_PI/4.0, found, p) < 0.15) hit = true;
    if (check_vision(test_x, test_y, robot.dir - M_PI/4.0, found, p) < 0.15) hit = true;

    if (check_vision(test_x, test_y, robot.dir + M_PI, found, p) < 0.15) hit = true;
    if (check_vision(test_x, test_y, robot.dir + 3*M_PI/4.0, found, p) < 0.15) hit = true;
    if (check_vision(test_x, test_y, robot.dir + 5*M_PI/4.0, found, p) < 0.15) hit = true;*/

    
    //if (!hit)
    {
        // Add deltas to positon
        robot.x += robot.dx;
        robot.y += robot.dy;
    }


        // Check collision with outer walls        
        if (robot.x < robot.x_size/2)
        {            
            robot.x = robot.x_size/2;
          //  robot.wall_hit++;    
        }
        
        if (robot.x > world.x_size - robot.x_size/2)
        {
            robot.x = world.x_size - robot.x_size/2;
          //  robot.wall_hit++;        
        }
            
        if (robot.y < robot.y_size/2)
        {
            robot.y = robot.y_size/2;
           // robot.wall_hit++;                    
        }
            
        if (robot.y > world.y_size - robot.y_size/2)
        {
            robot.y = world.y_size - robot.y_size/2;
           // robot.wall_hit++;        
        }
 
    
    static double last_x = 0;
    static double last_y = 0;
    
    if ((last_x-robot.x) * (last_x - robot.x) +
        (last_y-robot.y) * (last_y - robot.y) > (robot.size * robot.size)/64.0)
    {
    last_x = robot.x;
    last_y = robot.y;
        
    world_add_marker(robot.x , robot.y );
    }    
        
    
    
    
    
}


/* Manually Set motions for the first anaimal */
void robot_motion(double l, double r)
{
    robot.l_drive = l;
    robot.r_drive = r;
}
/* End of animal_motion */


/* Setup an animal for the first time */
void robot_init( void )
{
    robot.size = 0.5;
   
    robot.x_size = robot.size;
    robot.y_size = robot.size;
    
    robot.num_vision_points = 5;
    robot.num_touch_points = 6;
      
    robot.sensor_dist = (robot.x_size + robot.y_size) / 4.0;
    robot.sensor_size = (robot.x_size + robot.y_size) / 8.0;    
                                

    robot.x = f_rand(robot.x_size, world.x_size - robot.x_size);
    robot.y = f_rand(robot.y_size, world.y_size - robot.y_size);

    robot.dir = f_rand(0, 2*M_PI);

    robot.r_drive = 0;
    robot.l_drive = 0;
                    
                                
    // Clear touch sensor inputs
    for (int i = 0; i < 6; i++)    
        robot.touch[i] = 0;

    // Clear vision inputs    
  /*  for (int i = 0; i < robot.num_vision_points; i++)    
    {
        // Clear vision vectors
        robot.end_x[2*i] = robot.x;
        robot.end_y[2*i] = robot.y;
        
        robot.end_x[2*i+1] = robot.x;
        robot.end_y[2*i+1] = robot.y;
    }  */
    

    for (int i = 0; i < 6; i++)
       robot.wall_found[i] = false;
       
       
    robot.draw_vision = false;
}
/* End of animat_init */






