/**
 ******************************************************************************
 *
 * @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 "headingactual.h"
#include "ahrsstatus.h"
#include "alarms.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 update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude);
static void update_heading_actual(struct opahrs_msg_v1_rsp_heading * heading);
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial);

/**
 * Initialise the module, called on startup
 * \returns 0 on success or -1 if initialisation failed
 */
int32_t AHRSCommsInitialize(void)
{
  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_GetHeading(&rsp) == OPAHRS_RESULT_OK) {
	update_heading_actual(&(rsp.payload.user.v.rsp.heading));
      } else {
	/* Comms error */
	break;
      }
    
      /* Wait for the next update interval */
      vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
    }
  }
}

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_heading_actual(struct opahrs_msg_v1_rsp_heading * heading)
{
  HeadingActualData    data;

  data.raw[HEADINGACTUAL_RAW_X] = heading->raw_mag.x;
  data.raw[HEADINGACTUAL_RAW_Y] = heading->raw_mag.y;
  data.raw[HEADINGACTUAL_RAW_Z] = heading->raw_mag.z;
  
  data.heading                  = heading->heading;
  
  HeadingActualSet(&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);
}