/**
 ******************************************************************************
 * @addtogroup OpenPilotModules OpenPilot Modules
 * @{ 
 * @addtogroup AHRSCommsModule AHRSComms Module
 * @brief Handles communication with AHRS and updating position
 * Specifically updates the the @ref AttitudeActual "AttitudeActual" and @ref AttitudeRaw "AttitudeRaw" settings objects
 * @{ 
 *
 * @file       ahrs_comms.c
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @brief      Module to handle all comms to the AHRS on a periodic basis.
 *
 * @see        The GNU Public License (GPL) Version 3
 *
 ******************************************************************************/
/*
 * 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, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

/**
 * Input object: AttitudeSettings
 * Output object: AttitudeActual
 *
 * This module will periodically update the value of latest attitude solution
 * that is available from the AHRS.
 * The module settings can configure how often AHRS is polled for a new solution.
 *
 * The module executes in its own thread.
 *
 * UAVObjects are automatically generated by the UAVObjectGenerator from
 * the object definition XML file.
 *
 * Modules have no API, all communication to other modules is done through UAVObjects.
 * However modules may use the API exposed by shared libraries.
 * See the OpenPilot wiki for more details.
 * http://www.openpilot.org/OpenPilot_Application_Architecture
 *
 */

#include "ahrs_comms.h"
#include "attitudeactual.h"
#include "attitudesettings.h"
#include "attituderaw.h"
#include "ahrsstatus.h"
#include "alarms.h"
#include "altitudeactual.h"
#include "stdbool.h"
#include "positionactual.h"

#include "pios_opahrs.h" // library for OpenPilot AHRS access functions
#include "pios_opahrs_proto.h"

// Private constants
#define STACK_SIZE 400
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)

// Private types

// Private variables
static xTaskHandle taskHandle;

// Private functions
static void ahrscommsTask(void* parameters);
static void load_altitude_actual(struct opahrs_msg_v1_req_altitude * altitude);
static void load_position_actual(struct opahrs_msg_v1_req_gps * gps);
static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude);
static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw);
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial);

static bool AltitudeActualIsUpdatedFlag = false;
static void AltitudeActualUpdatedCb(UAVObjEvent * ev)
{
  AltitudeActualIsUpdatedFlag = true;
}

static bool PositionActualIsUpdatedFlag = false;
static void PositionActualUpdatedCb(UAVObjEvent * ev)
{
	PositionActualIsUpdatedFlag = true;
}

/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t AHRSCommsInitialize(void)
{
  AltitudeActualConnectCallback(AltitudeActualUpdatedCb);
  PositionActualConnectCallback(PositionActualUpdatedCb);

  PIOS_OPAHRS_Init();

  // Start main task
  xTaskCreate(ahrscommsTask, (signed char*)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);

  return 0;
}

/**
 * Module thread, should not return.
 */
static void ahrscommsTask(void* parameters)
{
  // Main task loop
  while (1) {
    struct opahrs_msg_v1 rsp;

    AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);

    /* Spin here until we're in sync */
    while (PIOS_OPAHRS_resync() != OPAHRS_RESULT_OK) {
      vTaskDelay(100 / portTICK_RATE_MS);
    }
      
    /* Give the other side a chance to keep up */
    //vTaskDelay(100 / portTICK_RATE_MS);

    if (PIOS_OPAHRS_GetSerial(&rsp) == OPAHRS_RESULT_OK) {
      update_ahrs_status(&(rsp.payload.user.v.rsp.serial));
    } else {
      /* Comms error */
      continue;
    }

    AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);

    /* We're in sync with the AHRS, spin here until an error occurs */
    while (1) {
      AttitudeSettingsData settings;

      /* Update settings with latest value */
      AttitudeSettingsGet(&settings);
  
      if (PIOS_OPAHRS_GetAttitude(&rsp) == OPAHRS_RESULT_OK) {
        update_attitude_actual(&(rsp.payload.user.v.rsp.attitude));
      } else {
        /* Comms error */
        break;
      }

      if (PIOS_OPAHRS_GetAttitudeRaw(&rsp) == OPAHRS_RESULT_OK) {
	      update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
      } else {
        /* Comms error */
        break;
      }

      if (AltitudeActualIsUpdatedFlag) {
        struct opahrs_msg_v1 req;

        load_altitude_actual(&(req.payload.user.v.req.altitude));
        if (PIOS_OPAHRS_SetAltitudeActual(&req) == OPAHRS_RESULT_OK) {
          AltitudeActualIsUpdatedFlag = false;
        } else {
          /* Comms error */
        }
      }

      if (PositionActualIsUpdatedFlag) {
        struct opahrs_msg_v1 req;
        
        load_position_actual(&(req.payload.user.v.req.gps));
        if (PIOS_OPAHRS_SetPositionActual(&req) == OPAHRS_RESULT_OK) {
          PositionActualIsUpdatedFlag = false;
        } else {
          /* Comms error */
        }
      }
      
      /* Wait for the next update interval */
      vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
    }
  }
}

