/**
 * MK & MK4due 3D Printer Firmware
 *
 * Based on Marlin, Sprinter and grbl
 * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
 * Copyright (C) 2013 - 2016 Alberto Cotronei @MagoKimbra
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

/**
 * vector_3.cpp - Vector library for bed leveling
 * Copyright (c) 2012 Lars Brubaker.  All right reserved.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "../../base.h"

#if ENABLED(AUTO_BED_LEVELING_FEATURE)
  #include <math.h>
  #include "vector_3.h"

  vector_3::vector_3() : x(0), y(0), z(0) { }

  vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }

  vector_3 vector_3::cross(vector_3 left, vector_3 right) {
    return vector_3(left.y * right.z - left.z * right.y,
                    left.z * right.x - left.x * right.z,
                    left.x * right.y - left.y * right.x);
  }

  vector_3 vector_3::operator+(vector_3 v) { return vector_3((x + v.x), (y + v.y), (z + v.z)); }
  vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }

  vector_3 vector_3::get_normal() {
    vector_3 normalized = vector_3(x, y, z);
    normalized.normalize();
    return normalized;
  }

  float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }

  void vector_3::normalize() {
    float length = get_length();
    x /= length;
    y /= length;
    z /= length;
  }

  void vector_3::apply_rotation(matrix_3x3 matrix) {
    float resultX = x * matrix.matrix[0][0] + y * matrix.matrix[1][0] + z * matrix.matrix[2][0];
    float resultY = x * matrix.matrix[0][1] + y * matrix.matrix[1][1] + z * matrix.matrix[2][1];
    float resultZ = x * matrix.matrix[0][2] + y * matrix.matrix[1][2] + z * matrix.matrix[2][2];
    x = resultX;
    y = resultY;
    z = resultZ;
  }

  void vector_3::debug(const char title[]) {
    ECHO_ST(DB, title);
    ECHO_MV(" x: ", x, 6);
    ECHO_MV(" y: ", y, 6);
    ECHO_EMV(" z: ", z, 6);
  }

  void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
    vector_3 vector = vector_3(x, y, z);
    vector.apply_rotation(matrix);
    x = vector.x;
    y = vector.y;
    z = vector.z;
  }

  matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
    //row_0.debug("row_0");
    //row_1.debug("row_1");
    //row_2.debug("row_2");
    matrix_3x3 new_matrix;
    new_matrix.matrix[0][0] = row_0.x; new_matrix.matrix[0][1] = row_0.y; new_matrix.matrix[0][2] = row_0.z;
    new_matrix.matrix[1][0] = row_1.x; new_matrix.matrix[1][1] = row_1.y; new_matrix.matrix[1][2] = row_1.z;
    new_matrix.matrix[2][0] = row_2.x; new_matrix.matrix[2][1] = row_2.y; new_matrix.matrix[2][2] = row_2.z;
    //new_matrix.debug("new_matrix");
    return new_matrix;
  }

  void matrix_3x3::set_to_identity() {
    matrix[0][0] = 1; matrix[0][1] = 0; matrix[0][2] = 0;
    matrix[1][0] = 0; matrix[1][1] = 1; matrix[1][2] = 0;
    matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1;
  }

  matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
    vector_3 z_row = target.get_normal();
    vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal();
    vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();

    // x_row.debug("x_row");
    // y_row.debug("y_row");
    // z_row.debug("z_row");

    // create the matrix already correctly transposed
    matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);

    // rot.debug("rot");
    return rot;
  }

  matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
    matrix_3x3 new_matrix;
    new_matrix.matrix[0][0] = original.matrix[0][0]; new_matrix.matrix[0][1] = original.matrix[1][0]; new_matrix.matrix[0][2] = original.matrix[2][0];
    new_matrix.matrix[1][0] = original.matrix[0][1]; new_matrix.matrix[1][1] = original.matrix[1][1]; new_matrix.matrix[1][2] = original.matrix[2][1];
    new_matrix.matrix[2][0] = original.matrix[0][2]; new_matrix.matrix[2][1] = original.matrix[1][2]; new_matrix.matrix[2][2] = original.matrix[2][2];
    return new_matrix;
  }

  void matrix_3x3::debug(const char title[]) {
    ECHO_LT(DB, title);
    for (uint8_t i = 0; i < 3; i++) {
      ECHO_S(DB);
      for (uint8_t j = 0; j < 3; j++) {
        if (matrix[i][j] >= 0.0) ECHO_C('+');
        ECHO_V(matrix[i][j], 6);
        ECHO_C(' ');
      }
      ECHO_E;
    }
  }

#endif // AUTO_BED_LEVELING_FEATURE