void prepareForSensorRead(void)
{
    //OpenSPI(SPI_FOSC_64, MODE_00, SMPEND);

    //Initialisation des ports de sélection des capteurs
    LATA |= 0x3F; // Waiting state is high
    ADCON1 |= 0x0F;
    TRISA &= 0xC0; // Ports en sortie

    DATA_DIRECTION = 1; // Data in
    CLOCK_PORT = 1;
    CLOCK_DIRECTION = 0; // RB1 en sortie

    OpenTimer1(     TIMER_INT_OFF
                &   T1_16BIT_RW
                &   T1_SOURCE_INT
                &   T1_PS_1_2
                &   T1_OSC1EN_OFF
                &   T1_SYNC_EXT_OFF); // Used to measure time between measurements

    IPR1bits.TMR2IP = 0;

    OpenTimer2(     TIMER_INT_ON
                &   T2_PS_1_16
                &   T2_POST_1_16);   // Used to specify time between measurements

    isSensorReadReady = TRUE;

    //shouldUpdateSensors = TRUE;
}

void ServoInitFunc(void) {
    servo(0);

    ConfigIntTimer1(T1_INT_PRIOR_1 & T1_INT_ON);
    //1msec(4/80Mhz×1×200=0.01msec)
    OpenTimer1(T1_ON & T1_GATE_OFF & T1_PS_1_1 & T1_SYNC_EXT_OFF & T1_SOURCE_INT,200-1);
}

/*
 * Setup a timer for a regular tick.
 */
void prvSetupTimerInterrupt( void )
{
const unsigned long ulCompareMatch = ( (configPERIPHERAL_CLOCK_HZ / portTIMER_PRESCALE) / configTICK_RATE_HZ ) - 1;

	OpenTimer1( ( T1_ON | T1_PS_1_8 | T1_SOURCE_INT ), ulCompareMatch );
	ConfigIntTimer1( T1_INT_ON | configKERNEL_INTERRUPT_PRIORITY );
}

/*! **********************************************************************
 * Function: configureRange(void)
 *
 * Include: Range.h
 *
 * Description: Configures the Range module
 *
 * Arguments: None
 *
 * Returns: None
 *************************************************************************/
void configureRange(void)
{
    unsigned char config;

    INIT_PIN = 0;

    //Enable global interrupts and interrupt priority
    INT_SETUP()

    readTemp();

    //Make sure the AD is configured
    configureAD();

    CCP1_INPT = 1;
    INIT_TRIS = 0;  //Make the INIT

    //Open  Timer
    config = T1_16BIT_RW & T1_SOURCE_INT & T1_OSC1EN_OFF & T1_PS_1_1 & T1_SYNC_EXT_OFF &TIMER_INT_ON;
    OpenTimer1(config);

    config = CAPTURE_INT_ON & CAP_EVERY_RISE_EDGE;

    //CloseCapture1, which will clear any interrupt flags etc
    CloseCapture1();

    //Open the input capture on compare1
    OpenCapture1(config);
}

void PmodHB5_INIT(UART_MODULE uartID)
{
	PORTSetPinsDigitalOut(IOPORT_D, BIT_7); //HB5 Direction
	PORTSetPinsDigitalOut(IOPORT_D, BIT_1); //HB5 Enable
	PORTSetPinsDigitalIn(IOPORT_D, BIT_9);  //HB5 Sensor A
	PORTSetPinsDigitalIn(IOPORT_C, BIT_1);  //HB5 Sensor B

	hBridge.sensorAport = IOPORT_D;
	hBridge.sensorAportBit = BIT_9;
	hBridge.sensorBport = IOPORT_C;
	hBridge.sensorBportBit = BIT_1;
	hBridge.directionPort = IOPORT_D;
	hBridge.directionPortBit = BIT_7;
	hBridge.currentDirection = PMOD_HB5_DIR_CW;
	hBridge.newDirection = PMOD_HB5_DIR_CW;
	hBridge.ocChannel = 2;
	

    OpenOC2(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
    OpenTimer2(T2_ON | T2_PS_1_256, SYSTEM_CLOCK/PB_DIV/PRESCALE/(TOGGLES_PER_SEC/2));
    
	OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_256, T1_TICK);
    ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_2);
	INTEnableSystemMultiVectoredInt();
	
	UARTPutS("\r\nPmodHB5 init complete\r\n",uartID);

}

