#ifndef MAIN_H
#define MAIN_H

/********************************* INCLUDES **********************************/

/* Standard includes */
#include <stdlib.h>
#include <math.h>

/* Scheduler includes */
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"

/* CIIIEmbLibs_rev1 includes */
#include "uart/uart.h"
#include "communication/communication.h"
#include "timer/timer.h"
#include "gpio/gpio.h"
#include "pll/pll.h"
#include "pwm/pwm.h"
#include "irq/irq.h"
#include "include/lpc2114.h"
#include "adc/adc.h"
#include "pid/pid.h"
#include "encoder/encoder.h"

/* RoMAA Application includes */
#include "commands.h"
#include "romaa_params.h"


/******************************** CONSTANTS **********************************/

// defines of RoMAA microcontroller
#define		ROMAA_XTAL                      14.7456e6
#define		ROMAA_TIMEBASE			( 1.0 / ( 4 * ROMAA_XTAL ) )

/* Constants to setup I/O */
#define mainTX_ENABLE        ( ( unsigned portLONG ) 0x0001 )
#define mainRX_ENABLE        ( ( unsigned portLONG ) 0x0004 )
#define mainP0_14            ( ( unsigned portLONG ) 0x4000 )
#define mainJTAG_PORT        ( ( unsigned portLONG ) 0x3E0000UL )

/* Constants to setup the PLL */
#define mainPLL_MUL_4        ( ( unsigned portCHAR ) 0x0003 )        
#define mainPLL_DIV_1        ( ( unsigned portCHAR ) 0x0000 )
#define mainPLL_ENABLE       ( ( unsigned portCHAR ) 0x0001 )
#define mainPLL_CONNECT      ( ( unsigned portCHAR ) 0x0003 )
#define mainPLL_FEED_BYTE1   ( ( unsigned portCHAR ) 0xaa )
#define mainPLL_FEED_BYTE2   ( ( unsigned portCHAR ) 0x55 )
#define mainPLL_LOCK         ( ( unsigned portLONG ) 0x0400 )

/* Constants to setup the MAM */
#define mainMAM_TIM_3        ( ( unsigned portCHAR ) 0x03 )
#define mainMAM_MODE_FULL    ( ( unsigned portCHAR ) 0x02 )

/* Constants to setup the peripheral bus */
#define mainBUS_CLK_FULL     ( ( unsigned portCHAR ) 0x01 )

/* Priorities for the application tasks */
// Mayor prioridad
#define mainTaskPIDLoop_PRIORITY                ( tskIDLE_PRIORITY + 5 )
#define mainTaskOdometry_PRIORITY               ( tskIDLE_PRIORITY + 4 )
#define mainTaskSerialCommunication_PRIORITY    ( tskIDLE_PRIORITY + 3 )
#define mainTaskLogging_PRIORITY                ( tskIDLE_PRIORITY + 2 )
#define mainTaskRTOSALive_PRIORITY              ( tskIDLE_PRIORITY + 1 )
// Menor prioridad

/* Tamaño de vectores para logging*/
#define MAX_FLOAT_DATA            3
#define MAX_INT_DATA              2

/* Modos de lazo*/
#define CLOSE_LOOP  0
#define OPEN_LOOP   1

/* Motores, llaves H y PWM */
#define MOTOR1_ENABLE_PIN        GPIO_PIN0_22
#define MOTOR2_ENABLE_PIN        GPIO_PIN0_27
// garantiza la carga del capacitor de Bootsrap de las llaves H
#define SATURATION_PWM            1450
#define ZERO_OFFSET               1500

/* Vector de comando de comunicación */
#define MAX_COMMAND             20      // Bytes del bufer de recepción
#define MAX_TIMEOUT_COMM        2000.0  // Máx tiempo sin comandos (ms)
#define CONTINUOUS_MODE         0x01    // Modo de comandos (0x01 - Activo)

/* Banderas de logging de datos */
#define LOG_STOP_FLAG           0x00
#define LOG_WHEEL_V_FLAG        0x01
#define LOG_WHEEL_W_FLAG        0x02
#define LOG_WHEEL_D_FLAG        0x04
#define LOG_ENC_COUNTER_FLAG    0x08
#define LOG_PWM_FLAG            0x10
#define LOG_VW_FLAG             0x20
#define LOG_ODOM_FLAG           0x40

/* Sentido de giro de encoders */
#define ROMAA_MODE              0x00
#define ROMAA_II_MODE           0x01

/* Comandos IAP */
#define FLASH_SECTOR_15         0x0003A000 // Desde 0x0003A000 hasta 0x0003BFFF 
#define IAP_LOCATION            0x7ffffff1 // Ubic. del Bootloader en Flash
#define	PREPAIR_WRITE           50         // Prepara sector
#define COPY_RAM_TO_FLASH       51         // Escribe en flash
#define ERASE_SECTORS           52         // Borra sector
#define MCU_CLK_KHZ             58982      // Fcia del micro

/************** VARIABLES, STRUCTURES and UNIONS declarations ****************/
/* Estructuras */
/********** 
Motores ***/
typedef struct {
//  pid_t pid;
  float angular_speed;
  float linear_speed;
  float feedback_cte; 
  int 	zero_offset;    
//  float reference;
} motor_loop_t;

/******************************************* 
Uniones y Variables para Recepcion y Envio */
typedef union {
  unsigned char uc_data[4];
  float f_data;
} uchar_to_float_t;

typedef union {
  unsigned char uc_data[4];
  int i_data;
} uchar_to_int_t;


