/**
 * @brief  Starts the PWM output.
 * @note   The pwmStart() function used in this code is not
 *         the original ChibiOS HAL function, but modified
 *         one with STM32_TIM_CR1_CEN flag removed.
 * @return none.
 */
void pwmOutputStart(void) {
#if STM32_PWM_USE_ADVANCED
  /* Get dead-time generator value for TIM1. */
  uint32_t bdtr_dt = pwmOutputGetBDTRDeadTime(g_pwmOutput[PWM_OUT_ROLL].dt_cmd_id & PWM_OUT_DT_ID_MASK);
  pwmcfg_d1_d8.bdtr |= bdtr_dt;
#endif
  pwmStart(&PWMD1, &pwmcfg_d1_d8);

#if STM32_PWM_USE_ADVANCED
  /* Clear bdtr_dt value from previous calculation. */
  pwmcfg_d1_d8.bdtr &= ~bdtr_dt;
  /* Get dead-time generator value for TIM8. */
  bdtr_dt = pwmOutputGetBDTRDeadTime(g_pwmOutput[PWM_OUT_PITCH].dt_cmd_id & PWM_OUT_DT_ID_MASK);
  pwmcfg_d1_d8.bdtr |= bdtr_dt;
#endif
  /* Configure TIM8 as master timer: */
  pwmcfg_d1_d8.cr2 = STM32_TIM_CR2_MMS(1); // Master mode set to Enable;
  pwmStart(&PWMD8, &pwmcfg_d1_d8);

  switch (g_pwmOutput[PWM_OUT_YAW].dt_cmd_id & PWM_OUT_DT_ID_MASK) {
  case PWM_OUT_DT750NS:
    pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_US7;
    break;
  case PWM_OUT_DT1000NS:
    pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_1US;
    break;
  case PWM_OUT_DT2000NS:
    pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_2US;
    break;
  case PWM_OUT_DT3000NS:
    pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_3US;
    break;
  case PWM_OUT_DT4000NS:
    pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_4US;
    break;
  case PWM_OUT_DT5000NS:
    pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_5US;
    break;
  default:
    pwmOutTIM4_5_DT = PWM_OUT_TIM4_5_DT_5US;
  }
  pwmStart(&PWMD4, &pwmcfg_d4);
  pwmStart(&PWMD5, &pwmcfg_d5);

  /* Configure TIM5 as slave timer: */
  PWMD5.tim->SMCR =
    STM32_TIM_SMCR_SMS(6) | // Trigger Mode;
    STM32_TIM_SMCR_TS(3);   // Trigger event comes from TIM8;

  /* Configure TIM4 as slave timer: */
  PWMD4.tim->SMCR =
    STM32_TIM_SMCR_SMS(6) | // Trigger Mode;
    STM32_TIM_SMCR_TS(3);   // Trigger event comes from TIM8;

  /* Switch to center-aligned mode 1 and start timers. */
  PWMD5.tim->CR1 |= (STM32_TIM_CR1_CMS(1)); // This is a slave timer - do not start;
  PWMD4.tim->CR1 |= (STM32_TIM_CR1_CMS(1)); // This is a slave timer - do not start;
  PWMD1.tim->CR1 |= (STM32_TIM_CR1_CMS(1) | STM32_TIM_CR1_CEN);
  PWMD8.tim->CR1 |= (STM32_TIM_CR1_CMS(1) | STM32_TIM_CR1_CEN); // Start TIM8, TIM4 and TIM5 simultaneously;
}

void InitPWM(void) {
  /*
   * Initializes the PWM drivers
   */
  pwmStart(&PWMD3, &pwmcfg);
  pwmStart(&PWMD4, &pwmcfg);
  pwmStart(&PWMD5, &pwmcfg);
//  pwmStart(&PWMD8, &pwmcfg);
}

/* This function will start the PWM generator.
 */
