/* 
    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 <windows.h>
#include <math.h>
#include <stdio.h>

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

world_type world;       // All world data 


/* Inital world setup */
void world_setup(void )
{   
    // Size in world space
    world.x_size = 10;
    world.y_size = 10;

    world_destroy(); // free any existing world

    // Create brushs
    world.white_brush = CreateSolidBrush(RGB(255, 255, 255));
    world.black_brush = CreateSolidBrush(RGB(0, 0, 0));
    world.green_brush = CreateSolidBrush(RGB(0, 255, 0));
    world.blue_brush = CreateSolidBrush(RGB(0, 0, 255));
    world.red_brush = CreateSolidBrush(RGB(255, 0, 0));
    world.color1_brush = CreateSolidBrush(RGB(0, 128, 200));
    

    // Create pens
    world.white_pen = CreatePen(PS_SOLID, 1, RGB(255, 255, 255));
    world.black_pen = CreatePen(PS_SOLID, 1, RGB(0, 0, 0));
    world.green_pen = CreatePen(PS_SOLID, 1, RGB(0, 255, 0));
    world.blue_pen = CreatePen(PS_SOLID, 1, RGB(0, 0, 255));
    world.color1_pen = CreatePen(PS_SOLID, 1, RGB(0, 128, 200));

    // Run time tweakable values
    world.scale1 = 0.11;
    world.scale2 = 0.86;
    world.scale3 = 0.193;
        
 
    // Start showing walls
    world.show_walls = true;
 

    // No walls
    world.walls = NULL;
    world.num_walls = 0;

    // No markers
    world.markers = NULL;

    // No Impacts
    world.impacts = NULL;

    // No waypoints
    world.waypoints = NULL;
    world.num_waypoints = 0;
    world.cur_waypoint = 0;

    // Do not draw grad grid
    world.draw_grad = false;
    
    
    world.target_enabled = true;
    world.target_x = world.x_size /2.0;
    world.target_y = world.y_size /2.0;
    
    
    // Setup surronding walls
    
    double wall_width = 0.05;
    
    // top
    world_add_wall(0, 0, world.x_size, wall_width);

    // Bottom
    world_add_wall(0, world.y_size-wall_width, world.x_size, wall_width);
    
    // Left
    world_add_wall(0, 0, wall_width, world.y_size);    
    
    // Right
    world_add_wall(world.x_size-wall_width, 0, wall_width, world.y_size);
    

    // Setup cross walls in center
   // world_add_wall(world.x_size/2 - world.x_size/4 , world.y_size/2-wall_width/2, world.x_size/2, wall_width);
   // world_add_wall(world.x_size/2-wall_width/2, world.y_size/2 - world.y_size/4 , wall_width, world.y_size/2);

    // Setup robot
    robot_init();


    // Compute first map interation
    world_compute_map_grid();
  
}
/* End of void world_setup(void ) */


/* Free all world data */
void world_destroy( void )
{
    world.num_walls = 0;
    
    world_free_walls();    
    world_free_markers();
    world_free_impacts();
    world_free_waypoints();
    
    DeleteObject(world.black_brush);
    DeleteObject(world.white_brush);
    DeleteObject(world.green_brush);
    DeleteObject(world.blue_brush);
    DeleteObject(world.red_brush);
    DeleteObject(world.color1_brush);
    
    DeleteObject(world.white_pen);
    DeleteObject(world.black_pen);
    DeleteObject(world.green_pen);
    DeleteObject(world.blue_pen);
    DeleteObject(world.color1_pen);
    
}
/* End of world_destroy */



