#include <iostream>
#include <iomanip>
#include <memory>
#include <random>
#include "AlignedMem.h"
#include "vec.h"

#define C_ALIGN	16
#define SEP	" "
#define N_VEC	18
#define EPS     1.0e-9

#define NEQ(x, y)	(fabs((x) - (y)) > EPS))

using namespace std;

extern void CrossProdAOS_(Vector* c, const Vector* a, const Vector* b, int n);
extern void CrossProdSOA_(VectorSoA& c, const VectorSoA& a, const VectorSoA& b, int n);

void CrossProdAOS(Vector* c, const Vector* a, const Vector* b, int n) {
    for (size_t i = 0; i < n; i++) {
        c[i].X = a[i].Y * b[i].Z - a[i].Z * b[i].Y;
        c[i].Y = a[i].Z * b[i].X - a[i].X * b[i].Z;
        c[i].Z = a[i].X * b[i].Y - a[i].Y * b[i].X;
    }
}

void CrossProdSOA(VectorSoA& c, const VectorSoA& a, const VectorSoA& b, int n) {
    for (size_t i = 0; i < n; i++) {
        c.X[i] = a.Y[i] * b.Z[i] - a.Z[i] * b.Y[i];
        c.Y[i] = a.Z[i] * b.X[i] - a.X[i] * b.Z[i];
        c.Z[i] = a.X[i] * b.Y[i] - a.Y[i] * b.X[i];
    }
}

void InitVec(Vector* p, VectorSoA& q, int n, unsigned short int seed) {
  std::mt19937 mt {seed};
  std::uniform_int_distribution<> rand100 {1, 100};

  for (int i=0; i < n; i++) {
    p[i].X = q.X[i] = (float) rand100(mt);
    p[i].Y = q.Y[i] = (float) rand100(mt);
    p[i].Z = q.Z[i] = (float) rand100(mt);
  }
}


bool CompareCP(Vector* p, Vector* q, int n) {
  for (int i = 0; i < n; i++) {
    if (fabs(p[i].X - q[i].X) > EPS || fabs(p[i].Y - q[i].Y) > EPS || fabs(p[i].Z - q[i].Z) > EPS)
      return false;
  }
  return true;
}

bool CompareCP(VectorSoA& p, VectorSoA& q, int n) {
  for (int i = 0; i < n; i++) {
    if (fabs(p.X[i] - q.X[i]) > EPS || fabs(p.Y[i] - q.Y[i]) > EPS || fabs(p.Z[i] - q.Z[i]) > EPS)
      return false;
  }
  return true;
}

bool CompareCP(Vector* p, VectorSoA& q, int n) {
  for (int i = 0; i < n; i++) {
    if (fabs(p[i].X - q.X[i]) > EPS || fabs(p[i].Y - q.Y[i]) > EPS || fabs(p[i].Z - q.Z[i]) > EPS)
      return false;
  }
  return true;
}

