//use floats, not doubles, for 32-bit iGPUs

#include "polypartiCL.hpp"
#include <cmath>
#include <sycl/sycl.hpp>
#include <exception>
#include <vector>
#include <fstream>
#include <algorithm>

using namespace sycl;

struct Polyparti::ParticleField::ColCode {
    std::vector<XYVector> positions, velocities;
    float t, fieldTime, lifeTime;
    int count, decay = 0;
    int color[4];
    //float decay = 1.0f;
    bool dies = false, dead = false;

    // allocate only one queue so we don't reinitialize every step of the calculation
    static queue& q_shared() {
        static queue q{gpu_selector_v, property::queue::in_order()};
        return q;
    }

    void checkCollisions(std::vector<XYVector> verticies) {
        queue& q = q_shared();
        int len_points = positions.size();

        //Pointers to malloc on GPU VRAM for calculations. malloc_shared pointers are entry/exit 
        //point for data in CPU RAM
        float *v_ptr = malloc_shared<float>(2 * len_points, q);
        float *d_ptr = malloc_shared<float>(2 * len_points, q);
        float *c_ptr = malloc_shared<float>(2 * len_points, q);
        float *ray_ptr = malloc_device<float>(2 * len_points, q);
        float *det_ptr = malloc_device<float>(len_points, q);
        float *bestA_ptr = malloc_device<float>(2 * len_points, q);
        float *bestSlope_ptr = malloc_device<float>(2 * len_points, q);
        float *r_ptr = malloc_device<float>(len_points, q);
        float *s_ptr = malloc_device<float>(len_points, q);
        float *rPref_ptr = malloc_device<float>(len_points, q);
        float *sPref_ptr = malloc_device<float>(len_points, q);
        float *nml_ptr = malloc_device<float>(len_points * 2, q);
        float *dot_ptr = malloc_device<float>(len_points, q);

        float *t_ptr = malloc_shared<float>(1, q);
        std::vector<float> sPrefInit(len_points, float(1000000.0f));

        XYVector A, B, borderSlope;

        //Copy arrays from CPU RAM to shared pointer with GPU VRAM & CPU RAM
        //SYCL can only handle one memcpy per kernel 

        q.submit([&](handler& h) {h.memcpy(c_ptr, &positions[0], len_points * 2 * sizeof(float)); });
        q.wait();

        q.submit([&](handler& h) {h.memcpy(v_ptr, &velocities[0], len_points * 2 * sizeof(float));});
        q.wait();
        
        q.submit([&](handler& h) {h.memcpy(sPref_ptr, &sPrefInit[0], len_points * sizeof(float)); });
        q.wait();

        q.submit([&](handler& h) { h.memcpy(t_ptr, &t, sizeof(float)); });
        q.wait();


        //Calculate expected location of each particle after imminent time step, and vectorize the
        //difference <dx, dy> from the current position. We will use Cramer's Rule with the 
        //vectorized boundary, also <dx, dy>, to check intersection

        q.submit([&](handler& h) {
            h.parallel_for(len_points * 2, [=](id<1> i) {
                d_ptr[i] = v_ptr[i] * *t_ptr + c_ptr[i];
                ray_ptr[i] = d_ptr[i] - c_ptr[i];
            });
        });
        q.wait();

        //Iterate over each room boundary in terms of their endpoints. Note that iterative and
        //parallel computing concepts can work in tandem, with the next kernel executed
        //sychronously within the for loop

        for (int i = 1; i < verticies.size(); i++) {
            A = verticies[i-1], B = verticies[i];
            borderSlope[0] = B[0] - A[0];
            borderSlope[1] = B[1] - A[1];

            q.submit([&](handler& h) {
                h.parallel_for(len_points, [=](id<1> i) {
                    //Calculate determinant of 2x2 matrix of both vectors
                    det_ptr[i] = borderSlope[0] * ray_ptr[(i*2) + 1] - borderSlope[1] * ray_ptr[(i*2)];
                    if (fabs(det_ptr[i]) > float(1e-5) * hypot(borderSlope[0], borderSlope[1]) * hypot(ray_ptr[i*2], ray_ptr[(i*2) + 1])) {
                        //
                        r_ptr[i] = ((c_ptr[i*2] - A[0]) * ray_ptr[(i*2)+1] - (c_ptr[(2*i)+1] - A[1]) * ray_ptr[i*2]) / det_ptr[i];
                        s_ptr[i] = ((c_ptr[i*2] - A[0]) * borderSlope[1] - (c_ptr[(i*2) + 1] - A[1]) * borderSlope[0]) / det_ptr[i];
                    } else {
                        r_ptr[i] = float(-1.0f);
                        s_ptr[i] = float(-1.0f);
                    }
                });
            });
            q.wait();

            q.submit([&](handler& h) {
                h.parallel_for(len_points, [=](id<1> i) {
                    if (r_ptr[i] >= float(0.0) && r_ptr[i] <= float(1.0) 
                            && s_ptr[i] >= float(0.0) && s_ptr[i] <= float(1.0)) {
                        if (s_ptr[i] < sPref_ptr[i]) {
                            sPref_ptr[i] = s_ptr[i];
                            rPref_ptr[i] = r_ptr[i];
                            bestA_ptr[i*2] = A[0];
                            bestA_ptr[(i*2) + 1] = A[1];
                            bestSlope_ptr[i*2] = borderSlope[0];
                            bestSlope_ptr[(i*2) + 1] = borderSlope[1];
                        }
                    }
                });
            });
            q.wait();
        }
        q.submit([&](handler& h) {
            h.parallel_for(len_points, [=](id<1> i) {
                if (sPref_ptr[i] < float(1000000.0f)) {
                    nml_ptr[(i*2)] = (-1 * bestSlope_ptr[(i*2) + 1]) / hypot(bestSlope_ptr[i*2], bestSlope_ptr[(i*2) + 1]);
                    nml_ptr[(i*2) + 1] = bestSlope_ptr[i*2] / hypot(bestSlope_ptr[i*2], bestSlope_ptr[(i*2) + 1]);

                    dot_ptr[i] = (nml_ptr[i*2] * v_ptr[i*2]) + (nml_ptr[(i*2) + 1] * v_ptr[(i*2) + 1]);

                    if (dot_ptr[i] > 0) {
                        nml_ptr[i*2] = nml_ptr[i*2] * -1;
                        nml_ptr[(i*2) + 1] = nml_ptr[(i*2) + 1] * -1;
                        dot_ptr[i] *= -1;
                    }

                    v_ptr[i*2] -= 2 * dot_ptr[i] * nml_ptr[i*2];
                    v_ptr[(i*2) + 1] -= 2 * dot_ptr[i] * nml_ptr[(i*2) + 1];

                    c_ptr[i*2] = bestA_ptr[i*2] + (rPref_ptr[i] * bestSlope_ptr[i*2]);
                    c_ptr[(i*2) + 1] = bestA_ptr[(i*2) + 1] + (rPref_ptr[i] * bestSlope_ptr[(i*2) + 1]);

                    c_ptr[i*2] -= nml_ptr[i*2] * 1e-4f;
                    c_ptr[(i*2) + 1] -= nml_ptr[(i*2) + 1] * 1e-4f;
                }
            });
        });
        q.wait();
       
        //Get the computed data back into pointers we can use
        q.submit([&](handler& h) {h.memcpy(&positions[0], c_ptr, len_points * 2 * sizeof(float));});
        q.submit([&](handler& h) {h.memcpy(&velocities[0], v_ptr, len_points * 2 * sizeof(float));});
        q.wait();   
            
        free(c_ptr, q);
        free(d_ptr, q);
        free(v_ptr, q);
        free(ray_ptr, q);
        free(det_ptr, q);
        free(bestA_ptr, q);
        free(bestSlope_ptr, q);
        free(r_ptr, q);
        free(s_ptr, q);
        free(rPref_ptr, q);
        free(sPref_ptr, q);
        free(nml_ptr, q);
        free(dot_ptr, q);
        free(t_ptr,q);

    }
    void moveParticles() {
        queue& q = q_shared();
        int len = positions.size();
        float *v_ptr = malloc_shared<float>(2 * len, q);
        float *p_ptr = malloc_shared<float>(2 * len, q);
        float *t_ptr = malloc_shared<float>(1, q);

        q.submit([&](handler& h) {
            h.memcpy(p_ptr, &positions[0], len * 2 * sizeof(float));
        });
        q.wait();
        q.submit([&](handler& h) { h.memcpy(v_ptr, &velocities[0], len * 2 * sizeof(float)); } );
        q.wait();

        q.submit([&](handler& h) { h.memcpy(t_ptr, &t, sizeof(float)); } );
        q.wait();

        q.submit([&](handler& h) { h.parallel_for(len * 2, [=](id<1> i) {
                p_ptr[i] += v_ptr[i] * *t_ptr;
        }); });
        q.wait();

        q.submit([&](handler& h) { h.memcpy(&positions[0], p_ptr, len * 2 * sizeof(float)); });
        q.wait();

        q.submit([&](handler& h) { h.memcpy(&velocities[0], v_ptr, len * 2 * sizeof(float)); });
        q.wait();

        free(p_ptr, q);
        free(v_ptr, q);
        free(t_ptr, q);
    }

};