static void load_altitude_actual(struct opahrs_msg_v1_req_altitude * altitude)
{
  AltitudeActualData   data;

  AltitudeActualGet(&data);

  altitude->altitude    = data.Altitude;
  altitude->pressure    = data.Pressure;
  altitude->temperature = data.Temperature;
}

static void load_position_actual(struct opahrs_msg_v1_req_gps * gps)
{
  PositionActualData data;
  PositionActualGet(&data);
  
  gps->latitude = data.Latitude;
	gps->longitude = data.Longitude;
	gps->altitude = data.GeoidSeparation;
  gps->heading = data.Heading;
  gps->groundspeed = data.Groundspeed;
  gps->status = data.Status;
}

static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude)
{
  AttitudeActualData   data;

  data.q1 = attitude->quaternion.q1;
  data.q2 = attitude->quaternion.q2;
  data.q3 = attitude->quaternion.q3;
  data.q4 = attitude->quaternion.q4;
  
  data.Roll  = attitude->euler.roll;
  data.Pitch = attitude->euler.pitch;
  data.Yaw   = attitude->euler.yaw;
  
  AttitudeActualSet(&data);
}

static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw)
{
  AttitudeRawData    data;

  data.magnetometers[ATTITUDERAW_MAGNETOMETERS_X] = attituderaw->mags.x;
  data.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y] = attituderaw->mags.y;
  data.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z] = attituderaw->mags.z;

  data.gyros[ATTITUDERAW_GYROS_X] = attituderaw->gyros.x;
  data.gyros[ATTITUDERAW_GYROS_Y] = attituderaw->gyros.y;
  data.gyros[ATTITUDERAW_GYROS_Z] = attituderaw->gyros.z;

  data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = attituderaw->gyros_filtered.x;
  data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = attituderaw->gyros_filtered.y;
  data.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Z] = attituderaw->gyros_filtered.z;
  
  data.gyrotemp[ATTITUDERAW_GYROTEMP_XY] = attituderaw->gyros.xy_temp;
  data.gyrotemp[ATTITUDERAW_GYROTEMP_Z] = attituderaw->gyros.z_temp;

  data.accels[ATTITUDERAW_ACCELS_X] = attituderaw->accels.x;
  data.accels[ATTITUDERAW_ACCELS_Y] = attituderaw->accels.y;
  data.accels[ATTITUDERAW_ACCELS_Z] = attituderaw->accels.z;

  data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_X] = attituderaw->accels_filtered.x;
  data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Y] = attituderaw->accels_filtered.y;
  data.accels_filtered[ATTITUDERAW_ACCELS_FILTERED_Z] = attituderaw->accels_filtered.z;
  
  AttitudeRawSet(&data);
}

static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial)
{
  AhrsStatusData       data;

  // Get the current object data
  AhrsStatusGet(&data);

  for (uint8_t i = 0; i < sizeof(serial->serial_bcd); i++) {
    data.SerialNumber[i] = serial->serial_bcd[i];
  }

  AhrsStatusSet(&data);
}

/**
  * @}
  * @}
  */