void CrossProd(void) {
    unique_ptr<Vector> a_aos_up { new Vector[N_VEC] };
    unique_ptr<Vector> b_aos_up { new Vector[N_VEC] };
    unique_ptr<Vector> c1_aos_up { new Vector[N_VEC] };
    unique_ptr<Vector> c2_aos_up { new Vector[N_VEC] };
    Vector* a_aos = a_aos_up.get();
    Vector* b_aos = b_aos_up.get();
    Vector* c1_aos = c1_aos_up.get();
    Vector* c2_aos = c2_aos_up.get();
    AlignedArray<float> a_soa_x_aa(N_VEC, C_ALIGN);
    AlignedArray<float> a_soa_y_aa(N_VEC, C_ALIGN);
    AlignedArray<float> a_soa_z_aa(N_VEC, C_ALIGN);
    AlignedArray<float> b_soa_x_aa(N_VEC, C_ALIGN);
    AlignedArray<float> b_soa_y_aa(N_VEC, C_ALIGN);
    AlignedArray<float> b_soa_z_aa(N_VEC, C_ALIGN);
    AlignedArray<float> c1_soa_x_aa(N_VEC, C_ALIGN);
    AlignedArray<float> c1_soa_y_aa(N_VEC, C_ALIGN);
    AlignedArray<float> c1_soa_z_aa(N_VEC, C_ALIGN);
    AlignedArray<float> c2_soa_x_aa(N_VEC, C_ALIGN);
    AlignedArray<float> c2_soa_y_aa(N_VEC, C_ALIGN);
    AlignedArray<float> c2_soa_z_aa(N_VEC, C_ALIGN);
    VectorSoA a_soa, b_soa, c1_soa, c2_soa;
    a_soa.X = a_soa_x_aa.Data();
    a_soa.Y = a_soa_y_aa.Data();
    a_soa.Z = a_soa_z_aa.Data();
    b_soa.X = b_soa_x_aa.Data();
    b_soa.Y = b_soa_y_aa.Data();
    b_soa.Z = b_soa_z_aa.Data();
    c1_soa.X = c1_soa_x_aa.Data();
    c1_soa.Y = c1_soa_y_aa.Data();
    c1_soa.Z = c1_soa_z_aa.Data();
    c2_soa.X = c2_soa_x_aa.Data();
    c2_soa.Y = c2_soa_y_aa.Data();
    c2_soa.Z = c2_soa_z_aa.Data();


    InitVec(a_aos, a_soa, N_VEC, 2021);
    InitVec(b_aos, b_soa, N_VEC, 2022);

    CrossProdAOS(c1_aos, a_aos, b_aos, N_VEC);
    CrossProdAOS_(c2_aos, a_aos, b_aos, N_VEC);
    CrossProdSOA(c1_soa, a_soa, b_soa, N_VEC);
    CrossProdSOA_(c2_soa, a_soa, b_soa, N_VEC);
    bool compare_cp = CompareCP(c1_aos, c2_aos, N_VEC)
      && CompareCP(c1_soa, c2_soa, N_VEC)
      && CompareCP(c1_aos, c1_soa, N_VEC);
    
    cout << "Results for CrossProd\n";
    cout << fixed << setprecision(1);
    for (size_t i = 0; i < N_VEC; i++) {
        const unsigned int w = 7;
        cout << "Vector cross product #" << i << endl;
        cout << "  a:      ";
        cout << setw(w) << a_aos[i].X << SEP;
        cout << setw(w) << a_aos[i].Y << SEP;
        cout << setw(w) << a_aos[i].Z << SEP;
        cout << "  b: ";
        cout << setw(w) << b_aos[i].X << SEP;
        cout << setw(w) << b_aos[i].Y << SEP;
        cout << setw(w) << b_aos[i].Z << endl;
        if (compare_cp)
            cout << "  c:      ";
        else
            cout << "  c1_aos: ";
        cout << setw(w) << c1_aos[i].X << SEP;
        cout << setw(w) << c1_aos[i].Y << SEP;
        cout << setw(w) << c1_aos[i].Z << endl;
        if (!compare_cp) {
            cout << "  c2_aos: ";
            cout << setw(w) << c2_aos[i].X << SEP;
            cout << setw(w) << c2_aos[i].Y << SEP;
            cout << setw(w) << c2_aos[i].Z << endl;
            cout << "  c1_soa: ";
            cout << setw(w) << c1_soa.X[i] << SEP;
            cout << setw(w) << c1_soa.Y[i] << SEP;
            cout << setw(w) << c1_soa.Z[i] << endl;
            cout << "  c2_soa: ";
            cout << setw(w) << c2_soa.X[i] << SEP;
            cout << setw(w) << c2_soa.Y[i] << SEP;
            cout << setw(w) << c2_soa.Z[i] << endl;
        }
    }
}

int main() {
    CrossProd();
    //CrossProd_BM();
    return 0;
}