Polyparti::ParticleField::ParticleField() : colcode(std::make_unique<ColCode>()) { }
Polyparti::ParticleField::~ParticleField() = default;
Polyparti::ParticleField::ParticleField(const ParticleField& other)
    : colcode(std::make_unique<ColCode>(*other.colcode)) {}
Polyparti::ParticleField::ParticleField(ParticleField&&) noexcept = default;
Polyparti::ParticleField& Polyparti::ParticleField::operator=(ParticleField&& other) noexcept = default;
Polyparti::ParticleField& Polyparti::ParticleField::operator=(const ParticleField& other) {
    if (this != &other) { *colcode = *other.colcode; }
    return *this;
}

void Polyparti::ParticleField::addParticle(float x, float y, float vx, float vy) {
    colcode->positions.push_back({x, y});
    colcode->velocities.push_back({vx, vy});
    colcode->count++;
}

void Polyparti::ParticleField::deleteParticle(int index) {
    colcode->positions.erase(colcode->positions.begin() + index);
    colcode->velocities.erase(colcode->velocities.begin() + index);
    colcode->count--;
}

void Polyparti::ParticleField::updateField(std::vector<XYVector> verticies) {
    colcode->checkCollisions(verticies);
    colcode->moveParticles();
    colcode->fieldTime += colcode->t;
    if (colcode->dies) {
        if (colcode->lifeTime >= colcode->fieldTime) {
            colcode->color[3] -= colcode->decay;
        }
        if (colcode->color[3] <= 0) {
            colcode->dead = true;
        }
    }
}

