#include <BasicLinearAlgebra.h>
using namespace BLA;

// Constants
static const int LED_PIN = 12;

BLA::Matrix<3, 3> create_rotation_x(float deg)
{
  float rad = radians(deg);
  float c = cos(rad);
  float s = sin(rad);
  // [1, 0, 0]
  // [0, c, -s]
  // [0, s, c]
  BLA::Matrix<3, 3> rotx = {1.0, 0, 0, 0, c, -s, 0, s, c};
  return rotx;
}

void setup() {

  // Welcome Message
  Serial.begin(115200);
  Serial.println("Arduino MEGA. . . !");

  // Setup
  pinMode(LED_PIN, OUTPUT);

  // Test
  digitalWrite(LED_PIN, LOW);
  delay(3000);
  digitalWrite(LED_PIN, HIGH);
  delay(3000);

  // Test rotation
  float deg = 30;
  Matrix<3,3> rotx = create_rotation_x(deg);
  Serial << "rotx matrix org: " << rotx << '\n';

  // Test time of calculation
  BLA::Matrix<3,3> test_mat = rotx;
  int loops_count = 1000;
  unsigned int start_time = micros();
  unsigned int start_time_ms = millis();
  for(int i = 0 ; i< loops_count; i++)
  {
    test_mat *= rotx;
  }
  unsigned long end_time = micros();
  unsigned long end_time_ms = millis();
  auto dur = end_time - start_time;
  auto dur_ms = end_time_ms - start_time_ms;
  Serial << "rotx matrix: " << rotx << '\n';
  Serial << "test_mat matrix: " << test_mat << '\n';
  Serial << "loops: " << String(loops_count).c_str() << "\n";
  Serial << "duration: " << String(dur).c_str() << " micros, " << String(dur_ms).c_str() << " ms" << "\n";
}

void loop() {
  // put your main code here, to run repeatedly:

}