#include <iostream>

#include <GL/gl.h>

#include "simulator.h"


void SimulatorXYZA::simulate(const Track &track, Real tool_radius) {
    points.clear();
    simu_index = 0;
    x0 = x1 = 0;

    if (!raster.get_data() || track.points.empty()) return;
    
    std::cout << "SimulatorXYZA::simulate" << std::endl;
    
    x0 = x1 = track.points.front().position.x;
    Real max_radius = 0;
    for(PointList::const_iterator i = track.points.begin(); i != track.points.end(); ++i) {
        Real radius = sqrt(i->position.y*i->position.y + i->position.z*i->position.z);
        if (max_radius < radius) max_radius = radius;
        if (x0 > i->position.x) x0 = i->position.x;
        if (x1 < i->position.x) x1 = i->position.x;
    }
    raster.fill(max_radius);
    
    if (x1 + 1e-5 < x0) {
        std::cout << "SimulatorXYZA::simulate: zero bound" << std::endl;
        return;
    }
    
    Real h = raster.get_height();
    Real kx = raster.get_width()/(x1 - x0);
    Real ka = 1/360.0;
    Real kr = h/(2*M_PI);

    Real dx = fabs(tool_radius*kx);
    int index = 0;
    for(PointList::const_iterator i = track.points.begin(); i != track.points.end(); ++i, ++index) {
        Real radius = sqrt(i->position.y*i->position.y + i->position.z*i->position.z);
        if (radius < 1e-5) radius = 1e-5;
        Real dy = fabs(tool_radius/radius*kr);
        
        Real x = (i->position.x - x0)*kx;
        Real y = i->angle*ka;
        y = (y - floor(y))*h;

        raster.touch(index, radius, x, y, dx, dy, points);
        if (y - dy < 0)
            raster.touch(index, radius, x, y + h, dx, dy, points);
        if (y + dy > h)
            raster.touch(index, radius, x, y - h, dx, dy, points);
        
        if (index % 1000 == 0)
            std::cout << "." << std::flush;
        if (index % 100000 == 0)
            std::cout << std::endl << std::flush;
    }
    simu_index = (int)points.size();

    std::cout << "SimulatorXYZA::simulate: done" << std::endl;
}
    
void SimulatorXYZA::scroll_to(int index) {
    while(simu_index < (int)points.size() && points[simu_index].index <= index)
        points[simu_index++].apply_next(raster);
    while(simu_index > 0 && points[simu_index-1].index > index)
        points[--simu_index].apply_prev(raster);
}
    
void SimulatorXYZA::draw() const {
    int w = raster.get_width();
    int h = raster.get_height();
    if (w < 2 || h < 2) return;

    Vector3 v, v0, v1;
    
    Real kx = (x1 - x0)/w;
    Real ka = 2*M_PI/h;

    glEnable(GL_LIGHTING);
    
    glColor4d(0, 0, 1, 0.5);
    
    glBegin(GL_TRIANGLE_STRIP);
    for(int r = 0; r < h; ++r) {
        int rr = r ? r-1 : h-1;
        Real a0 = rr*ka;
        Real a1 = r*ka;
        Real s0 = sin(a0), s1 = sin(a1);
        Real c0 = cos(a0), c1 = cos(a1);
        for(int c = 0; c < w; ++c) {
            v0.x = v1.x = x0 + c*kx;
            Real r0 = raster[rr][c];
            Real r1 = raster[r][c];
            v0.y = -s0*r0, v0.z = c0*r0;
            v1.y = -s1*r1, v1.z = c1*r1;
            
            Vector3 dv0(v0.x - v.x, v0.y - v.y, v0.z - v.z);
            Vector3 dv1(v1.x - v.x, v1.y - v.y, v1.z - v.z);
            Vector3 norm(
                dv0.y*dv1.z - dv0.z*dv1.y,
                dv0.z*dv1.x - dv0.x*dv1.z,
                dv0.x*dv1.y - dv0.y*dv1.x );
            Real l = sqrt(norm.x*norm.x + norm.y*norm.y + norm.z*norm.z);
            l = l > 1e-5 ? 1/l : 0;
            glNormal3d(norm.x*l, norm.y*l, norm.z*l);

            if (!c) glVertex3dv(v0.c);
            glVertex3dv(v0.c);
            glVertex3dv(v1.c);
            v = v1;
        }
        glVertex3dv(v.c);
    }
    glEnd();

    glDisable(GL_LIGHTING);
}