#include <cmath>
#include <iostream>

#include "generator.h"


void GeneratorRowsZAX::tool_point(Track &track, const TrackPoint &p) const {
    if (!track.points.empty()) {
        TrackPoint &last = track.points.back();
        if ( (last.position - p.position).len() <= precision
          && fabs(last.angle - p.angle) <= precision )
        {
            last.speed = p.speed;
            return;
        }
    }
    
    track.points.push_back(p);
}

void GeneratorRowsZAX::tool_up(Track &track, Real safe_z) const {
    if (track.points.empty()) return;

    const TrackPoint &prev = track.points.back();
    if (prev.position.z == safe_z) return;
    
    TrackPoint p;
    p.position = prev.position;
    p.position.z = safe_z;
    p.angle = prev.angle;
    p.speed = move_speed/60.0;
    
    tool_point(track, p);
}

void GeneratorRowsZAX::tool_feed(Track &track, Real safe_z, Real z1, Real z, Real a, Real x) const {
    TrackPoint p;
    p.position = Vector3(x, 0, safe_z);
    p.angle = a*180.0/M_PI;
    
    if (track.points.empty() || track.points.back().position.z == safe_z) {
        p.speed = move_speed/60.0;
        tool_point(track, p);

        if (safe_z > z1 && z1 > z) {
            p.position.z = z1;
            tool_point(track, p);
        }
    }

    p.position.z = z;
    p.speed = feed_speed/60.0;
    tool_point(track, p);
}


void GeneratorRowsZAX::touch(
    const Collider &collider,
    Track &track,
    Real safe_z, Real z1, Real z0,
    Real a, Real x,
    bool skip_z0 ) const
{
    Vector3 dir(0, sin(a), -cos(a));
    Vector3 pos(x, 0, 0);

    Real z = collider.distance_to_model(pos, dir);
    if (std::isinf(z))
        z = std::max(Real(0), z0);
    
    z = std::max(-z, z0);
    if (z > z1 || (skip_z0 && z == z0))
        tool_up(track, safe_z);
    else
        tool_feed(track, safe_z, z1, z, a, x);
}



bool GeneratorRowsZAX::generate(const Collider &collider, Track &track) const {
    track.points.clear();
    
    const Real precision = 1e-3;
    
    if (collider.model.triangles.empty()) return false;

    if (step_z != 0 && step_z <= precision) return false;
    if (step_a <= precision) return false;
    if (step_x <= precision) return false;
    if (feed_speed <= precision) return false;
    if (move_speed <= precision) return false;

    std::cout << "GeneratorRowsZAX::generate" << std::endl;

    unsigned long long max_count = 100ull*1024*1024/sizeof(track.points.front());
    
    Real min_x = this->min_x;
    Real max_x = this->max_x;
    Real min_z = this->min_z;
    Real max_z = this->max_z;
    Real safe_z = this->safe_z;

    const TriangleList &triangles = collider.model.triangles;
    
    // calc bounds
    Real bx0 = INFINITY, bx1 = -INFINITY, bz1 = 0;
    for(TriangleList::const_iterator i = triangles.begin(); i != triangles.end(); ++i) {
        for(int j = 0; j < 3; ++j) {
            const Vector3 &v = i->vertices[j];
            const Real z = sqrt(v.y*v.y + v.z*v.z);
            bx0 = std::min(bx0, v.x);
            bx1 = std::max(bx1, v.x);
            bz1 = std::max(bz1, z);
        }
    }

    bx0 += collider.tool.get_radius();
    bx1 -= collider.tool.get_radius();
    if (bx1 + precision <= bx0 || bz1 + precision <= 0) {
        std::cout << "GeneratorRowsZAX::generate: zero bound" << std::endl;
        return false;
    }
    
    if (max_x <= min_x) {
        min_x = bx0;
        max_x = bx1;
    } else {
        min_x = std::max(min_x, bx0);
        max_x = std::min(max_x, bx1);
    }
    if (max_x + precision <= min_x) return false;

    min_z = std::max(min_z, -max_z);
    max_z = std::max(max_z, bz1);
    safe_z = std::max(safe_z, max_z + 1);
    
    int count_z = step_z ? std::max(1, (int)ceil((max_z - min_z)/step_z - precision)) : 1;
    int count_x = std::max(1, (int)ceil((max_x - min_x)/step_x - precision)) + 1;

    bool forward_a = true;
    bool forward_x = true;
    Real z1 = max_z + precision;
    for(int iz = 0; iz < count_z; ++iz) {
        Real z0 = count_z <= 1 ? min_z : max_z - (iz+1)*(max_z - min_z)/count_z;
        
        int count_a = 3;
        if (fabs(z1) > precision)
            count_a = std::max(count_a, (int)ceil(2*M_PI*fabs(z1)/step_a - precision));
        Real sa = 2*M_PI/count_a;

        int count_aa = 0;
        if (fabs(z1) > precision)
            count_aa = (int)ceil(sa*fabs(z1)/step_x - precision);
        
        for(int ia = 0; ia < count_a; ++ia) {
            int iia = forward_a ? ia : count_a - ia - 1;
            Real a = iia*sa;
            
            
            if (feed_in_rotation && ia > 0) {
                for(int iaa = 1; iaa < count_aa; ++iaa) {
                    Real aa = sa - sa*iaa/count_a;
                    aa = forward_a ? a - aa : a + aa;
                    Real x = forward_x ? min_x : max_x;
                    touch(collider, track, safe_z, z1, z0, aa, x, skip_middle_layers);
                    if (track.points.size() >= max_count) break;
                }
            }
            
            for(int ix = 0; ix < count_x; ++ix) {
                int iix = forward_x ? ix : count_x - ix - 1;
                Real x = min_x + iix*(max_x - min_x)/(count_x - 1);
                touch(collider, track, safe_z, z1, z0, a, x, skip_middle_layers);
                if (track.points.size() >= max_count) break;
            }
            
            if (!feed_in_rotation)
                tool_up(track, safe_z);

            forward_x = !forward_x;
            if (track.points.size() >= max_count) break;
            std::cout << "." << std::flush;
        }

        forward_a = !forward_a;
        z1 = z0;
        if (track.points.size() >= max_count) break;
        std::cout << std::endl << std::flush;
    }

    tool_up(track, safe_z);
    
    if (track.points.size() >= max_count) {
        std::cout << "GeneratorRowsZAX::generate: reached memory limit" << std::endl;
        return false;
    }
    
    if (track.points.size() <= 1) {
        std::cout << "GeneratorRowsZAX::generate: generated track is empty" << std::endl;
        return false;
    }

    track.split(1, 0, 0);
    track.calc_length();
    track.split(0, 0.1, 0);
    
    std::cout << "GeneratorRowsZAX::generate: track dureation " << (track.points.back().time/60) << std::endl;
    std::cout << "GeneratorRowsZAX::generate: done" << std::endl;
    return true;
}