/* Draw entire world */
void world_draw( HDC dc )
{

    SelectObject(dc, world.white_pen);
    SelectObject(dc, world.white_brush);
      
    // Clear entire area
    Rectangle(dc, 0, 0, display_x, display_y);


//world_draw_grad(dc);


    if (world.draw_grad)
        world_draw_grad(dc);



//    if (world.draw_grad)
//         world_draw_map_grid(dc);


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

    world_draw_markers(dc);




    if (world.show_walls)
        world_draw_walls(dc);

    if (world.target_enabled)
    {
        double target_size = 0.15;

        SelectObject(dc, world.green_brush);        

        Ellipse(dc, get_screen_x(world.target_x - target_size/2), 
                    get_screen_y(world.target_y - target_size/2), 
                    get_screen_x(world.target_x + target_size/2), 
                    get_screen_y(world.target_y + target_size/2));
                
    }    


//   world_draw_impacts(dc);    

    world_draw_waypoints(dc);

    robot_draw(dc);
    
}
/* End of world_draw */


/* Update all world data */
void world_update( void )
{
 //   world_compute_map_grid();

    robot_update();
}
/* End of world_update */


    
/* Transform from world space to screen space */
int get_screen_x( double x )
{
    return((int)(x / (double) world.x_size * display_x));
}
/* End of get_screen_x */


/* Transform from world space to screen space */
int get_screen_y( double y )
{
    return((int)(y / (double) world.y_size * display_y));
}
/* End of get_screen_y */


/* Return world cords of a screen pos */
double get_world_x( unsigned int x)
{
    return((x / (double)display_x) * world.x_size);        
}
/* End of get_world_x */

/* Return world cords of a screen pos */
double get_world_y( unsigned int y)
{
    return((y / (double)display_y) * world.y_size);        
}
/* End of get_world_x */


/* Return a bounting box in screen space for an object */
void get_bounding_rect(double x, double y, double x_size, double y_size, RECT &r)
{
    r.left = get_screen_x(x - x_size / 2);
    r.top = get_screen_y(y - y_size / 2);
    r.right = get_screen_x(x + x_size / 2);
    r.bottom = get_screen_y(y + y_size / 2);
}
/* End of get_bounding_rect */
    


/* Return true if points is inside of wall */    
bool world_check_wall_point( double x, double y )
{
    wall_type *w = world.walls;

    while (w != NULL) 
    {
        if (x >= w->x && x <= w->x + w->x_size && 
            y >= w->y && y <= w->y + w->y_size)
            return(1);

        w = w->next;

    }
    return(false);
}    
/* End of world_check_wall_point */


/* Add a wall to the world */
void world_add_wall(double x, double y, double x_size, double y_size)
{
    wall_type *w = world.walls;
    
    world.walls = (wall_type *) malloc(sizeof(wall_type));

    world.walls->x = x;
    world.walls->y = y;

    world.walls->x_size = x_size;
    world.walls->y_size = y_size;
       
    world.walls->next = w;
    
    world.num_walls++;
}
/* End of world_add_wall */



/* Free wall memory */
void world_free_walls( void )
{
    wall_type *tmp;
        
    while (world.walls != NULL) 
    {
        tmp = world.walls->next;

        free(world.walls);

        world.walls = tmp; // Unlink
    }

    world.num_walls = 0;
}    
/* End of world_free walls */


    
/* Draw walls to DC */
void world_draw_walls( HDC dc )
{
    wall_type *tmp = world.walls;

    SelectObject(dc, (HBRUSH)world.color1_brush);
    SelectObject(dc, (HPEN)world.color1_pen);

    while (tmp != NULL) 
    {
        RECT r;
        
        Rectangle(dc, get_screen_x(tmp->x),
                      get_screen_y(tmp->y),
                      get_screen_x(tmp->x + tmp->x_size),
                      get_screen_y(tmp->y + tmp->y_size));

        tmp = tmp->next;
    }
}    
/* End of world_draw_walls */



/* Add waypoint to nav system */
void world_add_waypoint(double x, double y, double dir)
{
    waypoint_type *w = world.waypoints;
    
    world.waypoints = (waypoint_type *) malloc(sizeof(waypoint_type));

    world.waypoints->x = x;
    world.waypoints->y = y;
    world.waypoints->dir = dir;
      
    world.waypoints->next = w;
    
    world.num_waypoints++;
}
/* End of world_add_waypoint */