extern void bldcInit(void) {
  bldc.scheme = &pwmScheme;
  bldc.state = 0;          //Default to first state
  bldc.nextState = 0;
  bldc.directionFwd = TRUE;
  bldc.stateChangeInterval = MS2RTT(20);
  bldc.prevStateChange = halGetCounterValue();
  bldc.nextStateChange = bldc.prevStateChange + bldc.stateChangeInterval;
  bldc.pwmOutT0 = 0;
  bldc.pwmOutT1 = 0;
  bldc.stateCount = sizeof(pwmScheme)/3;
  bldc.dutyCycle = 1800;

  palWriteGroup (PWM_OUT_PORT, PWM_OUT_PORT_MASK, PWM_OUT_OFFSET,  PWM_OFF);
  palSetGroupMode (PWM_OUT_PORT, PWM_OUT_PORT_MASK, PWM_OUT_OFFSET, PAL_MODE_OUTPUT_PUSHPULL);
  palSetPadMode (GPIOA, GPIOA_PIN1, PAL_MODE_INPUT_ANALOG);

  pwmStart(&PWMD1, &pwmcfg);

  // ADC trigger channel. This will trigger the ADC read at 95% of the cycle,
  // when all the PWM outputs are set to 0
  pwmEnableChannel (&PWMD1, PWM_ADCTRIG_CH, PWM_PERCENTAGE_TO_WIDTH(&PWMD1, PWM_MAX_DUTY_CYCLE));

  // Start the ADC
  adcStart(&ADCD1, NULL);

}

	void goToPos(int32_t new_pos){
		if(new_pos == position)
			return;
		config.frequency = clk_freq;
		config.period = clk_freq / initial_freq;
		config.callback = cb;
		current_freq = initial_freq;
		config.channels[0] = {PWM_OUTPUT_DISABLED, NULL};
		config.channels[1] = {PWM_OUTPUT_DISABLED, NULL};
		config.channels[2] = {PWM_OUTPUT_DISABLED, NULL};
		config.channels[3] = {PWM_OUTPUT_DISABLED, NULL};
		config.channels[channel] = {PWM_OUTPUT_ACTIVE_HIGH, NULL};
		config.cr2 = 0;
		config.dier = 0;

		dir_positive = new_pos > position;
		target_position = new_pos;

		dir_pin.assign(dir_positive);

		ramp_length = (new_pos > position) ?
			((max_pos_freq - initial_freq) / ramp) :
			((max_neg_freq - initial_freq) / ramp);

		pwmStart(driver, &config);
		pwmEnableChannel(driver, channel, 10);
		pwmEnablePeriodicNotificationI(driver);
	}

int main(void)
{
  halInit();
  chSysInit();
  
  palSetPadMode(GPIOB, 0, PAL_MODE_ALTERNATE(2)); // alt mode is PWM-enabled TIM3
  palSetPadMode(GPIOB, INAPIN, PAL_MODE_OUTPUT_PUSHPULL); // set INA to be an output
  palSetPadMode(GPIOB, INBPIN, PAL_MODE_OUTPUT_PUSHPULL); // set INB to be an output
   
  pwmStart(&PWMD3, &pwmcfg); // TIM3 for motors
 
  while (TRUE) {
    // one direction (possibly forward)
    pwmEnableChannel(&PWMD3, 3, 500);
    palSetPad(GPIOB, INAPIN);
    palClearPad(GPIOB, INBPIN);
    chThdSleepMilliseconds(3000);

    // the other direction (possibly backward)
    pwmEnableChannel(&PWMD3, 3, 500);
    palSetPad(GPIOB, INBPIN);
    palClearPad(GPIOB, INAPIN);
    chThdSleepMilliseconds(3000);
  }
}

void PwmBacklight::Init()
{
	palSetPadMode(BACKLIGHT_PORT, BACKLIGHT_PIN, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	pwmStart(&PWMD4, &pwmconfig);

	FadeIn();
}

void convStart( void )
{
    /*
    pwmStart( &PWMD2, &pwmCfg );
    palSetPadMode( GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
    palSetPadMode( GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
    //palSetPadMode( GPIOA, 2, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
    pwmEnableChannel(&PWMD2, 0, PWM_PERCENTAGE_TO_WIDTH( &PWMD2, 9500 ) );
    pwmEnableChannel(&PWMD2, 1, PWM_PERCENTAGE_TO_WIDTH( &PWMD2, 3030 ) );
    //pwmEnableChannel(&PWMD2, 2, PWM_PERCENTAGE_TO_WIDTH( &PWMD2, 3030 ) );
    */

	// Start PWM peripherial.
    pwmStart( &CONV_PWM, &pwmCfg );
	// Init PWM pins.
    palSetPadMode( CONV_PORT, CONV_BUCK_PIN,  PAL_MODE_STM32_ALTERNATE_PUSHPULL );
    palSetPadMode( CONV_PORT, CONV_BOOST_PIN, PAL_MODE_STM32_ALTERNATE_PUSHPULL );
    // Set zero active period.
    pwmEnableChannel(&CONV_PWM, PWM_BOOST_CHAN, PWM_PERCENTAGE_TO_WIDTH( &CONV_PWM, 0000 ) );
    pwmEnableChannel(&CONV_PWM, PWM_BUCK_CHAN,  PWM_PERCENTAGE_TO_WIDTH( &CONV_PWM, 0000 ) );

    // Init ADC.
    palSetGroupMode(CONV_ADC_PORT, PAL_PORT_BIT( CONV_BUCK_FB_PIN ) |
    	                           PAL_PORT_BIT( CONV_BOOST_FB_PIN ) |
    	                           PAL_PORT_BIT( CONV_INPUT_FB_PIN ),
                                   0, PAL_MODE_INPUT_ANALOG);
    adcStart( &ADCD1, NULL );

    adcStartConversion( &ADCD1, &adcGroup, adcSamples, ADC_BUF_DEPTH );
}

/**
 * @brief     Put bus in active mode.
 */
static void ow_bus_active(onewireDriver *owp) {
  pwmStart(owp->config->pwmd, owp->config->pwmcfg);
#if defined(STM32F1XX)
  palSetPadMode(owp->config->port, owp->config->pad,
      owp->config->pad_mode_active);
#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();
  pwmStart(&PWMD1, &pwmcfg);

  /*
   * Activates the serial driver 2 using the driver default configuration.
   */

  palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(2));

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

  /*
   * Normal main() thread activity, in this demo it does nothing except
   * sleeping in a loop and check the button state.
   */
  while (true) {
    if (TRUE)  //(!palReadPad(GPIOC, GPIOC_BUTTON))
      //TestThread(&SD1);
    chThdSleepMilliseconds(5000);
  }
}

