/*
 * Copyright 2018 Google Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mgos.h"
#include "mgos_mpu9250_internal.h"
#include "mgos_i2c.h"

// Datasheet:
//

// Private functions follow
static bool mgos_mpu9250_ak8963_init(struct mgos_mpu9250 *imu, uint8_t i2caddr) {
  int device_id;

  if (!imu) {
    return false;
  }

  imu->i2caddr_ak8963 = i2caddr;

  device_id = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_WHO_AM_I);
  if (device_id != MGOS_MPU9250_DEVID_AK8963) {
    return false;
  }
  LOG(LL_INFO, ("Detected AK8963 at I2C 0x%02x", i2caddr));

  mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x00);
  mgos_usleep(10000);

  mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x0F);
  mgos_usleep(10000);

  uint8_t data[3];
  if (!mgos_i2c_read_reg_n(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_ASAX, 3, data)) {
    LOG(LL_ERROR, ("Could not read magnetometer adjustment registers"));
    return false;
  }
  imu->mag_adj[0] = (float)(data[0] - 128) / 256. + 1.;
  imu->mag_adj[1] = (float)(data[1] - 128) / 256. + 1.;
  imu->mag_adj[2] = (float)(data[2] - 128) / 256. + 1.;
  LOG(LL_DEBUG, ("magnetometer adjustment %.2f %.2f %.2f", imu->mag_adj[0], imu->mag_adj[1], imu->mag_adj[2]));

  mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x00);
  mgos_usleep(10000);

  // Set magnetometer data resolution and sample ODR
  mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x16);
  mgos_usleep(10000);

  return true;
}

// Private functions end

// Public functions follow
struct mgos_mpu9250 *mgos_mpu9250_create(struct mgos_i2c *i2c, uint8_t i2caddr) {
  struct mgos_mpu9250 *imu;
  int device_id;

  if (!i2c) {
    return NULL;
  }

  imu = calloc(1, sizeof(struct mgos_mpu9250));
  if (!imu) {
    return NULL;
  }
  imu->i2caddr = i2caddr;
  imu->i2c     = i2c;

  device_id = mgos_i2c_read_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_WHO_AM_I);
  switch (device_id) {
  case MGOS_MPU9250_DEVID_9250:
    LOG(LL_INFO, ("Detected MPU9250 at I2C 0x%02x", i2caddr));
    break;

  case MGOS_MPU9250_DEVID_9255:
    LOG(LL_INFO, ("Detected MPU9255 at I2C 0x%02x", i2caddr));
    break;

  default:
    LOG(LL_ERROR, ("Failed to detect MPU9250 at I2C 0x%02x (device_id=0x%02x)", i2caddr, device_id));
    free(imu);
    return NULL;
  }

  // Reset
  mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_PWR_MGMT_1, 0x80);
  mgos_usleep(100000);

  // Enable imus
  mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_PWR_MGMT_2, 0x00);

  // Magnetometer enable
  mgos_i2c_write_reg_b(i2c, i2caddr, MGOS_MPU9250_REG_INT_PIN_CFG, 0x02);

  // TODO(pim): is the mag always on 0x0C ?
  if (false == (imu->mag_enabled = mgos_mpu9250_ak8963_init(imu, MGOS_AK8963_DEFAULT_I2CADDR))) {
    LOG(LL_ERROR, ("Could not detect/initialize AK8963 magnetometer, disabling"));
  }

  return imu;
}

void mgos_mpu9250_destroy(struct mgos_mpu9250 **imu) {
  if (!*imu) {
    return;
  }
  free(*imu);
  *imu = NULL;
  return;
}

bool mgos_mpu9250_set_accelerometer_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_accelerometer_range range) {
  int val;

  if (!imu) {
    return false;
  }

  if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG)) < 0) {
    return false;
  }
  val &= 0xE7; // 11100111
  val |= range << 3;

  return mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG, val);
}

bool mgos_mpu9250_get_accelerometer_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_accelerometer_range *range) {
  int val;

  if (!imu) {
    return false;
  }

  if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_ACCEL_CONFIG)) < 0) {
    return false;
  }
  val   &= 0x18; // 00011000
  val  >>= 3;
  *range = val;
  return true;
}

bool mgos_mpu9250_get_accelerometer(struct mgos_mpu9250 *imu, float *x, float *y, float *z) {
  uint8_t data[6];
  int16_t ax, ay, az;
  enum mgos_mpu9250_accelerometer_range acc_range;
  uint16_t divider;

  if (!imu) {
    return false;
  }
  if (!mgos_mpu9250_get_accelerometer_range(imu, &acc_range)) {
    return false;
  }
  if (!mgos_i2c_read_reg_n(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_ACCEL_XOUT_H, 6, data)) {
    return false;
  }
  ax = (data[0] << 8) | (data[1]);
  ay = (data[2] << 8) | (data[3]);
  az = (data[4] << 8) | (data[5]);
//  LOG(LL_DEBUG, ("ax=%d ay=%d az=%d", ax, ay, az));

  switch (acc_range) {
  case RANGE_16G: divider = 2048; break;

  case RANGE_8G: divider = 4096; break;

  case RANGE_4G: divider = 8192; break;

  case RANGE_2G: divider = 16384; break;

  default: return false;
  }
  *x = (float)ax / divider;
  *y = (float)ay / divider;
  *z = (float)az / divider;

  return true;
}

bool mgos_mpu9250_set_gyroscope_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_gyroscope_range range) {
  int val;

  if (!imu) {
    return false;
  }

  if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG)) < 0) {
    return false;
  }
  val &= 0xE7; // 11100111
  val |= range << 3;

  return mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG, val);
}

bool mgos_mpu9250_get_gyroscope_range(struct mgos_mpu9250 *imu, enum mgos_mpu9250_gyroscope_range *range) {
  int val;

  if (!imu) {
    return false;
  }

  if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_GYRO_CONFIG)) < 0) {
    return false;
  }
  val   &= 0x18; // 00011000
  val  >>= 3;
  *range = val;
  return true;
}

bool mgos_mpu9250_get_gyroscope(struct mgos_mpu9250 *imu, float *x, float *y, float *z) {
  uint8_t data[6];
  int16_t gx, gy, gz;
  enum mgos_mpu9250_gyroscope_range gyr_range;
  float divider;

  if (!imu) {
    return false;
  }
  if (!mgos_mpu9250_get_gyroscope_range(imu, &gyr_range)) {
    return false;
  }
  if (!mgos_i2c_read_reg_n(imu->i2c, imu->i2caddr, MGOS_MPU9250_REG_GYRO_XOUT_H, 6, data)) {
    return false;
  }
  gx = (data[0] << 8) | (data[1]);
  gy = (data[2] << 8) | (data[3]);
  gz = (data[4] << 8) | (data[5]);
//  LOG(LL_DEBUG, ("gx=%d gy=%d gz=%d", gx, gy, gz));

  switch (gyr_range) {
  case RANGE_GYRO_2000: divider = 16.4; break;

  case RANGE_GYRO_1000: divider = 32.8; break;

  case RANGE_GYRO_500: divider = 65.5; break;

  case RANGE_GYRO_250: divider = 131.0; break;

  default: return false;
  }
  *x = (float)gx / divider;
  *y = (float)gy / divider;
  *z = (float)gz / divider;
  return true;
}

bool mgos_mpu9250_set_magnetometer_scale(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_scale scale) {
  int val;

  if (!imu || !imu->mag_enabled) {
    return false;
  }

  if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL)) < 0) {
    return false;
  }
  val &= 0x06;
  mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, 0x00);
  mgos_usleep(10000);

  mgos_i2c_write_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL, (scale << 4) | val);
  mgos_usleep(10000);

  return true;
}

bool mgos_mpu9250_get_magnetometer_scale(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_scale *scale) {
  int val;

  if (!imu || !imu->mag_enabled) {
    return false;
  }
  if ((val = mgos_i2c_read_reg_b(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_CNTL)) < 0) {
    return false;
  }
  *scale = (val >> 4) & 0x01;
  return true;
}

bool mgos_mpu9250_set_magnetometer_speed(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_speed speed) {
  if (!imu || !imu->mag_enabled) {
    return false;
  }
  return false;
}

bool mgos_mpu9250_get_magnetometer_speed(struct mgos_mpu9250 *imu, enum mgos_mpu9250_magnetometer_speed *speed) {
  if (!imu || !imu->mag_enabled) {
    return false;
  }
  return false;
}

bool mgos_mpu9250_get_magnetometer(struct mgos_mpu9250 *imu, float *x, float *y, float *z) {
  uint8_t data[7];
  int16_t mx, my, mz;
  enum mgos_mpu9250_magnetometer_scale mag_scale;
  float divider;

  if (!imu || !imu->mag_enabled) {
    return false;
  }
  if (!mgos_mpu9250_get_magnetometer_scale(imu, &mag_scale)) {
    return false;
  }
  if (!mgos_i2c_read_reg_n(imu->i2c, imu->i2caddr_ak8963, MGOS_MPU9250_REG_AK8963_XOUT_L, 7, data)) {
    return false;
  }
  if (data[6] & 0x08) {
    return false;
  }

  mx = (data[1] << 8) | (data[0]);
  my = (data[3] << 8) | (data[2]);
  mz = (data[5] << 8) | (data[4]);
//  LOG(LL_DEBUG, ("mx=%d my=%d mz=%d", mx, my, mz));

  switch (mag_scale) {
  case SCALE_14_BITS: divider = 8190.0; break;

  case SCALE_16_BITS: divider = 32760.0; break;

  default: return false;
  }
  *x = (float)mx * 4912.0 * imu->mag_adj[0] / divider;
  *y = (float)my * 4912.0 * imu->mag_adj[1] / divider;
  *z = (float)mz * 4912.0 * imu->mag_adj[2] / divider;
  return true;
}

bool mgos_mpu9250_i2c_init(void) {
  return true;
}

// Public functions end