/*
 ** sdl.cc
 ** Login : <mouret@mexico>
 ** Started on  Sat Jan 12 20:40:12 2008 Jeanbaptiste MOURET
 ** $Id$
 **
 ** Copyright (C) 2008 Jeanbaptiste MOURET
 ** 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
 */
#include "display.hpp"
#include <bitset>

#ifdef USE_SDL

namespace fastsim
{

void Display :: _events()
{
    SDL_Event event;
    while (SDL_PollEvent(&event))
    {
        switch (event.type)
        {
        case SDL_QUIT:
            _quit();
        case SDL_KEYDOWN:
            if (event.key.keysym.sym == SDLK_ESCAPE)
                _quit();
            break;
        }
    }
}

void Display :: _blit_map()
{
    for (unsigned i = 0; i < _map->get_pixel_w(); ++i)
        for (unsigned j = 0; j < _map->get_pixel_h(); ++j)
            if (_map->get_pixel(i, j) == Map::obstacle)
                _put_pixel(_map_bmp, i, j, 0, 0, 0);
            else
                _put_pixel(_map_bmp, i, j, 255, 255, 255);
    SDL_BlitSurface(_map_bmp, 0, _screen, 0);
    SDL_UpdateRect(_screen, 0, 0, _w, _h);
}

Display :: Display(const boost::shared_ptr<Map>& m, const Robot& r) :
            _map(m), _robot(r)
{
    _w = _map->get_pixel_w();
    _h = _map->get_pixel_h();
    if (SDL_Init(SDL_INIT_VIDEO) == -1)
        throw Exception(SDL_GetError());
    _screen = SDL_SetVideoMode(_w, _h, 
            32, SDL_SWSURFACE);
    if (!_screen)
        throw Exception(SDL_GetError());

    Uint32 rmask, gmask, bmask, amask;
#if SDL_BYTEORDER == SDL_BIG_ENDIAN
    rmask = 0xff000000;
    gmask = 0x00ff0000;
    bmask = 0x0000ff00;
    amask = 0x000000ff;
#else
    rmask = 0x000000ff;
    gmask = 0x0000ff00;
    bmask = 0x00ff0000;
    amask = 0xff000000;
#endif

    _map_bmp = SDL_CreateRGBSurface(SDL_SWSURFACE, _w, _h, 32, 
            rmask, gmask, bmask, amask);
    _blit_map();
    _bb_to_sdl(_robot.get_bb(), &_prev_bb);
}

void Display :: _line(SDL_Surface* surf,
        int x0, int y0, int x1, int y1,
        Uint32 color)
{
    x0 = std::max(x0, 0); x0 = std::min(x0, surf->w - 1);
    x1 = std::max(x1, 0); x1 = std::min(x1, surf->w - 1);
    y0 = std::max(y0, 0); y0 = std::min(y0, surf->h - 1);
    y1 = std::max(y1, 0); y1 = std::min(y1, surf->h - 1);
    int dy = y1 - y0;
    int dx = x1 - x0;
    int stepx, stepy;

    if (dy < 0) { dy = -dy;  stepy = -1; } else { stepy = 1; }
    if (dx < 0) { dx = -dx;  stepx = -1; } else { stepx = 1; }
    dy <<= 1; // dy is now 2*dy
    dx <<= 1; // dx is now 2*dx

    _put_pixel(surf, color, x0, y0);
    if (dx > dy) 
    {
        int fraction = dy - (dx >> 1); // same as 2*dy - dx
        while (x0 != x1)
        {
            if (fraction >= 0)
            {
                y0 += stepy;
                fraction -= dx; // -= 2*dx
            }
            x0 += stepx;
            fraction += dy; //  -= 2*dy
            _put_pixel(surf, color, x0, y0);
        }
    }
    else 
    {
        int fraction = dx - (dy >> 1);
        while (y0 != y1)
        {
            if (fraction >= 0)
            {
                x0 += stepx;
                fraction -= dy;
            }
            y0 += stepy;
            fraction += dx;
            _put_pixel(surf, color, x0, y0);
        }
    }
}

void Display :: _try_pixel(bool& res,
        SDL_Surface* surf,
        Uint32 color, int x, int y)
{

    if (x >= 0 && y >= 0 
            && x < _map->get_pixel_w()
            && y < _map->get_pixel_h()
            && _map->get_pixel(x, y) == Map::free)
    {
        _put_pixel(surf, color, x, y);
        res = false;
    }
    else
        res = true;
}

void Display :: _circle_points(SDL_Surface* surf,
        int cx, int cy, int x, int y, Uint32 color)
{

    if (x == 0) {
        _put_pixel(surf, color, cx, cy + y);
        _put_pixel(surf, color, cx, cy - y);
        _put_pixel(surf, color, cx + y, cy);
        _put_pixel(surf, color, cx - y, cy);
    }
    else if (x == y) {
        _put_pixel(surf, color, cx + x, cy + y);
        _put_pixel(surf, color, cx - x, cy + y);
        _put_pixel(surf, color, cx + x, cy - y);
        _put_pixel(surf, color, cx - x, cy - y);
    } else if (x < y) {
        _put_pixel(surf, color, cx + x, cy + y);
        _put_pixel(surf, color, cx - x, cy + y);
        _put_pixel(surf, color, cx + x, cy - y);
        _put_pixel(surf, color, cx - x, cy - y);
        _put_pixel(surf, color, cx + y, cy + x);
        _put_pixel(surf, color, cx - y, cy + x);
        _put_pixel(surf, color, cx + y, cy - x);
        _put_pixel(surf, color, cx - y, cy - x);
    }
}

void Display :: _circle(SDL_Surface *surf,
        int x_center, int y_center, int radius,
        Uint32 color)
{
    int x = 0;
    int y = radius;
    int p = (5 - radius * 4) / 4;

    _circle_points(surf, x_center, y_center, x, y, color);
    while (x < y) 
    {
        x++;
        if (p < 0)
            p += 2 * x + 1;
        else
        {
            y--;
            p += 2 * (x - y) + 1;
        }
        _circle_points(surf, x_center, y_center, x, y, color);
    }
}
/// see http://en.wikipedia.org/wiki/Midpoint_circle_algorithm
// transform setPixel(x0 + x, y0 + y); setPixel(x0 - x, y0 + y);
// to  _line(x0 - x,, y0+y, x0 + x, y0 + y)
void Display :: _disc(SDL_Surface *surf,
        int x_center, int y_center, int radius,
        Uint32 color)
{
    int f = 1 - radius;
    int ddF_x = 1;
    int ddF_y = -2 * radius;
    int x = 0;
    int y = radius;
    int x0 = x_center;
    int y0 = y_center;

    _line(surf, x0, y0 - radius, x0, y0 + radius, color);
    _line(surf, x0 - radius, y0, x0 + radius, y0, color);    
    while(x < y)
    {
        // ddF_x == 2 * x + 1;
        // ddF_y == -2 * y;
        // f == x*x + y*y - radius*radius + 2*x - y + 1;
        if(f >= 0)
        {
            y--;
            ddF_y += 2;
            f += ddF_y;
        }
        x++;
        ddF_x += 2;
        f += ddF_x;
        _line(surf, x0 - x, y0 + y, x0 + x, y0 + y, color);
        _line(surf, x0 - x, y0 - y, x0 + x, y0 - y, color);
        _line(surf, x0 - y, y0 + x, x0 + y, y0 + x, color);
        _line(surf, x0 - y, y0 - x, x0 + y, y0 - x, color);
    }
}



void Display :: _disp_bb()
{
    unsigned x = _map->real_to_pixel(_robot.get_bb().x);
    unsigned y = _map->real_to_pixel(_robot.get_bb().y);
    unsigned w = _map->real_to_pixel(_robot.get_bb().w);
    unsigned h = _map->real_to_pixel(_robot.get_bb().h);

    assert(x >= 0);
    assert(y >= 0);
    assert(x + w < _screen->w);
    assert(y + h < _screen->h);
    _line(_screen, x, y, x + w, y, 0);
    _line(_screen, x + w, y, x + w, y + h, 0);
    _line(_screen, x + w, y + h, x, y + h, 0);
    _line(_screen, x, y + h, x, y, 0);    
}

void Display :: _disp_goals()
{
    for (size_t i = 0; i < _map->get_goals().size(); ++i)
    {
        const Goal& goal = _map->get_goals()[i];
        unsigned x = _map->real_to_pixel(goal.get_x());
        unsigned y = _map->real_to_pixel(goal.get_y());
        unsigned diam = _map->real_to_pixel(goal.get_diam());
        Uint8 r = 0, g = 0, b = 0;
        switch (goal.get_color())
        {
        case 0:
            r = 255;
            break;
        case 1:
            g = 255;
            break;
        case 2:
            b = 255;
            break;
        default:
            assert(0);
        }
        _circle(_screen, x, y, diam, r, g, b);
    }
}

void Display :: _disp_radars()
{
    unsigned r = _map->real_to_pixel(_robot.get_radius())  / 2;
    unsigned x = _map->real_to_pixel(_robot.get_pos().x());
    unsigned y = _map->real_to_pixel(_robot.get_pos().y());

    for (size_t i = 0; i < _robot.get_radars().size(); ++i)
    {
        const Radar& radar = _robot.get_radars()[i];
        if (radar.get_activated_slice() != -1)
        {
            float a1 = _robot.get_pos().theta() + radar.get_inc() * radar.get_activated_slice();
            float a2 = _robot.get_pos().theta() + radar.get_inc() * (radar.get_activated_slice() + 1);
            _line(_screen,
                    cos(a1) * r + x, sin(a1) * r + y,
                    cos(a2) * r + x, sin(a2) * r + y,
                    0x0000FF);
            const Goal& g = _map->get_goals()[radar.get_color()];
            unsigned gx = _map->real_to_pixel(g.get_x());
            unsigned gy = _map->real_to_pixel(g.get_y());
            _line(_screen, x, y, gx, gy, 0x0000FF);
        }

    }

}
void Display :: _disp_bumpers()
{
    // convert to pixel
    unsigned x = _map->real_to_pixel(_robot.get_pos().x());
    unsigned y = _map->real_to_pixel(_robot.get_pos().y());
    unsigned r = _map->real_to_pixel(_robot.get_radius());
    float theta = _robot.get_pos().theta();
    Uint32 cb_left = SDL_MapRGB(_screen->format, _robot.get_left_bumper() ? 255 : 0, 0, 0);
    Uint32 cb_right = SDL_MapRGB(_screen->format, _robot.get_right_bumper() ? 255 : 0, 0, 0);
    _line(_screen,
            (int) (r * cosf(theta + M_PI / 2.0f) + x),
            (int) (r * sinf(theta + M_PI / 2.0f) + y),
            (int) (r * cosf(theta) + x),
            (int) (r * sinf(theta) + y),
            cb_left);
    _line(_screen,
            (int) (r * cosf(theta - M_PI / 2.0f) + x),
            (int) (r * sinf(theta - M_PI / 2.0f) + y),
            (int) (r * cosf(theta) + x),
            (int) (r * sinf(theta) + y),
            cb_right);
}

Uint32 Display :: _color_from_id(SDL_Surface* surf, int x)
{
    std::bitset<4> bs = x;	
    Uint8 k = 1 - bs[4];
    Uint8 r = bs[0] * 128 + 127 * k;
    Uint8 g = bs[1] * 128 + 127 * k;
    Uint8 b = bs[2] * 128 + 127 * k;

    return SDL_MapRGB(surf->format, r, g, b);
}

void Display :: _disp_switches()
{
    for (size_t i = 0; i < _map->get_illuminated_switches().size(); ++i)
    {
        const IlluminatedSwitch& sw = *_map->get_illuminated_switches()[i];
        unsigned x = _map->real_to_pixel(sw.get_x());
        unsigned y = _map->real_to_pixel(sw.get_y());
        unsigned rad = _map->real_to_pixel(sw.get_radius());
        Uint32 color = _color_from_id(_screen, sw.get_color());

        _circle(_screen, x, y, rad, color);
        if (sw.get_on())
            _disc(_screen, x, y, rad, color);
    }
}

void Display :: _disp_lasers()
{
    for (size_t i = 0; i < _robot.get_lasers().size(); ++i)
    {
        unsigned x_laser = _map->real_to_pixel(_robot.get_pos().x()
                + _robot.get_lasers()[i].get_gap_dist()
                * cosf(_robot.get_pos().theta()
                        + _robot.get_lasers()[i].get_gap_angle()));
        unsigned y_laser = _map->real_to_pixel(_robot.get_pos().y()
                + _robot.get_lasers()[i].get_gap_dist()
                * sinf(_robot.get_pos().theta()
                        + _robot.get_lasers()[i].get_gap_angle()));
        _line(_screen, x_laser, y_laser,
                _robot.get_lasers()[i].get_x_pixel(),
                _robot.get_lasers()[i].get_y_pixel(),
                0xFF00000);
    }
}

void Display :: _disp_light_sensors()
{
    for (size_t i = 0; i < _robot.get_light_sensors().size(); ++i)
    {
        const LightSensor& ls = _robot.get_light_sensors()[i];
        unsigned x_ls = _map->real_to_pixel(_robot.get_pos().x());
        unsigned y_ls = _map->real_to_pixel(_robot.get_pos().y());
        unsigned x_ls1 = _map->real_to_pixel(_robot.get_pos().x()
                + 200./(float)ls.get_color()
                * cosf(_robot.get_pos().theta()
                        + ls.get_angle()-ls.get_range()/2.0));
        unsigned y_ls1 = _map->real_to_pixel(_robot.get_pos().y()
                + 200./(float)ls.get_color()
                * sinf(_robot.get_pos().theta()
                        + ls.get_angle()-ls.get_range()/2.0));
        _line(_screen, x_ls, y_ls, x_ls1, y_ls1, _color_from_id(_screen, ls.get_color()));
        unsigned x_ls2 = _map->real_to_pixel(_robot.get_pos().x()
                + 200./(float)ls.get_color()
                * cosf(_robot.get_pos().theta()
                        + ls.get_angle()+ls.get_range()/2.0));
        unsigned y_ls2 = _map->real_to_pixel(_robot.get_pos().y()
                + 200./(float)ls.get_color()
                * sinf(_robot.get_pos().theta()
                        + ls.get_angle()+ls.get_range()/2.0));
        _line(_screen, x_ls, y_ls, x_ls2, y_ls2, _color_from_id(_screen, ls.get_color()));
        _line(_screen, x_ls1, y_ls1, x_ls2, y_ls2, _color_from_id(_screen, ls.get_color()));

        if (ls.get_activated())
        {
            const IlluminatedSwitch& is =
                    *_map->get_illuminated_switches()[ls.get_num()];
            unsigned x_is = _map->real_to_pixel(is.get_x());
            unsigned y_is = _map->real_to_pixel(is.get_y());
            _line(_screen, x_ls, y_ls, x_is, y_is, _color_from_id(_screen, is.get_color()));
        }
    }
}

void Display :: _disp_camera()
{
    static const int pw = 20;
    if (!_robot.use_camera())
        return;
    unsigned x_ls = _map->real_to_pixel(_robot.get_pos().x());
    unsigned y_ls = _map->real_to_pixel(_robot.get_pos().y());
    float a1 = _robot.get_pos().theta() + _robot.get_camera().get_angular_range() / 2.0;
    _line(_screen, x_ls, y_ls, cos(a1) * 200 + x_ls, 
            sin(a1) * 200 + y_ls, 0x0000ff);
    float a2 = _robot.get_pos().theta() - _robot.get_camera().get_angular_range() / 2.0;
    _line(_screen, x_ls, y_ls, cos(a2) * 200 + x_ls, 
            sin(a2) * 200 + y_ls, 0x0000ff);

    for (size_t i = 0; i < _robot.get_camera().pixels().size(); ++i)
    {
        int pix = _robot.get_camera().pixels()[i];
        Uint32 color = pix == -1 ? 0xffffff : _color_from_id(_screen, pix);
        SDL_Rect r; r.x = i * pw; r.y = 0; r.w = pw; r.h = pw;
        SDL_FillRect(_screen, &r, color);
    }

}

void Display :: update()
{
    _events();
    // convert to pixel
    unsigned x = _map->real_to_pixel(_robot.get_pos().x());
    unsigned y = _map->real_to_pixel(_robot.get_pos().y());
    unsigned r = _map->real_to_pixel(_robot.get_radius());
    float theta = _robot.get_pos().theta();

    // erase robot
    SDL_BlitSurface(_map_bmp, &_prev_bb, _screen, &_prev_bb);
    // erase all
    SDL_BlitSurface(_map_bmp, 0, _screen, 0);

    // lasers
    std::cout << "Drawing lasers" << std::endl;
    _disp_lasers();

    // goals
    std::cout << "Drawing goals" << std::endl;
    _disp_goals();

    // illuminated switches
    std::cout << "Drawing switches" << std::endl;
    _disp_switches();

    // light sensor
    std::cout << "Drawing light sensors" << std::endl;
    _disp_light_sensors();

    // radars
    std::cout << "Drawing radars" << std::endl;
    _disp_radars();

    // camera
    std::cout << "Drawing camera" << std::endl;
    _disp_camera();

    // draw the circle again (robot)
    std::cout << "Drawing robot" << std::endl;
    unsigned int col=_robot.color();
    _disc(_screen, x, y, r, _color_from_id(_screen,col));
    _circle(_screen,x,y,r,255,0,0);
    // direction
    Uint32 color = SDL_MapRGB(_screen->format, 0, 255, 0);
    _line(_screen, x, y, (int) (r * cosf(theta) + x), (int)(r * sinf(theta) + y), color);

    // bumpers
    std::cout << "Drawing bumpers" << std::endl;
    _disp_bumpers();

    std::cout << "I have not clue what is happening here" << std::endl;
    SDL_Rect rect;
    _bb_to_sdl(_robot.get_bb(), &rect);
    using namespace std;
    rect.x = max(0, min((int)rect.x, (int)_prev_bb.x));
    rect.y = max(0, min((int)rect.y, (int)_prev_bb.y));
    rect.w = max(rect.w, _prev_bb.w);
    rect.h = max(rect.h, _prev_bb.h);

    if (rect.x + rect.w > _w) rect.w = _w;
    if (rect.y + rect.h > _h) rect.h = _h;

    std::cout << "Updating the screen" << std::endl;
    // the fast one
    SDL_UpdateRect(_screen, rect.x, rect.y, rect.w, rect.h);
    // the slow one
    SDL_UpdateRect(_screen, 0, 0, _screen->w, _screen->h);
    _bb_to_sdl(_robot.get_bb(), &_prev_bb);

}



}

#endif