const std::vector<XYVector>& Polyparti::ParticleField::getPositions() const {
    return colcode->positions;
}

int Polyparti::ParticleField::getSize() {
    return colcode->positions.size();
}

void Polyparti::ParticleField::setColor(std::string newColor) {
    colcode->color[3] = 255;
    if (newColor == "red" ) {
        colcode->color[0] = 255;
        colcode->color[1] = 0;
        colcode->color[2] = 0;
    } else if (newColor == "orange") {
        colcode->color[0] = 255;
        colcode->color[1] = 127;
        colcode->color[2] = 0;
    } else if (newColor == "yellow") {
        colcode->color[0] = 255;
        colcode->color[1] = 255;
        colcode->color[2] = 0;
    } else if (newColor == "green") {
        colcode->color[0] = 0;
        colcode->color[1] = 255;
        colcode->color[2] = 0;
    } else if (newColor == "blue") {
        colcode->color[0] = 0;
        colcode->color[1] = 0;
        colcode->color[2] = 255;
    } else if (newColor == "indigo") {
        colcode->color[0] = 31;
        colcode->color[1] = 0;
        colcode->color[2] = 127;
    } else if (newColor == "violet") {
        colcode->color[0] = 127;
        colcode->color[1] = 0;
        colcode->color[2] = 127;
    } else if (newColor == "cyan") {
        colcode->color[0] = 0;
        colcode->color[1] = 255;
        colcode->color[2] = 255;
    } else if (newColor == "darkgreen") {
        colcode->color[0] = 0;
        colcode->color[1] = 127;
        colcode->color[2] = 0;
    } else if (newColor == "magenta") {
        colcode->color[0] = 255;
        colcode->color[1] = 0;
        colcode->color[2] = 255;
    } else {
        colcode->color[0] = 255;
        colcode->color[1] = 255;
        colcode->color[2] = 255;
    }
}

