#ifndef COMPUTE_H
#define COMPUTE_H
#include "usart.h"
#include "print.h"
#ifdef __linux__
#include <stdio.h>
#include <math.h>
#define Debug ::printf
#else
#define Debug(...);
#endif


extern void ReadData (int * data, const int len);
extern float msqrtf (const float x);
extern float sincos (const float x, const bool even);

static constexpr unsigned N = 3u;

class Vector {
  float data [N];
public:
  explicit Vector () { for (unsigned n=0u; n<N; n++) data [n] = 0.0f; }
  explicit Vector (const int * ptr, const unsigned n = N) {
    for (unsigned i=0u; i<n; i++) {
      data [i] = static_cast<float>(ptr [i]);
    }
  }
  float & operator[] (const unsigned n) { return data [n]; }
  float abs () const {
    float r = 0.0f;
    for (unsigned n=0u; n<N; n++) {
      const float e = data [n];
      r += e * e;
    }
    return msqrtf (r);
  }
};
struct Matrix {
  float data [N][N];
  Vector operator* (Vector & v) const {
    Vector r;
    for (unsigned i=0; i<N; i++) {
      for (unsigned j=0; j<N; j++) {
        r[j] += data [i][j] * v [i];
      }
    }
    return r;
  }
};

class Compute {
  Usart usart;
  Print cout;
  unsigned passcnt;
  public:
    explicit Compute () noexcept : usart (115200), cout (DEC), passcnt (0u) {
      cout += usart;
    }
    void multiply_test () {
      cout << "                        \rBegin tests ...\r\n";
      // Rotace o 45 deg v rovině xy.
      const Matrix m { .data = {{0.707f, 0.707f, 0.0f}, {-0.707f, 0.707f, 0.0f}, {0.0f, 0.0f, 1.0f}} };
      int x [N] = {1, 1, 2};
      Vector v (x);
      Vector r = m * v;
      cout << "origin: x=" << v[0] << ", y=" << v[1] << ", z=" << v[2] << EOL;
      cout << "result: x=" << r[0] << ", y=" << r[1] << ", z=" << r[2] << EOL;
    }
    void sincos_test () {
      constexpr float x = 3.1415926f / 6.0f;
      const float s = sincos(x, true);
      const float c = sincos(x, false);
      cout << "sin (" << x << ") = " << s << EOL;
      cout << "cos (" << x << ") = " << c << EOL;
      cout << "ctrl:  sin^2 + cos^2 = " << s * s + c * c << EOL;
    }
    bool pass () {
      int x [N];
      ReadData(x, N);
      Vector v (x);
      cout << v.abs() << EOL;
      passcnt += 1;
      if (passcnt > 20) return true;
      return false;
    }
};

#endif // COMPUTE_H