/* App initialisation
 * init ADC
 * init timer 1
 * init interrupts
 */
void InitApp(void)
{
    cur_State = LIPO_ALGO_STARTED;
    strncpy(battery.battery_type,"LIPO\0",5);
    battery.charge.restore_Lowest_Voltage = 5;
    battery.number_of_cells = 1;
    battery.charge.restore_Charge_Current = 0.5;
    seconds = 0;

    OpenADC( ADC_FOSC_64 & ADC_RIGHT_JUST & ADC_6_TAD,
            ADC_CH0 & ADC_INT_OFF & ADC_REF_VDD_VSS,
            ADC_1ANA );
    /* open timers */
    OpenTimer1( TIMER_INT_ON &
                T1_8BIT_RW &
                T1_SOURCE_EXT &
                T1_PS_1_1 &
                T1_OSC1EN_ON &
                T1_SYNC_EXT_OFF );

    //ei();

    USBDeviceInit();            //usb_device.c
    #if defined(USB_INTERRUPT)
      USBDeviceAttach();        //usb_device.c
    #endif

    /* TODO init PI structure */
    PI.Ki = 2;
    PI.Kp = 35;
    /* TODO init analog port */
}

/**
* This function will initial board.
 */
void rt_hw_board_init()
{
    // Configure the device for maximum performance, but do not change the PBDIV clock divisor.
	// Given the options, this function will change the program Flash wait states,
	// RAM wait state and enable prefetch cache, but will not change the PBDIV.
    // The PBDIV value is already set via the pragma FPBDIV option above.
   	SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);

   	/* use DBPRINTF */
   	/* rt_hw_console_init(); */

	rt_hw_usart_init();
	rt_console_set_device("uart1");

   	rt_hw_show_info();

    // enable multi-vector interrupts
    INTEnableSystemMultiVectoredInt();
    rt_hw_interrupt_disable();

//    // STEP 2. configure the core timer
//    OpenCoreTimer(CORE_TICK_RATE);
//
//    // set up the core timer interrupt with a prioirty of 2 and zero sub-priority
//    mConfigIntCoreTimer((CT_INT_ON | CT_INT_PRIOR_2 | CT_INT_SUB_PRIOR_0));

    // STEP 2. configure Timer 1 using internal clock, 1:256 prescale
    OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_256, T1_TICK);
    // set up the timer interrupt with a priority of 2
    ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_2);

    /* Setup the software interrupt. */
	mConfigIntCoreSW0( CSW_INT_ON | CSW_INT_PRIOR_1 | CSW_INT_SUB_PRIOR_0 );
}

void init_core(void)
{
	// === Timer und Capture/Compare-Einheit ===

	// 10 MHz Oszillatortakt -> 40 MHz Systemtakt -> 10 MHz Instruktionstakt
	
	// 10 MHz Instruktionstakt / 16 Prescaler -> 1,6 µs Timertaktlänge, daher theoretisch alle 65536
	// Timertakte = 104,8ms ein Timerüberlauf
	OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_16);
	
	// 10 MHz Instruktionstakt / 8 Prescaler -> 0,8 µs Timertaktlänge
	OpenTimer1(T1_16BIT_RW & TIMER_INT_OFF & T1_PS_1_8 & T1_SYNC_EXT_ON & T1_SOURCE_CCP & T1_SOURCE_INT);
	OpenCapture1(CAPTURE_INT_ON & C1_EVERY_FALL_EDGE);
	
//	TxQ.Head = TxQ.Tail = RxCheckSum = 0;
	
	INTCONbits.PEIE = true;	// Wofür ist das?
	// INTCONbits.TMR0IE = true;

	// === Fernbedienungsdecoder ===

	raw_rc_data[0] = 0xDEADBEEF;
	current_rc_frame_ok = false;
	new_rc_values_available = false;

}

/* Specify Interrupt Priority Level = 2 */
void __ISR(_TIMER_1_VECTOR, IPL2) _Timer1Handler(void) {

    mT1ClearIntFlag();
    if(bitIndex == 0)
    {
        
        OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_1, (getEtu()/5));
        write(PIN_IO);
        bitIndex++;
    }
    else if(bitIndex == 39)
    {
        write(PIN_IO);
        bitIndex++;
        setFlag();
    }
    else if(bitIndex == 40)
    {
        bitIndex =0;
        ConfigINT2(EXT_INT_ENABLE | FALLING_EDGE_INT | EXT_INT_PRI_1);
        ConfigIntTimer1(T1_INT_OFF);
    }
    else
    {
        write(PIN_IO);
        bitIndex++;
    }
}