/********************************************************
Estrucutra para almacenamiento de configuración en Flash*/
typedef struct {
  /* Tiempos de repetición de las tareas */  
  unsigned char loop_pid_ms;
  unsigned char logging_ms;
  unsigned char odometry_ms;
  unsigned char communication_ms;
  unsigned char rtos_alive_ms; 
  /* Variables cinematicas */
  float wheelbase;
  float wheel_radious;
  /* Variables del ADC y medición de batería */
  float VBatteryHigh;
  float VBatteryLow;
  float ADCConvConstant;
  int   checkBattery;
  /* Constantes de los controles PID */
  float v_pid_kp;
  float v_pid_ki;
  float v_pid_kd;
  float vw_pid_kp;
  float vw_pid_ki;
  float vw_pid_kd;
} sInFlashConfiguration;


/*************************** FUNCTION PROTOTYPES *****************************/

/* ---------------------------- Functions ---------------------------------- */
/**
 * \fn static void prvSetupHardware(void) 
 * \brief Funcion para la configuracion de todo el hardware del sistema. 
 *        Incluye setup de las I/O, clock del sistema, tiempos de acceso, 
 *        UART, interrupciones, PWM, etc.
 * \return No retorna ningun valor.
 */
static void prvSetupHardware(void);

/**
 * \fn void loop_motor_init(motor_loop_t* loop_motor, int zero_offset, 
 *     float feedback_cte)
 * \brief Funcion para la inicializacion de las variables de lazo de los
 *        motores.
 * \param[out] loop_motor Una puntero a variable de tipo "motor_loop_t", indica
 *                        el lazo que se va a inicializar.
 * \param[in] zero_offset Una variable de tipo "int" indica la correcion de
 *                        offset.
 * \param[in] feedback_cte Una variable de tipo float que indica la constante
 *                         de realimentacion del lazo.
 * \return No retorna ningun valor.
 */
void loop_motor_init(motor_loop_t*, int, float);

/**
 * \fn void reset_all(void)
 * \brief Funcion de inicializacion de las las variables de los PID, motores, 
 *        periodos de tareas, odometria, entre otros.
 * \return No retorna ningun valor.
 */
void reset_all(void);

/**
 * \fn void init_all(void)
 * \brief Función de creación e inicialización de todos los recursos de 
 *        software a ser utilizados.
 * \return No retorna ningun valor.
 */
void init_all(void);

/* ----------------------------- Callbacks --------------------------------- */

/**
 * \fn c_timer0_capture0(void)
 * \brief Funcion de callback para la interrupcion causada por el capture 0 del
 *        timer 0.
 * \return No retorna ningun valor.
 */
void c_timer0_capture0(void);

/**
 * \fn c_timer0_capture3(void)
 * \brief Funcion de callback para la interrupcion causada por el capture 3 del
 *        timer 0.
 * \return No retorna ningun valor.
 */
void c_timer0_capture3(void);

/* ------------------------------ Tasks ------------------------------------ */
/**
 * \fn static void TaskSerialCommunication( void *pvParameters )
 * \brief Tarea de comunicacion serial RS232 con la PC de abordo.
 * \param[in] pvParameters Una puntero generico (void *) que se utiliza para
 *                         pasar agumentos a una tarea.
 * \return No retorna ningun valor.
 */
static void TaskSerialCommunication( void *pvParameters );

/**
 * \fn static void TaskPIDLoop( void *pvParameters )
 * \brief Tarea de calculo y actualizacion de los valores de los lazos de
 *        control PID.
 * \param[in] pvParameters Una puntero generico (void *) que se utiliza para
 *                         pasar agumentos a una tarea.
 * \return No retorna ningun valor.
 */
static void TaskPIDLoop( void *pvParameters );

/**
 * \fn static void TaskLogging( void *pvParameters )
 * \brief Tarea de actualizacion de los valores de mediciones a devolver
 *        segun la peticion realizada de logging.  
 * \param[in] pvParameters Una puntero generico (void *) que se utiliza para
 *                         pasar agumentos a una tarea.
 * \return No retorna ningun valor.
 */
static void TaskLogging( void *pvParameters );

/**
 * \fn static void TaskOdometry( void *pvParameters )
d * \brief Tarea de calculo de la odometria del RoMAA-II.
 * \param[in] pvParameters Una puntero generico (void *) que se utiliza para
 *                         pasar agumentos a una tarea.
 * \return No retorna ningun valor.
 */
static void TaskOdometry( void *pvParameters );

/**
 * \fn static void TaskRTOSAlive ( void *pvParameters )
 * \brief Tarea de control del correcto funcionamiento del RTOS mediante el 
 *        parpadeo de un LED indicador.
 * \param[in] pvParameters Una puntero generico (void *) que se utiliza para
 *                         pasar agumentos a una tarea.
 * \return No retorna ningun valor.
 */
static void TaskRTOSAlive ( void *pvParameters );


/**
 * \fn unsigned int checkBlankFlash( void )
d * \brief Función que chequea si un sector de la flash está en blanco.
 * \return unsigned int: retorna la respuesta del comando IAP, para determinar
 *                       si el sector está en blanco.
 */
unsigned int checkBlankFlash();


/**
 * \fn void readConfigFromFlash( void )
d * \brief Función que lee de la flash los parámetros de configuración.
 * \return No retorna ningun valor.
 */
void readConfigFromFlash(void);


/**
 * \fn void writeConfigToFlash( void )
d * \brief Función que escribe a la flash los parámetros de configuración.
 * \return No retorna ningun valor.
 */
void writeConfigToFlash(void);


#endif /* MAIN_H */
