/*
 * Copyright 2016 <Admobilize>
 * MATRIX Labs  [http://creator.matrix.one]
 * This file is part of MATRIX Creator HAL
 *
 * MATRIX Creator HAL 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/>.
 */

#include <math.h>
#include <iostream>
#include <limits>
#include <vector>

#include "../cpp/driver/everloop.h"
#include "../cpp/driver/everloop_image.h"
#include "../cpp/driver/imu_data.h"
#include "../cpp/driver/imu_sensor.h"
#include "../cpp/driver/wishbone_bus.h"

namespace hal = matrix_hal;

int main() {
  float mag_max_x = std::numeric_limits<float>::min();
  float mag_max_y = std::numeric_limits<float>::min();
  float mag_min_x = std::numeric_limits<float>::max();
  float mag_min_y = std::numeric_limits<float>::max();

  float mag_off_x = 0;
  float mag_off_y = 0;
  float mag_x = 0;
  float mag_y = 0;

  bool xy_valid = false;

  float amp_distance_xy = 0;
  float offset_distance_xy = 0;
  float xy_rot = 0;
  int xy_angle_index = 0;

  hal::WishboneBus bus;
  bus.SpiInit();

  hal::IMUSensor imu_sensor;
  imu_sensor.Setup(&bus);
  hal::IMUData imu_data;

  hal::Everloop everloop;
  everloop.Setup(&bus);
  hal::EverloopImage image1d;

  while (true) {
    imu_sensor.Read(&imu_data);

    mag_x = imu_data.mag_x;
    mag_y = imu_data.mag_y;

    mag_max_x = (mag_x > mag_max_x) ? mag_x : mag_max_x;
    mag_min_x = (mag_x < mag_min_x) ? mag_x : mag_min_x;
    mag_max_y = (mag_y > mag_max_y) ? mag_y : mag_max_y;
    mag_min_y = (mag_y < mag_min_y) ? mag_y : mag_min_y;

    mag_off_x = (mag_max_x + mag_min_x) / 2;
    mag_off_y = (mag_max_y + mag_min_y) / 2;

    // validating : Waiting to have enough values to start calculating angle.
    amp_distance_xy = sqrt(pow(mag_min_x - mag_max_x, 2) + pow(mag_min_y - mag_max_y, 2));
    offset_distance_xy = sqrt(pow(mag_off_x, 2) + pow(mag_off_y, 2));
    xy_valid = offset_distance_xy / amp_distance_xy < 5;

    if (xy_valid) {
      // from rad to 0-360 deg
      xy_rot = atan2(mag_y - mag_off_y, mag_x - mag_off_x) * 180 / M_PI + 180;
      // saturation in 360 range
      xy_angle_index = fmin(xy_rot, 359);
      // saturation in 0 range
      xy_angle_index = fmax(xy_rot, 0);
      // making 24 slots of 360/24 = 15 deg
      xy_angle_index = xy_angle_index / 10;
    }

    for (hal::LedValue& led : image1d.leds)
      led.red = led.green = led.blue = led.white = 0;

    image1d.leds[((35 - xy_angle_index)+17)%34].red = 100;
    everloop.Write(&image1d);
  }
  return 0;
}