extern void timerStart(timer * pTimer, const timerCalc * pTimerCalc)
{
  pTimer->m_TimerCalc = *pTimerCalc;

  const uint16_t        PrescalerBits = pTimer->m_TimerCalc.PrescalerBits;
  const uint16_t        PriorityBits  = pTimer->m_TimerCalc.PriorityBits;
  const uint16_t        Ticks         = pTimer->m_TimerCalc.Ticks;
  const timer_tCallback pCallback     = pTimer->m_pCallback;

  pTimer->m_OverflowCount = pTimer->m_TimerCalc.OverflowCount;

  switch (pTimer->m_TimerNumber) {
    case 1: 
      OpenTimer1(T1_ON | T1_SOURCE_INT | PrescalerBits, Ticks);
      ConfigIntTimer1((pCallback == NULL ? T1_INT_OFF : T1_INT_ON) | PriorityBits);
      break;
    case 2: 
      OpenTimer2(T2_ON | T2_SOURCE_INT | PrescalerBits, Ticks);
      ConfigIntTimer2((pCallback == NULL ? T2_INT_OFF : T2_INT_ON) | PriorityBits);
      break;
    case 3: 
      OpenTimer3(T3_ON | T3_SOURCE_INT | PrescalerBits, Ticks);
      ConfigIntTimer3((pCallback == NULL ? T3_INT_OFF : T3_INT_ON) | PriorityBits);
      break;
    case 4: 
      OpenTimer4(T4_ON | T4_SOURCE_INT | PrescalerBits, Ticks);
      ConfigIntTimer4((pCallback == NULL ? T4_INT_OFF : T4_INT_ON) | PriorityBits);
      break;
    case 5: 
      OpenTimer5(T5_ON | T5_SOURCE_INT | PrescalerBits, Ticks);
      ConfigIntTimer5((pCallback == NULL ? T5_INT_OFF : T5_INT_ON) | PriorityBits);
      break;
  }
}

int main(void)
{
  uint16_t i;
  FRESULT rc;

  map_io();
  init_port();

  InitRTCC();

  uart2_init();
  xdev_out(uart2_put);
  xdev_in(uart2_get);

  dbg_printf("$" PROJECT_NAME "\n");
  dbg_printf("$" __DATE__ " " __TIME__ "\n");

  rc = f_mount(&fatfs, "", 1);
  dbg_printf("$FF,f_mount,%s\n", get_rc(rc));

  OpenTimer1(T1_PS_1_256 & T1_GATE_OFF & T1_SOURCE_INT & T1_IDLE_CON &
    T1_ON & T1_SYNC_EXT_OFF, 0xFFFF);
  ConfigIntTimer1(T1_INT_ON & T1_INT_PRIOR_1);
  OpenCapture1(IC_IDLE_STOP & IC_TIMER1_SRC & IC_INT_1CAPTURE & IC_EVERY_RISE_EDGE,
    IC_CASCADE_DISABLE & IC_TRIGGER_ENABLE & IC_UNTRIGGER_TIMER & IC_SYNC_TRIG_IN_DISABLE);
  ConfigIntCapture1(IC_INT_ON & IC_INT_PRIOR_5);
  _IC1IF = 0;
  

  while (1) {
    while (_RTCSYNC == 0);
    while (_RTCSYNC == 1);
    if (gps_pr > 0) {
      _T1IE = 0;
      float f = (float) TMR1 / gps_pr;
      _T1IE = 1;
      xprintf("%u\n", (uint16_t) (f * 1000));
    }
    if (ngpslines > 0) {
      ngpslines--;
      if (xgets(gps_line, 128)) {
        xprintf("$GPS%s\n", gps_line);
      }
    }
  }

  while (0) {
    while (_RTCSYNC == 0);
    while (_RTCSYNC == 1);
    if (gps_pr > 0) {
      _T1IE = 0;
      float f = (float) TMR1 / gps_pr;
      _T1IE = 1;
      xprintf("%u\n", (uint16_t) (f * 1000));
    }
  }

  return (EXIT_SUCCESS);
}

void delayus(unsigned t) {
    OpenTimer1(T1_ON | T1_PS_1_256, 0xFFFF);
    while (t--) { // t x 1ms loop
        WriteTimer1(0);
        while (ReadTimer1() < SYS_FREQ / 256 / 1000000);
    }
    CloseTimer1();
}// Delayus