void PwmBacklight::Init()
{
	palSetPadMode(GPIOB, 6, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
	pwmStart(&PWMD4, &pwmconfig);

	FadeIn();
}

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));
}

msg_t pwm2sub_node(void * arg) {
	Node node("pwm2sub");
	Subscriber<PWM2Msg, 5> sub(callback);

	(void) arg;

	chRegSetThreadName("pwm2sub");

	palSetPad(DRIVER_GPIO, DRIVER_RESET);
	chThdSleepMilliseconds(500);
	pwmStart(&PWM_DRIVER, &pwmcfg);

	node.subscribe(sub, "pwm2");

	for (;;) {
		if (!node.spin(Time::ms(100))) {
			// Stop motor if no messages for 100 ms
			chSysLock()
			;
			pwm_lld_enable_channel(&PWM_DRIVER, 0, 0);
			pwm_lld_enable_channel(&PWM_DRIVER, 0, 0);
			chSysUnlock();
		}
	}
	return CH_SUCCESS;
}

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.    */
}

void motor_init(void) {
	pwmStart(&PWMD2, &pwmcfg);
	
	pwmEnableChannel(&PWMD2, 0, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 500)); // 1mS Pulse
	pwmEnableChannel(&PWMD2, 1, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 500)); // 1mS Pulse
	pwmEnableChannel(&PWMD2, 2, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 500)); // 1mS Pulse
	pwmEnableChannel(&PWMD2, 3, PWM_PERCENTAGE_TO_WIDTH(&PWMD2, 500)); // 1mS Pulse
}

bool Pwm_chibios::init(void)
{
    pwmStop(driver_);
    pwmStart(driver_, &config_);
    pwmEnablePeriodicNotification(driver_);
    pwmEnableChannelNotification(driver_, channel_);
    return true;
}

/*
 * Application entry point.
 */
int main(void) {
  Thread *shelltp = NULL;

  /*
   * 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 using the driver default configuration.
   */
  sdStart(&SERIAL_DRIVER, NULL);

  /*
   * Initializes the PWM driver.
   */
  pwmStart(&PWM_DRIVER, &pwmcfg);

  /*
   * Initializes the GPT driver.
   */
  gptStart(&GPT_DRIVER, &gptcfg);

  /*
   * Initializes the EXT driver.
   */
  extStart(&EXT_DRIVER, &extcfg);

  /*
   * Shell manager initialization.
   */
  shellInit();

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

  /*
   * Normal main() thread activity, in this demo it does nothing except
   * sleeping in a loop and check the button state.
   */
  while (TRUE) {
    if (!shelltp)
      shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
    else if (chThdTerminated(shelltp)) {
      chThdRelease(shelltp);
      shelltp = NULL;
    }
    chprintf(&SERIAL_DRIVER, "M: %6umm T1: %6u T2: %6u\r\n", measure / 2, tmp1, tmp2);
    chThdSleepMilliseconds(200);
  }
}

void MotorsInit(void){

	pwmStart(&PWMD8, &pwmcfg);
	palSetPadMode(GPIOC, 6, PAL_MODE_ALTERNATE(3));
	palSetPadMode(GPIOC, 7, PAL_MODE_ALTERNATE(3));
	palSetPadMode(GPIOC, 8, PAL_MODE_ALTERNATE(3));
	palSetPadMode(GPIOC, 9, PAL_MODE_ALTERNATE(3));

	unsigned i;
	for (i=0; i<NUM_MOTORS; i++){
		MotorsSetSpeed(i, 0);
	}
}

