#include <iostream></iostream>

#include <gl gl.h=""></gl>

#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);

}