void SetupTimer1(void)
{
    unsigned int T1CON1value, T1PERvalue;
    T1CON1value = T1_ON & T1_SOURCE_INT & T1_PS_1_8 & T1_GATE_OFF &
                  T1_SYNC_EXT_OFF;
    T1PERvalue = 0x01F4; //clock period = 0.0001s = ((T1PERvalue * prescaler)/FCY) (100Hz)
    OpenTimer1(T1CON1value, T1PERvalue);	
}

// 100 us minimum for acuracy
void inline __attribute__((always_inline)) timer1_delay_us(unsigned int us)
{
    counter = (int)(us * 0.022);
    OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_1, 50); // micro second interrupts
    while(counter < us);
    T1CONCLR = BIT_15; // stop the tomer
    TMR1 = 0x0;
}

void InitTMR1(void)
{
    /* config: T1_OFF & T1_IDLE_CON & T1_SOURCE_INT & T1_PS_1_64 */
    // Interrupt period = 0.005 sec with a 64 prescaler
    OpenTimer1(T1_OFF & T2_GATE_OFF & T1_IDLE_CON & T1_PS_1_64 & T1_SOURCE_INT,3124);
    ConfigIntTimer1(T1_INT_PRIOR_6 & T1_INT_ON);
    T1CONbits.TON = 1; // Turn on timer 1
    return;
}

// timer 1 is used for main pid motor control loop
void SetupTimer1(void)
{
    unsigned int T1CON1value, T1PERvalue;
    T1CON1value = T1_ON & T1_SOURCE_INT & T1_PS_1_8 & T1_GATE_OFF &
                  T1_SYNC_EXT_OFF & T1_INT_PRIOR_7;
    T1PERvalue = 0x03E8; //clock period = 0.0002s = ((T1PERvalue * prescaler)/FCY) (5000Hz)
  	t1_ticks = 0;
    OpenTimer1(T1CON1value, T1PERvalue);
}

/****************************************************************************
 Function
     TIMERS_Init

 Parameters
    none

 Returns
     None.

 Description
     Initializes the timer module
 Notes
     None.

 Author
     Max Dunne, 2011.11.15
 ****************************************************************************/
void TIMERS_Init(void) {
    TimerActiveFlags = 0;
    TimerEventFlags = 0;
    FreeRunningTimer = 0;
    OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_1, F_PB / TIMER_FREQUENCY);
    ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_3);

    mT1IntEnable(1);
}

/******************************************************************************
 * Function:        void initSpecialEventTrigger(void)
 *
 * PreCondition:    None
 * Input:           None
 * Output:          None
 * Side Effects:    None
 *
 * Overview:    The special event trigger is used for automatic ADC
 *
 * Note:        NYI ;-)
 *
 *****************************************************************************/
void initSpecialEventTrigger(void)
{
    OpenTimer1(TIMER_INT_OFF & T1_16BIT_RW & T1_SOURCE_INT &
            T1_PS_1_8 & T1_OSC1EN_OFF & T1_SYNC_EXT_OFF);
    T1CONbits.TMR1ON = 0;
//    mCCPTIMER_STOP()
    T3CONbits.T3CCP1 = 0;       // Timer1 is the clock source for compare ECCP1
    CCPR1 = 60000;              // 12M (clk) / 8 (TMR1 Prescaler)    -> 25Hz
    CCP1CONbits.CCP1M = 0b1011; // Compare Mode with Special Event Trigger
}

void delayms(unsigned t)
// This uses Timer 1, can be changed to another timer.
{
    OpenTimer1(T1_ON | T1_PS_1_256, 0xFFFF);
    while (t--) { // t x 1ms loop
        WriteTimer1(0);
        while (ReadTimer1() < SYS_FREQ / 256 / 1000);
    }
    CloseTimer1();
} // Delayms

void startTimer1()
{
	// Setup the timer with a 1:1 prescaler with 16 bits resolution
	// Therefore the timer0 freq is 500 kHz / 4 / 4 = 31.250 kHz
	OpenTimer1( TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_4 );
	// Should take a little over 2 seconds to overflow the counter from TMR0 = 0
	// If you write in a different starting value for TMR0 it'll overflow sooner

	WriteTimer1(0);
	PIR1bits.TMR1IF = 0;  
}