static __attribute__((noreturn)) msg_t PWMThread2(void *arg){
    (void)arg;
    chRegSetThreadName("PWM Thread 2");
    uint16_t pwmVal2 = 1;
    palSetPadMode(GPIOA, GPIOA_PA1, PAL_MODE_OUTPUT_PUSHPULL);
    pwmStart(&PWMD2,&pwmcfg2);
    while(TRUE){
        if(pwmVal2 >= 1024){ pwmVal2 = 1; }
        else { pwmVal2 <<= 1; }
        pwmEnableChannel(&PWMD2,1,pwmVal2);
        chThdSleepMilliseconds(100);
    }
}

int main(void) {
    halInit();
    chSysInit();

    chThdSleepMilliseconds(200);

    // input capture & high-res timer
    const ICUConfig icuConfig = { ICU_INPUT_ACTIVE_HIGH, 1000000, nullptr, nullptr, nullptr, nullptr, nullptr };
    icuStart(&TIMING_ICU, &icuConfig);
    icuEnable(&TIMING_ICU);

    // serial setup
    const SerialConfig btSerialConfig = { 921600, 0, USART_CR2_STOP1_BITS, USART_CR3_CTSE | USART_CR3_RTSE };
    sdStart(&BT_SERIAL, &btSerialConfig);

    // PWM setup
    const PWMConfig mPWMConfig = { STM32_TIMCLK1, PWM_PERIOD, nullptr, {
            { PWM_OUTPUT_DISABLED, nullptr },
            { PWM_OUTPUT_DISABLED, nullptr },
            { PWM_OUTPUT_ACTIVE_HIGH, nullptr },
            { PWM_OUTPUT_ACTIVE_HIGH, nullptr } }, 0, };
    pwmStart(&M1_PWM, &mPWMConfig);

    // SPI setup
    // speed = pclk/8 = 5.25MHz
    const SPIConfig m1SPIConfig = { NULL, GPIOC, GPIOC_M1_NSS, SPI_CR1_DFF | SPI_CR1_BR_1 };
    const SPIConfig m2SPIConfig = { NULL, GPIOD, GPIOD_M2_NSS, SPI_CR1_DFF | SPI_CR1_BR_1 };
    const SPIConfig adcSPIConfig = { NULL, GPIOA, GPIOA_ADC_NSS, SPI_CR1_BR_2 | SPI_CR1_CPHA };
    spiStart(&M1_SPI, &m1SPIConfig);
    spiStart(&M2_SPI, &m2SPIConfig);
    spiStart(&ADC_SPI, &adcSPIConfig);

    // motor setup
    A4960 m1(&M1_SPI, &M1_PWM, M1_PWM_CHAN);
    A4960 m2(&M2_SPI, &M2_PWM, M2_PWM_CHAN);

    // ADC setup
    ADS1259 adc(&ADC_SPI);

    // initialize control structure
    Tortilla tortilla(m1, m2, adc, &TIMING_ICU, &BT_SERIAL);

    // start slave threads
//    chThdCreateStatic(waHeartbeat, sizeof(waHeartbeat), IDLEPRIO, tfunc_t(threadHeartbeat), nullptr);
    chThdCreateStatic(waIO, sizeof(waIO), LOWPRIO, tfunc_t(threadIO), &tortilla);

    // done with setup
    palClearPad(GPIOC, GPIOC_LEDB);
    palClearPad(GPIOB, GPIOB_LED2);
    tortilla.fastLoop();
}

/*
 * Application entry point.
 */
int main(void)
{
#if 0
	static const evhandler_t evhndl[] = {
		InsertHandler,
		RemoveHandler
	};
#endif
	/*
	* 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();
	crypto_init();
	buttons_init();

	chHeapInit(fast_heap, FAST_HEAP_ADDR, FAST_HEAP_SIZE);

	sdStart(&SD2, NULL);
	palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
	palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
	lcd_init();

	setvbuf(stdin, NULL, _IONBF, 0);

	mmcObjectInit(&MMCD1);
	mmcStart(&MMCD1, &mmccfg);
	mount_card();

	pwmStart(&PWMD1, &lcd_pwmcfg);
	palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(1));

	// XXX moveme
	palSetPadMode(GPIOB, 6, PAL_MODE_INPUT_PULLUP);
	palSetPadMode(GPIOB, 15, PAL_MODE_INPUT_PULLUP);
	palSetPadMode(GPIOB, 14, PAL_MODE_INPUT_PULLUP);
	palSetPadMode(GPIOC, 10, PAL_MODE_INPUT_PULLUP);
	palSetPadMode(GPIOC, 11, PAL_MODE_INPUT_PULLUP);

	fiprintf(lcd_stdout, "HIHI");
	while (TRUE) {
		console_cmd_loop();
		//chEvtDispatch(evhndl, chEvtWaitOne(ALL_EVENTS));
	}

	return 0;
}