/*
 * sensor.c
 *
 *  Created on: Feb 20, 2016
 *      Author: ucart
 */
 
#include "sensor.h"
#include "communication.h"
#include "commands.h"
#include "type_def.h"

int sensor_init(hardware_t *hardware_struct, raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
{
  struct IMUDriver *imu = &hardware_struct->imu;
  struct LidarDriver *lidar = &hardware_struct->lidar;
  struct OpticalFlowDriver *of = &hardware_struct->of;

  if (imu->reset(imu, &raw_sensor_struct->gam)) {
    return -1;
  }
  if (lidar->reset(lidar, &raw_sensor_struct->lidar)) {
     return -1;
  }
  if (of->reset(of, &raw_sensor_struct->optical_flow)) {
     return -1;
  }

  imu->read(imu, &raw_sensor_struct->gam);

  // Sets the first iteration to be at the accelerometer value since gyro initializes to {0,0,0} regardless of orientation
  sensor_struct->pitch_angle_filtered = raw_sensor_struct->gam.accel_roll;
  sensor_struct->roll_angle_filtered = raw_sensor_struct->gam.accel_pitch;

  return 0;
}

int get_sensors(hardware_t *hardware_struct, log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t* raw_sensor_struct)
{
  struct IMUDriver *imu = &hardware_struct->imu;
  struct LidarDriver *lidar = &hardware_struct->lidar;
  struct OpticalFlowDriver *of = &hardware_struct->of;
  int status = 0;

  status |= imu->read(imu, &raw_sensor_struct->gam);
  static lidar_t lidar_val;
  int lidar_status = lidar->read(lidar, &lidar_val);
  if (lidar_status == 0) {
		raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
  }
  status |= lidar_status;
  
  status |= of->read(of, &raw_sensor_struct->optical_flow);
  log_struct->gam = raw_sensor_struct->gam;

  return 0;
}