PWMPlatform::PWMPlatform() {
  pwmStart(&PWMD1, &MOTOR_PWM_CONFIG);
  palSetPadMode(GPIOC, 6, PAL_MODE_ALTERNATE(4));
  palSetPadMode(GPIOC, 7, PAL_MODE_ALTERNATE(4));
  palSetPadMode(GPIOC, 8, PAL_MODE_ALTERNATE(4));
  palSetPadMode(GPIOC, 9, PAL_MODE_ALTERNATE(4));
}

void initializeRCReceiver(void) {
    // Initializes the ICU drivers in order to access the PWM values from the RC-receiver ICU sensor.
    icuStart(&ICUD1, &icuRCReceiverCH0);
    icuStart(&ICUD8, &icuRCReceiverCH1);
#if !USE_ONBOARD_ACCELEROMETER
    icuStart(&ICUD14, &icuRCReceiverCH2);
#endif

    // RC-receiver ICU sensor channel 0 is connected to PA8 and is monitored by timer 1/channel 1.
    palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM1));

    // RC-receiver ICU sensor channel 1 is connected to PC6 and is monitored by timer 8/channel 1.
    palSetPadMode(GPIOC, 6, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM8));

#if !USE_ONBOARD_ACCELEROMETER
    // RC-receiver ICU sensor channel 2 is connected to PA7 and is monitored by timer 14/channel 1.
    palSetPadMode(GPIOA, 7, PAL_MODE_ALTERNATE(STM32F4GPIO_AF_TIM14));
#endif

    // Enable ICU reading in background.
    icuEnable(&ICUD1);
    icuEnable(&ICUD8);
#if !USE_ONBOARD_ACCELEROMETER
    icuEnable(&ICUD14);
#endif
}

/*
 * Application entry point.
 */
int main(void) {

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /* Prepares the Serial driver 1.*/
  sdStart(&SD1, NULL);
  palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(1));       /* USART1 TX.       */
  palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(1));      /* USART1 RX.       */

  /* Running the test.*/
  irq_storm_execute(&irq_storm_config);

  /* Normal main() thread activity, nothing in this test.*/
  while (true) {
    chThdSleepMilliseconds(5000);
  }
}

void mpu9150_init(void) {
	failed_reads = 0;
	failed_mag_reads = 0;
	read_callback = 0;
	last_update_time = 0;
	update_time_diff = 0;
	mag_updated = 0;
	mpu_addr = MPU_ADDR1;
	is_mpu9250 = 0;

	memset((void*)mpu9150_gyro_offsets, 0, sizeof(mpu9150_gyro_offsets));

	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

	// I2C configuration
	palSetPadMode(SCL_GPIO, SCL_PAD,
			PAL_MODE_ALTERNATE(GPIO_AF_I2C2) |
			PAL_STM32_OTYPE_OPENDRAIN |
			PAL_STM32_OSPEED_MID1);
	palSetPadMode(SDA_GPIO, SDA_PAD,
			PAL_MODE_ALTERNATE(GPIO_AF_I2C2) |
			PAL_STM32_OTYPE_OPENDRAIN |
			PAL_STM32_OSPEED_MID1);
	chThdSleepMilliseconds(10);

	I2C_DEV.state = I2C_STOP;
	i2cStart(&I2C_DEV, &i2cfg);

	reset_init_mpu();

	chThdCreateStatic(mpu_thread_wa, sizeof(mpu_thread_wa), NORMALPRIO + 1,
			mpu_thread, NULL );
}

void initCan(void) {
#if EFI_PROD_CODE || defined(__DOXYGEN__)
	addConsoleAction("caninfo", canInfo);
	if (!engineConfiguration->isCanEnabled)
		return;
#endif /* EFI_PROD_CODE */

#if STM32_CAN_USE_CAN2 || defined(__DOXYGEN__)
	// CAN1 is required for CAN2
	canStart(&CAND1, &canConfig);
	canStart(&CAND2, &canConfig);
#else
	canStart(&CAND1, &canConfig);
#endif

	canStart(&EFI_CAN_DEVICE, &canConfig);
#if EFI_PROD_CODE || defined(__DOXYGEN__)

	chThdCreateStatic(canTreadStack, sizeof(canTreadStack), NORMALPRIO, (tfunc_t) canThread, NULL);

	mySetPadMode2("CAN TX", boardConfiguration->canTxPin, PAL_MODE_ALTERNATE(EFI_CAN_TX_AF));
	mySetPadMode2("CAN RX", boardConfiguration->canRxPin, PAL_MODE_ALTERNATE(EFI_CAN_RX_AF));

#endif /* EFI_PROD_CODE */
}