int* Polyparti::ParticleField::getColor() {
    return colcode->color;
}

int Polyparti::ParticleField::getCount() {
    return colcode->count;
}

void Polyparti::ParticleField::setInterval(const float interval) {
    colcode->t = interval;
}

void Polyparti::ParticleField::setDecay(const float t, const float d) {
    if (t != 0) {
        colcode->dies = true;
        colcode->lifeTime = fabs(t);
        colcode->decay = static_cast<int>(fabs(d) * 255);
    }
}

bool Polyparti::ParticleField::isDead() {
    return colcode->dead;
}
struct Polyparti::Room::ImplRoom {
    std::vector<XYVector> corners;
    XYVector midpoint;
//    std::vector<ParticleField> field;
};
Polyparti::Room::Room() : implRoom(std::make_unique<ImplRoom>()) {}

Polyparti::Room::Room(int w, int h) : Room() { screenWidth = w; screenHeight = h;}

Polyparti::Room::Room(int w, int h, int fps) : Room() {
    screenWidth = w;
    screenHeight = h;
    interval = 1.0f/fps;
}
Polyparti::Room::~Room() = default;
Polyparti::Room::Room(const Room& other) 
    : implRoom(std::make_unique<ImplRoom>(*other.implRoom)),
      info(other.info),
      field(other.field),
      x_min(other.x_min),
      x_max(other.x_max),
      y_min(other.y_min),
      y_max(other.y_max){}
Polyparti::Room::Room(Room&& other) noexcept = default;
Polyparti::Room& Polyparti::Room::operator=(const Room& other) {
    if (this != &other) {
        *implRoom = *other.implRoom;
        info = other.info;
        field = other.field;
        x_min = other.x_min;
        x_max = other.x_max;
        y_min = other.y_min;
        y_max = other.y_max;
    }
    return *this;
}

Polyparti::Room& Polyparti::Room::operator=(Room&& other) noexcept = default;


void Polyparti::Room::addCorner(float x, float y) { implRoom->corners.push_back({x,y}); }

void Polyparti::Room::checkCorners() {
    if (implRoom->corners.front() != implRoom->corners.back()) { implRoom->corners.push_back(implRoom->corners.front()); }
}

std::vector<XYVector> Polyparti::Room::getCorners() {
    return implRoom->corners;
}
void Polyparti::Room::loadRoom(std::string name) {
    std::ifstream roomfile;
    std::string line, item;
    bool relative = false;
    float a, b, tot_x = 0, tot_y = 0;
    int n = 0;
    char check;
    roomfile.open(name);
    if (roomfile.is_open()) {check = roomfile.get();}
    else { std::cout << "error" << std::endl;}

    x_min = screenHeight;
    y_min = screenWidth;

    if (check == 'r') { relative = true; }

    if (roomfile.is_open()) {
        while (roomfile >> a >> b) {
            if (relative) { a *= screenWidth; b *= screenHeight; }
            addCorner(a,b);
           tot_x += a;
            tot_y += b;
            n++;
            
            if ( a > x_max) {x_max = a;}
            if ( a < x_min) {x_min = a;}
            if ( b > y_max) {y_max = b;}
            if ( b < y_min) {y_min = b;}
        }
    } else {
        std::cout << "error: rooms/load.room not found" << '\n';
    }
    checkCorners();
    roomfile.close();

    implRoom->midpoint[0] = tot_x / n;
    implRoom->midpoint[1] = tot_y / n;
}