/* Free waypoint memory */
void world_free_waypoints( void )
{
    waypoint_type *tmp;
        
    while (world.waypoints != NULL) 
    {
        tmp = world.waypoints->next;

        free(world.waypoints);

        world.waypoints = tmp; // Unlink
    }
    world.num_waypoints = 0;
}    
/* End of world_free_waypoints */


/* Draw nav waypoints to DC */
void world_draw_waypoints( HDC dc )
{
    waypoint_type *tmp = world.waypoints;

    SelectObject(dc, (HBRUSH)world.color1_brush);
    SelectObject(dc, (HPEN)world.color1_pen);

    while (tmp != NULL) 
    {
        RECT r;

        if (tmp->num == world.cur_waypoint)
        {
           SelectObject(dc, (HBRUSH)world.red_brush);
           SelectObject(dc, (HPEN)world.black_pen);
            
            
        }else
        {
           SelectObject(dc, (HBRUSH)world.color1_brush);
           SelectObject(dc, (HPEN)world.color1_pen);
        }
        
        Ellipse(dc, get_screen_x(tmp->x - 0.05),
                      get_screen_y(tmp->y - 0.05),
                      get_screen_x(tmp->x  + 0.05),
                      get_screen_y(tmp->y  + 0.05));

        tmp = tmp->next;

    }
}
/* End of draw waypoints */

    

/* Add marker to the world */
void world_add_marker(double x, double y)
{
    return;
    marker_type *m = world.markers;
    
    world.markers = (marker_type *) malloc(sizeof(marker_type));

    world.markers->x = x;
    world.markers->y = y;
       
    world.markers->next = m;
}
/* End of world_add_marker */



/* Free marker memory */
void world_free_markers( void )
{
    marker_type *tmp;
        
    while (world.markers != NULL) 
    {
        tmp = world.markers->next;


        free(world.markers);

        world.markers = tmp; // Unlink
    }
}    
/* End of world free markers */


/* draw markers to DC */
void world_draw_markers( HDC dc )
{
    marker_type *m = world.markers;

return;

    double x_size = 0.1;
    double y_size = 0.1;
    

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

    int count = 500;

    while (m != NULL && count--) 
    {
        RECT r;
        
        get_bounding_rect(m->x,m->y,x_size, y_size, r);

        Ellipse(dc, r.left, r.top, r.right, r.bottom);
        

        m = m->next;

    }
}    
/* End of world_draw_markers */

    
/* Add impact to world */
void world_add_impact(double x, double y)
{
    impact_type *i = world.impacts;
 
    while (i != NULL) 
    {
        double dist = (x-i->x)*(x-i->x) + (y-i->y)*(y-i->y);
        
        if (dist < 0.01) return;
        
        i = i->next;

    }

    i = world.impacts;
    
    world.impacts = (impact_type *) malloc(sizeof(impact_type));

    world.impacts->x = x;
    world.impacts->y = y;
       
    world.impacts->next = i;
    
    world.num_impact++;
    
    world_compute_map_grid();
}
/* End of world_add_impact */


/* Free impact memory */
void world_free_impacts( void )
{
    impact_type *tmp;
        
    while (world.impacts != NULL) 
    {
        tmp = world.impacts->next;

        free(world.impacts);

        world.impacts = tmp; // Unlink
    }
    world.num_impact = 0;
}    
/* End of world_free_impacts */


/* Draw imapcts to DC */
void world_draw_impacts( HDC dc )
{
    impact_type *i = world.impacts;

//return;
    double x_size = 0.2;
    double y_size = 0.2;
    

    SelectObject(dc, (HBRUSH)world.red_brush);
    SelectObject(dc, (HPEN)world.black_pen);
    

    while (i != NULL) 
    {
        RECT r;
        
        get_bounding_rect(i->x,i->y,x_size, y_size, r);

        Ellipse(dc, r.left, r.top, r.right, r.bottom);

        i = i->next;
    }
}    
/* End of world_draw_impacts */