static msg_t uart_thread(void *arg) {
	(void)arg;

	chRegSetThreadName("UART");

	uartStart(&HW_UART_DEV, &uart_cfg);
	palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_PULLUP);
	palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_PULLUP);

	systime_t time = chTimeNow();

	for(;;) {
		time += MS2ST(1);

		if ((systime_t) ((float) chTimeElapsedSince(last_uart_update_time)
				/ ((float) CH_FREQUENCY / 1000.0)) > (float)TIMEOUT) {
			mcpwm_set_brake_current(-10.0);
		} else {
			set_output(out_received);
		}

		chThdSleepUntil(time);
	}

	return 0;
}

void demo_led_init(void)
{
    /*
     * PWM configuration structure.
     * Cyclic callback enabled, channels 1 and 4 enabled without callbacks,
     * the active state is a logic one.
     */
    static const PWMConfig pwmcfg = {
        100000,                                   /* 100kHz PWM clock frequency.  */
        128,                                      /* PWM period is 128 cycles.    */
        NULL,
        {
         {PWM_OUTPUT_ACTIVE_HIGH, NULL},
         {PWM_OUTPUT_ACTIVE_HIGH, NULL},
         {PWM_OUTPUT_ACTIVE_HIGH, NULL},
         {PWM_OUTPUT_ACTIVE_HIGH, NULL}
},
        /* HW dependent part.*/
        0,
        0
    };

    /*
     * Initializes the PWM driver 4, routes the TIM4 outputs to the board LEDs.
     */
    pwmStart(&PWMD4, &pwmcfg);
    palSetPadMode(GPIOD, GPIOD_LED4, PAL_MODE_ALTERNATE(2));      /* Green.   */
    palSetPadMode(GPIOD, GPIOD_LED3, PAL_MODE_ALTERNATE(2));      /* Orange.  */
    palSetPadMode(GPIOD, GPIOD_LED5, PAL_MODE_ALTERNATE(2));      /* Red.     */
    palSetPadMode(GPIOD, GPIOD_LED6, PAL_MODE_ALTERNATE(2));      /* Blue.    */
}

/*
 * Application entry point.
 */
int main(void) {

    /*
     * System initializations.
     * - HAL initialization, this also initializes the configured device drivers
     *   and performs the board-specific initializations.
     */
    halInit();

    /*
     * Enabling interrupts, initialization done.
     */
    osalSysEnable();

    /*
     * Activates the serial driver 2 using the driver default configuration.
     * PA2(TX) and PA3(RX) are routed to USART2.
     */
    sdStart(&SD2, NULL);
    palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
    palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));

    /*
     * Normal main() thread activity, in this demo it just performs
     * a shell respawn upon its termination.
     */
    while (true) {
        chnWriteTimeout(&SD2, (uint8_t *)"Hello World!\r\n", 14, TIME_INFINITE);

        palSetPad(GPIOD, GPIOD_LED3);       /* Orange.  */
        osalThreadSleepMilliseconds(500);
        palClearPad(GPIOD, GPIOD_LED3);     /* Orange.  */
        osalThreadSleepMilliseconds(500);
    }
}