void Polyparti::Room::loadParticles(std::string name) { 
    fieldInfo newEntry;
    std::ifstream partfile(name);
    bool relativeScreen = false;
    bool relativeRoom = false;
    int count;
    float x, y, deg_start, deg_end, life, decay;
    std::string color;

    char check = partfile.get();
    if (check =='s') { relativeScreen = true;}
    else if (check == 'r') { relativeRoom = true; }

    if (partfile.is_open()) {
        while (partfile >> count >> x >> y >> deg_start >> deg_end >> color) {
            if (relativeScreen) {
                x *= screenWidth;
                y *= screenHeight;
            } else if (relativeRoom) {
                x = implRoom->midpoint[0] + (x * (x_max - x_min) / 2);
                y = implRoom->midpoint[1] + (y * (y_max - y_min) / 2);
            }
            newEntry.count = count;
            newEntry.x = x;
            newEntry.y = y;
            newEntry.deg_start = deg_start;
            newEntry.deg_end = deg_end;
            newEntry.color = color;
            //newEntry.life = life;
            //newEntry.decayRate = decay;

            this->info.push_back(newEntry);
        }
    }
    else {
        std::cout << "error: particles/load.prtc not found" << '\n';
    }

}
void Polyparti::Room::initField(Polyparti::Room::fieldInfo info) {
    ParticleField newField;
    double rad = 4 * std::acos(0.0);
    info.deg_start = (info.deg_start / 360) * rad;
    info.deg_end = (info.deg_end/360) * rad;
    for (int i = 0; i < info.count; i++) {
        double angle = (float(i) / info.count) * (info.deg_end - info.deg_start);
        newField.addParticle(info.x, info.y, velocity * (std::cos(angle + info.deg_start)), velocity * (std::sin(angle + info.deg_start)));
    }
    newField.setColor(info.color);
    newField.setInterval(interval);
    newField.setDecay(info.life, info.decayRate);
    field.push_back(std::move(newField));
}

void Polyparti::Room::rotateRoom(float degrees) {
    float rad = (degrees / 360) * 4 * std::acos(0.0);

    /*float tot_x = 0, tot_y = 0;
    int n = implRoom->corners.size();
    for (auto& point : implRoom->corners) {
                tot_x += point[0];
                tot_y += point[1];
            }
    implRoom->midpoint[0] = tot_x / n;
    implRoom->midpoint[1] = tot_y / n;*/

    for (auto& point : implRoom->corners) {
        float temp = point[0];
        point[0] = ((temp - implRoom->midpoint[0]) * std::cos(rad) - (point[1] - (implRoom->midpoint[1])) * std::sin(rad));
        point[1] = ((temp - implRoom->midpoint[0]) * std::sin(rad) + (point[1] - (implRoom->midpoint[1])) * std::cos(rad));
        point[0] += implRoom->midpoint[0];
        point[1] += implRoom->midpoint[1];
        }
    }

void Polyparti::Room::addFields() { for (auto& i: info) initField(i);}

void Polyparti::Room::updateFields() { 
    for (auto& f : field) {
        f.updateField(implRoom->corners);
    }
    field.erase(std::remove_if(field.begin(), field.end(), [](auto& f) { return f.isDead();}), field.end());
}



Polyparti::ParticleField& Polyparti::Room::getField(int index) { return field.at(index); }

void Polyparti::Room::adjustInterval(const float diff) { 
    //float diff = 1.0 + pct;
    interval += diff;
    for (auto &f : field) {
        f.setInterval(diff);
    }
}

void Polyparti::Room::reverseInterval() {
    interval = -interval;
    for (auto &f : field) {
        f.setInterval(interval);
    }
}
int Polyparti::Room::getFieldCount() {
    return field.size();
}