/* Set target location */
void world_set_target(double x, double y)
{
    world.target_enabled = true;

    world.target_x = x;
    world.target_y = y;    
    
    world_compute_map_grid();
}
/* End of world_set_target */



/* Get direction to current waypoint */
void compute_waypoint_vector(double x, double y, point &v)
{
    v.x = 0;
    v.y = 0;

    if (world.num_waypoints == 0) return;
  
   double waypoint_x;
   double waypoint_y;
   
    // find current waypoint

    waypoint_type *w = world.waypoints;
    waypoint_type *waypoint = NULL;

    // Find current waypoint
    while (w != NULL)
    {
        if (w->num == world.cur_waypoint)
           waypoint = w;
        
         w = w->next;       
    }

    // Current waypoint not found
    if (waypoint == NULL)
    {
        write_console("waypoint not found\n");
        return;
    }

    v.x = x - waypoint->x;
    v.y = y - waypoint->y;    
}
/* End of compute_waypoint_vector */


/* Compute vector to closest impact */
void compute_impact_vector(double x, double y, point &v)
{
    v.x = 0;
    v.y = 0;
    
     if (world.num_impact == 0) return;
    
    impact_type *i = world.impacts;
   
    double closest_impact_x;
    double closest_impact_y;
   
    double closest_impact_dist = world.x_size * world.y_size;
   
   
    // Find closest impact 
    while (i != NULL) 
    {
        double x_diff = x - i->x;
        double y_diff = y - i->y;    
          
        double dist_squared = x_diff * x_diff + y_diff * y_diff;

        if (dist_squared < closest_impact_dist)
        {
            closest_impact_dist = dist_squared;
            closest_impact_x = i->x;
            closest_impact_y = i->y;            
        }
            
        i = i->next;        
     }


    v.x = x - closest_impact_x;
    v.y = y - closest_impact_y;        
}
/* End of compute_impact_vector */



/* Compute gradient at a point */
void compute_grad_point(double x, double y, double &dir, double &mag)
{
    double x_sum = 0, y_sum = 0;
    mag = 0;
    
    double dist;

    point v;

    // Compute vector to closest impact
    compute_impact_vector(x, y, v);
    
    dist = vect_mag(v.x, v.y);

    if (dist > 0)
    {
        double  force_x =  world.scale1 * v.x / pow(dist, 1.5);
        double  force_y =  world.scale1 * v.y / pow(dist, 1.5);

        x_sum += force_x;
        y_sum += force_y;

        mag += vect_mag(force_x, force_y);
    
    }
    
    
    compute_waypoint_vector(x, y, v);
    
    
    dist = vect_mag(v.x, v.y);
    
    if (dist > 0)
    {
        double force_x = world.scale2 * v.x;
        double force_y = world.scale2 * v.y;
        
       
        x_sum -= force_x;
        y_sum -= force_y;
    
        mag -= vect_mag(force_x, force_y);
    }
    

    
    compute_map_nav(v, x, y);

    dist = vect_mag(v.x, v.y);
    
    if (dist > 0)
    {
        double force_x = world.scale3 * v.x/dist;
        double force_y = world.scale3 * v.y/dist;
        
        x_sum += force_x;
        y_sum += force_y;
    
        mag += vect_mag(force_x, force_y);
    }
    








    dir = atan2(y_sum, x_sum);//+f_rand(-0.5, 0.5); 
}
/* End of compute_grad_point */