void startConsole(Logging *sharedLogger, CommandHandler console_line_callback_p) {
	logger = sharedLogger;
	console_line_callback = console_line_callback_p;

#if (defined(EFI_CONSOLE_UART_DEVICE) && ! EFI_SIMULATOR) || defined(__DOXYGEN__)

	palSetPadMode(CONSOLE_MODE_SWITCH_PORT, CONSOLE_MODE_SWITCH_PIN, PAL_MODE_INPUT_PULLUP);

	b_isCommandLineConsoleOverTTL = GET_CONSOLE_MODE_VALUE() == EFI_USE_UART_FOR_CONSOLE;

	if (isCommandLineConsoleOverTTL()) {
		/*
		 * Activates the serial
		 * it is important to set 'NONE' as flow control! in terminal application on the PC
		 */
		serialConfig.speed = engineConfiguration->uartConsoleSerialSpeed;
		sdStart(EFI_CONSOLE_UART_DEVICE, &serialConfig);

// cannot use pin repository here because pin repository prints to console
		palSetPadMode(EFI_CONSOLE_RX_PORT, EFI_CONSOLE_RX_PIN, PAL_MODE_ALTERNATE(EFI_CONSOLE_AF));
		palSetPadMode(EFI_CONSOLE_TX_PORT, EFI_CONSOLE_TX_PIN, PAL_MODE_ALTERNATE(EFI_CONSOLE_AF));

		isSerialConsoleStarted = true;

		chEvtRegisterMask((event_source_t *) chnGetEventSource(EFI_CONSOLE_UART_DEVICE), &consoleEventListener, 1);
	}
#else
	b_isCommandLineConsoleOverTTL = false;
#endif /* EFI_PROD_CODE */

	chThdCreateStatic(consoleThreadStack, sizeof(consoleThreadStack), NORMALPRIO, (tfunc_t)consoleThreadThreadEntryPoint, NULL);
	addConsoleAction(SWITCH_TO_BINARY_COMMAND, switchToBinaryProtocol);
}

/*
 * Application entry point.
 */
int main(void) {

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /*
   * Activates the serial driver 1, PA9 and PA10 are routed to USART1.
   */
  uartStart(&UARTD1, &uart_cfg_1);
  palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(1));       /* USART1 TX.       */
  palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(1));      /* USART1 RX.       */

  /*
   * Starts the transmission, it will be handled entirely in background.
   */
  uartStartSend(&UARTD1, 13, "Starting...\r\n");

  /*
   * Normal main() thread activity, in this demo it does nothing.
   */
  while (TRUE) {
    chThdSleepMilliseconds(500);
  }
}

void startTunerStudioConnectivity(void) {
	initLogging(&logger, "tuner studio");

	if (sizeof(engine_configuration_s) != getTunerStudioPageSize(0))
		firmwareError("TS page size mismatch: %d/%d", sizeof(engine_configuration_s), getTunerStudioPageSize(0));

	if (sizeof(TunerStudioOutputChannels) != TS_OUTPUT_SIZE)
		firmwareError("TS outputs size mismatch: %d/%d", sizeof(TunerStudioOutputChannels), TS_OUTPUT_SIZE);

	memset(&tsState, 0, sizeof(tsState));
#if EFI_PROD_CODE
	if (isSerialOverUart()) {
		print("TunerStudio over USB serial");
		usb_serial_start();
	} else {

		print("TunerStudio over USART");
		mySetPadMode("tunerstudio rx", TS_SERIAL_RX_PORT, TS_SERIAL_RX_PIN, PAL_MODE_ALTERNATE(TS_SERIAL_AF));
		mySetPadMode("tunerstudio tx", TS_SERIAL_TX_PORT, TS_SERIAL_TX_PIN, PAL_MODE_ALTERNATE(TS_SERIAL_AF));

		sdStart(TS_SERIAL_UART_DEVICE, &tsSerialConfig);
	}
#endif /* EFI_PROD_CODE */
	syncTunerStudioCopy();

	addConsoleAction("tsinfo", printStats);

	chThdCreateStatic(TS_WORKING_AREA, sizeof(TS_WORKING_AREA), NORMALPRIO, tsThreadEntryPoint, NULL);
}

static void initSpiModule(SPIDriver *driver, ioportid_t sckPort, ioportmask_t sckPin, ioportid_t misoPort,
		ioportmask_t misoPin, ioportid_t mosiPort, ioportmask_t mosiPin, int af) {
	mySetPadMode("SPI clock", sckPort, sckPin, PAL_MODE_ALTERNATE(af));

	mySetPadMode("SPI master out", mosiPort, mosiPin, PAL_MODE_ALTERNATE(af));
	mySetPadMode("SPI master in ", misoPort, misoPin, PAL_MODE_ALTERNATE(af));
}

