/**************************************************************** 
 * RQ3Main.cpp
 * based on skeleton created by EPICS script makeBaseApp.pl
 *
 * application main ....
 * 	  initializes rPi I2C, SPI & GPIO interfaces
 * 	  spawns threads to handle I2C & SPI transactions
 * 	  starts EPICS IOC
 *
 *****************************************************************/
#define GLOB_rPi 1

#include <stddef.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <stdio.h>

#include "epicsExit.h"
#include "epicsThread.h"
#include "iocsh.h"
 
#include "rPi.h"

/* 
#ifdef __cplusplus
extern "C" {
#endif
extern  void  devRQ3_Tsk0_Ftn0p1(unsigned char);
#ifdef __cplusplus
}
#endif
*/

int main(int argc,char *argv[])
  {
  int i;
  int stat;

  
  /* Test function */
  /* bcm2835_set_debug(1); */

  /*****  initialize bcm2835 library  *****/
  if(!bcm2835_init())
    {
    printf("bcm2835_init failed - quit\n");
    return(0);
    }


    
  /*****  initialize I2C interface  *****/
  bcm2835_i2c_begin();
  
  /* Select 100 kHz clock rate
   * For a bcm2835 core frequency of 250 MHz, required divider is 2500 */
  bcm2835_i2c_setClockDivider(BCM2835_I2C_CLOCK_DIVIDER_2500);
  
  /* clear I2C queue */  
  for(i = 0; i < RPI_QSIZE; i++) rPiI2Cque[i] = NULL;
  
  /* Mutex to control access to I2C bus */
  pthread_mutex_init(&rPiI2Cbus_MUTX, NULL);

  /* Mutex to control access to I2C queue pointers */
  pthread_mutex_init(&rPiI2Cque_MUTX, NULL);
  
  /* Semaphore to control flow of I2C requests  */
  sem_init(&rPiI2Cque_SEM, 0, 0);
  
  /* create thread to handle I2C queue */
  stat = pthread_create(&rPiI2Cque_TID, NULL, &rPiI2Cque_process, NULL);



  /*****  initialize SPI interface  *****/
  bcm2835_spi_begin();
  
  /* SPI bit order, default, not supported by SPI0 */
  bcm2835_spi_setBitOrder(BCM2835_SPI_BIT_ORDER_MSBFIRST);
  
  /* SPI mode 0: CPOL = 0, CPHA = 0 */
  bcm2835_spi_setDataMode(BCM2835_SPI_MODE0);
  
  /* Select f_clk ~500 kHz.
   * For a bcm2835 core frequency of 250 MHz, required divider is ~512 */
  bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_512);
  
  /* Using chip-select 0 */
  bcm2835_spi_chipSelect(BCM2835_SPI_CS0);
  
  /* CS -> low to select device */
  bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW);
  
  /* clear SPI queue */  
  for(i = 0; i < RPI_QSIZE; i++) rPiSPIque[i] = NULL;
  
  /* Mutex to control access to SPI bus */
  pthread_mutex_init(&rPiSPIbus_MUTX, NULL);

  /* Mutex to control access to SPI queue pointers */
  pthread_mutex_init(&rPiSPIque_MUTX, NULL);
  
  /* Semaphore to control flow of SPI requests  */
  sem_init(&rPiSPIque_SEM, 0, 0);
  
  /* create thread to handle SPI queue */
  stat = pthread_create(&rPiSPIque_TID, NULL, &rPiSPIque_process, NULL);
  
  
  
  /* Set in output mode GPIO pins used to drive,
   * (a) SPIcse multiplexer (4-ch mode)
   * (b) left (right) stepper motor direction
   */
  bcm2835_gpio_fsel(RPI_BPLUS_GPIO_J8_11, BCM2835_GPIO_FSEL_OUTP);
  bcm2835_gpio_fsel(RPI_BPLUS_GPIO_J8_22, BCM2835_GPIO_FSEL_OUTP);
  bcm2835_gpio_fsel(RPI_BPLUS_GPIO_J8_15, BCM2835_GPIO_FSEL_OUTP);
  bcm2835_gpio_fsel(RPI_BPLUS_GPIO_J8_16, BCM2835_GPIO_FSEL_OUTP);
 /* SPIcse (8-chn mode) also needs
  * bcm2835_gpio_fsel(RPI_BPLUS_GPIO_J8_18, BCM2835_GPIO_FSEL_OUTP);
  */

 
 
  /* start EPICS IOC */
  if(argc>=2)
    {    
    iocsh(argv[1]);
    epicsThreadSleep(.2);
    }
  iocsh(NULL);
  epicsExit(0);
	  
  /***** close I2C interface & processes related to it *****/
  /*cancel thread */
  pthread_cancel(rPiI2Cque_TID);
  
  /* remove mutexs & semaphores */
  pthread_mutex_destroy(&rPiI2Cque_MUTX);
  pthread_mutex_destroy(&rPiI2Cbus_MUTX);
  sem_destroy(&rPiI2Cque_SEM);
  
  /* reset interface */
  bcm2835_i2c_end();
  
  
  /***** close SPI interface & processes related to it *****/
  /* cancel thread */
  pthread_cancel(rPiSPIque_TID);
  
  /* remove mutexs & semaphores */
  pthread_mutex_destroy(&rPiSPIque_MUTX);
  pthread_mutex_destroy(&rPiSPIbus_MUTX);
  sem_destroy(&rPiSPIque_SEM);
  
  /* disable step generation */
  /*
//  devRQ3_Tsk0_Ftn0(0);
//  devRQ3_Tsk0_Ftn0(1);
  */
  
  /* reset interface */
  bcm2835_spi_end();
  
  /* close bcm2835 library */
  bcm2835_close();
  
  return(0);
}