/* Draw gradient field */
void world_draw_grad(HDC dc)
{

 SelectObject(dc, world.green_pen);

    double x, y;
    
    int x_size = display_x /NUM_X_GRAD;
    int y_size = display_y /NUM_Y_GRAD;
    
    
    HBRUSH br;
    HPEN pen;
    
    for (int xg = 0; xg < NUM_X_GRAD; xg++)
        for (int yg = 0; yg < NUM_Y_GRAD; yg++)
        {
            x = xg / (double)NUM_X_GRAD * world.x_size;
            y = yg / (double)NUM_Y_GRAD * world.y_size;

            double dir, mag;

              compute_grad_point(x,y, dir, mag);


            double val = mag;
            
            if (val < -1) val = -1;
            if (val > 1) val = 1;
            
            int color = (int)(fabs(val) * 255);
            
            if (val < 0)
            {
                 br = CreateSolidBrush(RGB(color, 0, 0));
                pen = CreatePen(PS_SOLID, 1, RGB(color,0,0));                
            }
            else
            { 
                br = CreateSolidBrush(RGB(0, 0, color));
                pen = CreatePen(PS_SOLID, 1, RGB(0,0,color));
            }


             SelectObject(dc, br);
             SelectObject(dc, pen);

             Rectangle(dc, get_screen_x(x)-x_size/2-1,
                           get_screen_y(y)-y_size/2-1,
                           get_screen_x(x)+x_size/2+1,
                           get_screen_y(y)+y_size/2+1);

             DeleteObject(br);
             DeleteObject(pen);

             SelectObject(dc, world.green_pen);    

           // double mag =  fabs(world.grad_mag[yg][xg]) / 100.0;
            
            mag = fabs(mag / 5.0);

            if (mag > 0.1) mag = 0.1;

            
         //   mag = 0.05;

            draw_arrow(dc, x, y, dir, mag);
    }
}
/* End of void world_draw_grad(HDC dc) */


/* Load world file */
void world_load( void )
{
    OPENFILENAME ofname;

    char filename[300] = {0};
             
    char filter[] = "World files\0*.dat\0\0";
           
    memset(&ofname, 0, sizeof(ofname));
             
    ofname.lStructSize = sizeof(ofname);
    ofname.lpstrFilter = filter; 
    ofname.lpstrFile = filename; 
    ofname.nMaxFile = 300; 
    ofname.lpstrFileTitle = NULL; 
//    ofname.nMaxFileTitle = 300; 
    ofname.lpstrInitialDir = "."; 

    // Get filename from user
    if (!GetOpenFileName(&ofname))
        return;

    FILE *f = fopen(filename, "rt");

    unsigned int num_walls;


    world_setup();

    world_free_walls();

    fscanf(f, "%lf\n", &robot.x);
    fscanf(f, "%lf\n", &robot.y);
    fscanf(f, "%lf\n", &robot.dir);

    fscanf(f, "%d\n", &num_walls);
  
    for (int i = 0; i < num_walls && !feof(f); i++)
    {
        double x, y, x_size, y_size;
        
        fscanf(f, "%lf %lf %lf %lf\n", &x, &y, &x_size, &y_size);

        world_add_wall(x, y, x_size, y_size);
    }
}
/* End of world_load */


/* Save world data */
void world_save( void )
{
    OPENFILENAME ofname;
    char filename[300] = {0};
    char filter[] = "Map files\0*.dat\0\0";             
             
    memset(&ofname, 0, sizeof(ofname));
             
    ofname.lStructSize = sizeof(ofname);
    ofname.lpstrFilter = filter; 
    ofname.lpstrFile = filename; 
    ofname.nMaxFile = 300; 
    ofname.lpstrFileTitle = NULL; 
    ofname.lpstrInitialDir = "."; 

    // Get filename from user
    if (!GetSaveFileName(&ofname))
    {
        write_console("erer");
        return;
    }


    FILE *f = fopen(filename, "wt");
    

    fprintf(f, "%f\n", robot.x);
    fprintf(f, "%f\n", robot.y);
    fprintf(f, "%f\n", robot.dir);

    fprintf(f, "%d\n", world.num_walls);
    
    
    wall_type *w = world.walls;
    
    while(w != NULL)
    {
        fprintf(f, "%f %f %f %f\n", w->x, w->y, w->x_size, w->y_size);   
        
        w = w -> next;
    }
    
    fclose(f);
}
/* End of save world */




