static void initI2CHw(void)
{
    i2cStart(&I2C_DRIVER, &i2cfg1);
    palSetPadMode(GPIOB, 8, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_PUDR_PULLUP);
    palSetPadMode(GPIOB, 9, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_PUDR_PULLUP);

}

/*
 * Application entry point.
 */
int main(void) {
  uint32 blinker_id;

  /* HAL initialization, this also initializes the configured device drivers
     and performs the board-specific initializations.*/
  halInit();

  /* OS initialization.*/
  (void) OS_API_Init();

  /* Activates the serial driver 2 using the driver default configuration.
    PA2(TX) and PA3(RX) are routed to USART2.*/
  sdStart(&SD2, NULL);
  palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
  palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));

  /* Starting the blinker thread.*/
  (void) OS_TaskCreate(&blinker_id, "blinker", blinker,
                       (uint32 *)wa_blinker, sizeof wa_blinker,
                       128, 0);

  /* In the ChibiOS/RT OSAL implementation the main() function is an
     usable thread with priority 128 (NORMALPRIO), here we just sleep
     in a loop printing a message on the serial port.*/
  while (true) {
    sdWrite(&SD2, (uint8_t *)"Hello World!\r\n", 14);
    OS_TaskDelay(500);
  }
}

/*
 * Application entry point.
 */
int main(void) {

  /* HAL initialization, this also initializes the configured device drivers
     and performs the board-specific initializations.*/
  halInit();

  /* The kernel is initialized but not started yet, this means that
     main() is executing with absolute priority but interrupts are
     already enabled.*/
  osKernelInitialize();

  /* Activates the serial driver 2 using the driver default configuration.
    PA2(TX) and PA3(RX) are routed to USART2.*/
  sdStart(&SD2, NULL);
  palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
  palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));

  /* Creates the example thread, it does not start immediately.*/
  osThreadCreate(osThread(Thread1), NULL);

  /* Kernel started, the main() thread has priority osPriorityNormal
     by default.*/
  osKernelStart();

  /* In the ChibiOS/RT CMSIS RTOS implementation the main() is an
     usable thread, here we just sleep in a loop printing a message.*/
  while (true) {
    sdWrite(&SD2, (uint8_t *)"Hello World!\r\n", 14);
    osDelay(500);
  }
}

int wconfInit(void)
{
	thread_t *listener;

	palSetPadMode(GPIOB, 10, PAL_MODE_ALTERNATE(7)); // used function : USART3_TX
	palSetPadMode(GPIOB, 11, PAL_MODE_ALTERNATE(7)); // used function : USART3_RX
	/* Start the serial driver(change it's state to ready) pointed by arg1 with configurations in arg2
     * if arg2 is NULL then use default configuration in halconf.h*/
	 sdStart(&SD3, &uartCfg);

	/*Initialize mailbox object pointed by arg1, which uses buffer pointed by arg2 and have
	 * arg3 objects*/
	chMBObjectInit(&serialMbox, txMailboxArea, MAILBOXSIZE);

	/* Create a thread uses arg2 of area arg1 with priority of
	 * arg3(LOWPRIO,NORMALPRIO,HIGHPRIO or any value between 0-255), thread function is arg4
	 *
	 * Returns pointer to thread object*/
    listener = chThdCreateStatic(serialListen, sizeof(serialListen), LOWPRIO, rxListen, NULL);

    chThdCreateStatic(serialTalk, sizeof(serialTalk), LOWPRIO, txWrite, NULL);

    /*Wait until thread pointed in arg1 finish*/
	//chThdWait(listener);
}

/*
 * Application entry point.
 */
int main(void) {

  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /* Configuring I2C SCK and I2C SDA related GPIOs .*/
  palSetLineMode(LINE_ARD_D15, PAL_MODE_ALTERNATE(4) |
                 PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN);
  palSetLineMode(LINE_ARD_D14, PAL_MODE_ALTERNATE(4) |
                 PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN);

  /* Activates the serial driver 2 using the driver default configuration.*/
  sdStart(&SD2, NULL);

  /* Creates the blinker thread.*/
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  /* LSM303AGR Object Initialization.*/
  lsm303agrObjectInit(&LSM303AGRD1);

  /* Activates the LSM303AGR driver.*/
  lsm303agrStart(&LSM303AGRD1, &lsm303agrcfg);

  /* Normal main() thread activity, printing MEMS data on the SD2. */
  while (true) {
    lsm303agrAccelerometerReadRaw(&LSM303AGRD1, accraw);
    chprintf(chp, "LSM303AGR Accelerometer raw data...\r\n");
    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
      chprintf(chp, "%c-axis: %d\r\n", axisID[i], accraw[i]);
    }

    lsm303agrCompassReadRaw(&LSM303AGRD1, compraw);
    chprintf(chp, "LSM303AGR Compass raw data...\r\n");
    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {
      chprintf(chp, "%c-axis: %d\r\n", axisID[i], compraw[i]);
    }

    lsm303agrAccelerometerReadCooked(&LSM303AGRD1, acccooked);
    chprintf(chp, "LSM303AGR Accelerometer cooked data...\r\n");
    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
      chprintf(chp, "%c-axis: %.3f\r\n", axisID[i], acccooked[i]);
    }

    lsm303agrCompassReadCooked(&LSM303AGRD1, compcooked);
    chprintf(chp, "LSM303AGR Compass cooked data...\r\n");
    for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {
      chprintf(chp, "%c-axis: %.3f\r\n", axisID[i], compcooked[i]);
    }
    chThdSleepMilliseconds(100);
    cls(chp);
  }
  lsm303agrStop(&LSM303AGRD1);
}

void gpdrive_deinit(void) {
	if (!m_init_done) {
		return;
	}

	m_init_done = false;

	timer_thd_stop = true;

	while (timer_thd_stop) {
		chThdSleepMilliseconds(1);
	}

	TIM_DeInit(TIM1);
	TIM_DeInit(TIM12);
	ADC_DeInit();
	DMA_DeInit(DMA2_Stream4);
	nvicDisableVector(ADC_IRQn);
	dmaStreamRelease(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)));

	// Restore pins
	palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_FLOATING);
	palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_FLOATING);
}

void klineInit(bool honda)
{
  // Set pin mode to GPIO
  palSetPadMode(PORT_KLINE_TX, PAD_KLINE_RX, PAL_MODE_INPUT_ANALOG);
  palSetPadMode(PORT_KLINE_TX, PAD_KLINE_TX, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);

  // Toggle K-line bus
  if (honda) {
    // Honda HDS specific init
    K_LOW(70); // Low for 70ms
    K_HIGH(130); // High for 130ms
  }
  else {
    // KWP2000 Fast init (Kawasaki KDS)
    K_LOW(25); // Low for 25ms
    K_HIGH(25); // High for 25ms
  }

  // Set pin mode back to UART
  palSetPadMode(PORT_KLINE_TX, PAD_KLINE_TX, PAL_MODE_ALTERNATE(7) | \
          PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_PUPDR_PULLUP);
  palSetPadMode(PORT_KLINE_TX, PAD_KLINE_RX, PAL_MODE_ALTERNATE(7) | \
                  PAL_STM32_OTYPE_OPENDRAIN);

}

void runRusEfi(void) {

#if defined(EFI_CONSOLE_UART_DEVICE)
  /*
   * Activates the serial driver 1 using the driver default configuration.
   * PA9 and PA10 are routed to USART1.
   */
  sdStart(&SD1, NULL);
  palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(1));       /* USART1 TX.       */
  palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(1));      /* USART1 RX.       */
#endif

  /*
   * Creates the blinker threads.
   */
  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

  initMax31855(NULL, max31855_cs);

  /*
   * Normal main() thread activity, in this demo it does nothing except
   * sleeping in a loop and check the button state, when the button is
   * pressed the test procedure is launched with output on the serial
   * driver 1.
   */
  while (true) {
//    if (palReadPad(GPIOA, GPIOA_BUTTON))
//      TestThread(&SD1);
    chThdSleepMilliseconds(50);

    printPending();
  }


}