#include <ch32fun.h>
#include <stdint.h>
#include <stdio.h>
#include <fsusb.h>

#include "funconfig.h"
#include "lib_i2c.h"
#include "display.h"
#include "filter.h"
#include "sc7a20.h"
#include "pd.h"
#include "ntc.h"
#include "fpmath.h"
#include "coroutine.h"


// Task timers
struct timer {
	uint32_t start, set;
};

// Set a timer, call timer_expired repeatedly to update it
static inline void timer_set(struct timer *t, uint32_t ms)
{
	t->start = funSysTick32();
	t->set = Ticks_from_Ms(ms);
}

// Check if enough time has passed from timer_set call
static inline bool timer_expired(struct timer *t)
{
	return funSysTick32() > (t->start + t->set);
}


// Button
struct button {
	bool now, prev;
};

static inline void button_init(struct button *btn, bool pullup)
{
	btn->now = btn->prev = pullup;
}

static inline void button_update(struct button *btn, bool state)
{
	btn->prev = btn->now;
	btn->now = state;
}

#define button_rising(btn) ((btn).now == true && (btn).prev == false)
#define button_falling(btn) ((btn).now == false && (btn).prev == true)


/* ------------------------------ Global State ------------------------------ */
u8g2_t *u8g2;                   // Display state
int16_t encoder;                // Rotary encoder counter
struct pd_profile_t pd_profile; // USB Power Delivery profile
uint16_t vcc_mv = 3300;         // Logic (and analog) voltage
uint16_t tip_mv;                // Tip amplifier reading in mV
uint16_t tip_temp_c;            // Converted tip temperature in degrees C
int16_t temp_c;                 // Board temperature in degrees C
uint16_t vbus_mv;               // USB bus voltage in mV
uint16_t current_ma;            // USB bus current to the heater in mA
uint16_t power;                 // Current power provided to the heater
uint16_t duty;                  // Current mosfet driver duty cycle (0-100%)
bool enabled = false;           // Power electronics enabled


// convert the raw TPA191 adc reading to a current in milliamps
static inline int16_t get_current_ma(uint16_t adc_reading)
{
	// Rshunt = 4 milliOhm
	// Gain = 100
	u32 mv = ((u32)adc_reading * vcc_mv) / 4096;
	return (mv * 10) / 4;
}


// this callback is mandatory when FUNCONF_USE_USBPRINTF is defined,
// can be empty though
void handle_usbfs_input(int numbytes, uint8_t *data)
{
	(void)numbytes;
	(void)data;
}


/*
 *    __      ____      ____
 * A:   |____|    |____|    |____
 *         ____      ____      __
 * B: ____|    |____|    |____|
 *      ^ ^  ^ ^  ^ ^  ^ ^  ^ ^
 *      f r  r f  f r  r f  f r
 *      |<---->| single detent
 */
void update_encoder(void)
{
	// 0 = 00, 1 = 01, 2 = 10, 3 = 11
	static uint8_t last_state = 0;
	static int8_t count;
	// Lookup table: state_table[last_state][current_state]
	//  0 = invalid move or bounce (ignore)
	//  1 = valid forward step
	// -1 = valid backward step
	static const int8_t state_table[4][4] = {
		{ 0,  1, -1,  0}, // from 00
		{-1,  0,  0,  1}, // from 01
		{ 1,  0,  0, -1}, // from 10
		{ 0, -1,  1,  0}  // from 11
	};

	// FIXME: even with interrupts disabled and debounce, a single detent triggers
	// multiple interrupts, leading to multiple encoder updates in a short time.
	if (funDigitalRead(PIN_BTN) == 0) return;
	bool a = funDigitalRead(PIN_ENC_A);
	bool b = funDigitalRead(PIN_ENC_B);
	Delay_Us(100);
	if (a != funDigitalRead(PIN_ENC_A) || b != funDigitalRead(PIN_ENC_B))
		return; // debounce

	uint8_t current_state = (a << 1) | b;

	// Find the movement direction based on transition
	count += state_table[last_state][current_state];

#if ENCODER_INV
	if (count == 4) encoder--, count = 0;
	if (count == -4) encoder++, count = 0;
#else
	if (count == 4) encoder++, count = 0;
	if (count == -4) encoder--, count = 0;
#endif

	// Save current state for the next interrupt
	last_state = current_state;
}

// Attached to PIN_ENC_A rising and falling edges
void EXTI7_0_IRQHandler(void) __attribute__((interrupt));
void EXTI7_0_IRQHandler(void)
{
	__disable_irq();
	update_encoder();
	EXTI->INTFR = EXTI_Line3;
	__enable_irq();
}

// Attached to PIN_ENC_B rising and falling edges
void EXTI15_8_IRQHandler(void) __attribute__((interrupt));
void EXTI15_8_IRQHandler(void)
{
	__disable_irq();
	update_encoder();
	EXTI->INTFR = EXTI_Line11;
	__enable_irq();
}


// Procedure to get hardware I2C working on the CH32X035F8U6
static inline void setup_i2c(void)
{
	// Order here matters, first initialize the AFIO and I2C subsystems then
	// change register values, do that the other way around and the configuration
	// wont take effect
	// Enable AFIO (Alternate Function IO)
	RCC->APB2PCENR |= RCC_AFIOEN;
	// Init I2C
	i2c_init(I2C_TARGET, FUNCONF_SYSTEM_CORE_CLOCK, 400000);

	// To utilize the I2C bus we need to disable SWD first, since the pins overlap
	AFIO->PCFR1 &= ~(0b0111 << 24);
	AFIO->PCFR1 |=   0b0100 << 24;

	// Map SCL to PC18 and SDA to PC19
	AFIO->PCFR1 |= 0b0101 << 2;
	// Manually set the I2C pins to Alternate Function IO,  CNF=10b, MODE=10b
	GPIOC->CFGXR &= ~((0xF << 8) | (0xF << 12)); // first clear the bits
	// then set them
	GPIOC->CFGXR |= 0b1010 << 8; // PC18
	GPIOC->CFGXR |= 0b1010 << 12; // PC19
}


volatile uint16_t adc_buffer[16];
// Set-up the adc to perform continous conversion of enabled channels, enable
// channel injection and setup DMA to automatically transfer conversion results
// to a buffer
static inline void setup_adc_and_dma(uint8_t *channels, uint8_t ch_size, uint8_t *injected, uint8_t in_size)
{
	if (channels == NULL || ch_size == 0) return;
	// FIXME: for now only support a single configuration register, so at max 6 channels
	//        I don't want to implement logic to switch registers right now
	// TODO: report an error
	if (ch_size > 6) return;

	// General ADC initialization, I use this only for enabling clocks and such
	funAnalogInit(); // general initialization

	// Set the SCAN and CONT bits to enter continous conversion mode
	// Do not set the IAUTO bit to ensure injected channels are converted only
	// when specified by software
	// Set the regular channel trigger mode to software (EXTSEL = 0b111) to ensure
	// conversion starts in sync with DMA (this should already be done by funAnalogInit())
	ADC1->CTLR1 |= ADC_SCAN;
	ADC1->CTLR2 |= ADC_CONT | ADC_EXTSEL;

	// Setup regular channels (scanned continously)
	// TODO: setup sampling time
	for (uint8_t i = 0; i < ch_size; i++) {
		// Increase sampling time to have less cross talk between channels
		// 0b111 means 11 ADC clock cycles (max)
		if (channels[i] < 10) {
			ADC1->SAMPTR2 |= 0b111 << (channels[i]*3);
		} else {
			ADC1->SAMPTR1 |= 0b111 << ((channels[i]-10)*3);
		}
		ADC1->RSQR3 |= channels[i] << (i*5);
	}
	// Set the number of normal conversion channels
	ADC1->RSQR1 |= (ch_size-1) << 20;

	// Set-up injection channels
	// The injection channel register allows for up to 4 channels, if the buffer is larger
	// skip this step
	// TODO: report an error
	if (injected != NULL && in_size != 0 && in_size <= 4) {
		// enable injection group
		// JEXTSEL = 0b111 (software trigger)
		ADC1->CTLR2 |= ADC_JEXTSEL;
		for (uint8_t i = 0; i < in_size; i++) {
			ADC1->ISQR |= injected[i] << ((4-in_size+i)*5);
		}
		// Set the number of injection channels
		ADC1->ISQR |= (in_size-1) << 20;
	}

	// Set-up DMA, ADC is connected only to channel 1
	// Turn on DMA
	RCC->AHBPCENR |= RCC_AHBPeriph_DMA1;
	// 1. Set the address of the source peripheral, in this case the output
	//    register of the ADC
	DMA1_Channel1->PADDR = (uint32_t)&(ADC1->RDATAR);
	// 2. Set the destination (base) address of the data
	DMA1_Channel1->MADDR = (uint32_t)adc_buffer;
	// 3. Set the number of transfers to be done each cycle, in this case it's
	//    the same as the number of regular conversion cycles
	DMA1_Channel1->CNTR = ch_size;
	// 4. Set mode: Peripheral to Memory, MEM2MEM=0 DIR=0
	//    Circular mode CIRC=1, restart after CNTR transfers
	//    Max priority to not lose conversions and offset values in the buffer
	DMA1_Channel1->CFGR  =
		DMA_M2M_Disable |
		DMA_Priority_VeryHigh |
		DMA_MemoryDataSize_HalfWord |
		DMA_PeripheralDataSize_HalfWord |
		DMA_MemoryInc_Enable |
		DMA_Mode_Circular |
		DMA_DIR_PeripheralSRC;
	// Turn on DMA channel 1
	DMA1_Channel1->CFGR |= DMA_CFGR1_EN;

	// Enable DMA requests from the ADC
	ADC1->CTLR2 |= ADC_DMA;
	// Power up ADC again
	ADC1->CTLR2 |= ADC_ADON;
	// start conversion
	ADC1->CTLR2 |= ADC_SWSTART;
}


// Perform a conversion on the injected ADC channels, this is synchronous and
// halts conversion on the normal channels until it completes
volatile uint16_t injection_results[4];
static inline bool adc_injection_conversion() {
	// Clear any pending flags
	ADC1->STATR &= ~(ADC_JEOC);

	// Start injection conversion using external trigger method
	ADC1->CTLR2 |= ADC_JSWSTART;

	// Wait for all conversions to complete
	s32 timeout = 1000000;
	while(!(ADC1->STATR & ADC_JEOC) && timeout--) {}

	if (timeout <= 0) return false;

	// Read all results
	injection_results[0] = ADC1->IDATAR1 & 0x0FFF;
	injection_results[1] = ADC1->IDATAR2 & 0x0FFF;
	injection_results[2] = ADC1->IDATAR3 & 0x0FFF;
	injection_results[3] = ADC1->IDATAR4 & 0x0FFF;

	// Clear JEOC flag
	ADC1->STATR &= ~ADC_JEOC;

	return true;
}

// mask for the CCxP bits
// when set PWM outputs are held HIGH by default and pulled LOW
// when zero PWM outputs are held LOW by default and pulled HIGH
// #define TIM3_DEFAULT 0xff
#define TIM3_DEFAULT 0x00
static inline void setup_pwm(void)
{
	// Enable TIM3 clock
	RCC->APB1PCENR |= RCC_APB1Periph_TIM3;

	// Since TIM3 is enabled with TIM3->SMCFGR.SMS = 0b000 (default value) the
	// clock source is the internal clock
	// The internal clock goes through the HBB prescaler RCC->CFGR0.HPRE which
	// by default is disabled

	// Reset TIM3 to init all regs
	RCC->APB1PRSTR |= RCC_APB1Periph_TIM3;
	RCC->APB1PRSTR &= ~RCC_APB1Periph_TIM3;

	// set TIM3 clock prescaler divider
	TIM3->PSC = 0x0000; // no prescaler
	// set PWM total cycle width
	static_assert((FUNCONF_SYSTEM_CORE_CLOCK / PWM_FREQ_HZ - 1) < 0xffff, "PWM_FREQ_HZ too low for TIM3");
	TIM3->ATRLR = FUNCONF_SYSTEM_CORE_CLOCK / PWM_FREQ_HZ - 1;

	// for channel 1 let CCxS stay 00 (output), set OC1M to 110 (PWM 1)
	// enabling preload (OC1PE) causes the new pulse width in compare capture
	// register only to come into effect when UG bit in SWEVGR is set
	// (= initiate update) (auto-clears)
	TIM3->CHCTLR1 |= TIM_OC1M_2 | TIM_OC1M_1 | TIM_OC1PE;

	// Enable channel 1 output (PA6)
	TIM3->CCER |= TIM_CC1E | ( TIM_CC1P & TIM3_DEFAULT );

	// CTLR1: default is up, events generated, edge align
	// enable auto-reload of preload
	TIM3->CTLR1 |= TIM_ARPE;
}


static inline void pwm_on(void)
{
	// initialize counter
	TIM3->SWEVGR |= TIM_UG;
	// Enable TIM3
	TIM3->CTLR1 |= TIM_CEN;
}


static inline void pwm_off(void)
{
	// Disable TIM3
	TIM3->CTLR1 &= ~TIM_CEN;
	TIM3->CH1CVR = 0;
}


static inline void pwm_set(uint16_t pulse_width)
{
	TIM3->CH1CVR = pulse_width;
}


// Input: temperature difference
// Output: duty-cycle 0-max_duty
uint16_t pid(int16_t delta, int16_t max_duty)
{
	// PID coefficients
	const fp16_t Kp = num2fp( 1, 1700, 4);
	const fp16_t Ti = num2fp(10, 0000, 4);
	const fp16_t Td = num2fp( 0, 1500, 4);

	static fp16_t err_p, err_i, intgrt, err_d, dt, prev_err;
	static u32 t, prev_t;

	t = funSysTick32();
	dt = fp_div(i2fp((t-prev_t)/DELAY_MS_TIME), i2fp(1000));

	// if pid was paused, reset the integrator
	if (dt > num2fp(0, 2, 1)) intgrt = 0;

	fp16_t err = i2fp(delta); // temperature delta as fixed point number

	err_p = err;
	err_i = fp_mul(fp_div(err, Ti), dt);
	err_d = fp_mul(fp_div(fp_sub(err, prev_err), dt), Td);

	prev_err = err;
	prev_t = t;

	fp16_t e = 0;
	e = fp_add(e, err_p);
	e = fp_add(fp_add(e, err_i), intgrt);
	e = fp_add(e, err_d);
	e = fp_mul(e, Kp);

	// TODO: use a back calculation anti windup strategy
	// only integrate if the output is less then max
	if (e < i2fp(100) && e > 0) {
		intgrt = fp_add(intgrt, err_i);
	}

	e = fp_clamp(e, i2fp(0), i2fp(100));

	// TODO: implement minimum duty cycle
	return fp2i(fp_map(e, 0, i2fp(100), 0, i2fp(max_duty)));
}


/* ------------------------------ Board Setup ------------------------------- */
static inline void setup(void)
{
	// Generic system setup
	SystemInit();
	funGpioInitAll();
	USBFSSetup();

	// ====================== Analog pin configuration ====================== //
	funPinMode(PIN_VBUS, GPIO_CFGLR_IN_ANALOG);
	funPinMode(PIN_CURRENT, GPIO_CFGLR_IN_ANALOG);
	funPinMode(PIN_NTC, GPIO_CFGLR_IN_ANALOG);
	funPinMode(PIN_TEMP, GPIO_CFGLR_IN_ANALOG);

	// Analog inputs setup (dma and injection conversion)
	uint8_t adc_channels[] = {
		VBUS_ADC_CHANNEL,
		CURRENT_ADC_CHANNEL,
		NTC_ADC_CHANNEL
	};
	uint8_t adc_injected[] = {
		TEMP_ADC_CHANNEL,
		VREF_INT_CHANNEL
	};
	setup_adc_and_dma(adc_channels, sizeof(adc_channels), adc_injected, sizeof(adc_injected));

	// ====================== Digital pin configuration ===================== //
	funPinMode(PIN_12V, GPIO_CFGLR_OUT_10Mhz_PP);
	funDigitalWrite(PIN_12V, 0);
	funPinMode(PIN_HEATER, GPIO_CFGLR_OUT_10Mhz_AF_PP);
	funDigitalWrite(PIN_HEATER, 0);
	funPinMode(PIN_DISP_RST, GPIO_CFGLR_OUT_10Mhz_PP);
	funDigitalWrite(PIN_DISP_RST, 1);

	funPinMode(PIN_ENC_A, GPIO_CFGLR_IN_PUPD); // enable pull-up/down
	funDigitalWrite(PIN_ENC_A, 1);             // specify pull-up
	funPinMode(PIN_ENC_B, GPIO_CFGLR_IN_PUPD); // enable pull-up/down
	funDigitalWrite(PIN_ENC_B, 1);             // specify pull-up
	funPinMode(PIN_BTN, GPIO_CFGLR_IN_FLOAT);

	// Setup PWM timer, includes pin setup
	setup_pwm();
	pwm_off();

	// =========================== Encoder Setup ============================ //
	// Configure the IO as an interrupt.
	// PIN_ENC_B is on port B, channel 11
	AFIO->EXTICR1 |= AFIO_EXTICR1_EXTI11_PB; // Port B channel (pin) 11
	EXTI->INTENR |= EXTI_INTENR_MR11;        // Enable EXT11
	EXTI->FTENR |= EXTI_FTENR_TR11;          // Falling edge trigger
	EXTI->RTENR |= EXTI_RTENR_TR11;          // Rising edge trigger
	// enable interrupt
	NVIC_EnableIRQ(EXTI15_8_IRQn);

	// PIN_ENC_A is on port B, channel 3
	AFIO->EXTICR1 |= AFIO_EXTICR1_EXTI3_PB; // Port B channel (pin) 3
	EXTI->INTENR |= EXTI_INTENR_MR3;        // Enable EXT3
	EXTI->FTENR |= EXTI_FTENR_TR3;          // Falling edge trigger
	EXTI->RTENR |= EXTI_RTENR_TR3;          // Rising edge trigger
	// enable interrupt
	NVIC_EnableIRQ(EXTI7_0_IRQn);

	// ============================= I2C Setup ============================== //
	// Setup i2c line, includes setting the pin alternate mode
	setup_i2c();
	// Start i2c peripherals
	u8g2 = display_init();
	sc7a20_init();
}


// Update the global istantaneous voltage, current, power and board temperature
// this requires the automatic conversion (ADC+DMA) to be running
static inline void update_power_and_temperature(void)
{
	// Voltage current and power
	vbus_mv    = U16_FP_EMA_K4(vbus_mv, ((u32)adc_buffer[0]*vcc_mv*11)/4096);
	current_ma = U16_FP_EMA_K4(current_ma, get_current_ma(adc_buffer[1]));
	power      = ((u32)vbus_mv*current_ma)/1000000;
	// Board temperature
	temp_c     = I16_FP_EMA_K4(temp_c, get_temp_c(adc_buffer[2]));
}


// Update VCC and the tip temperature, both are sampled with injection conversion
// so it is triggered in this function
static inline void update_tip_and_vcc(void)
{
	// Tip calibration factors
	const fp16_t tip_k = num2fp(0, 14473, 5);
	const fp16_t tip_off = num2fp(0, 0, 0);

	adc_injection_conversion();
	// Calibrate VCC with the internal reference value
	vcc_mv = I16_FP_EMA_K4(vcc_mv, ((uint32_t)1200 * 4096)/injection_results[1]);

	uint16_t tip_mv = ((u32)injection_results[0]*vcc_mv)/4096;
	int16_t tt_now  = fp2i(fp_add(fp_mul(i2fp(tip_mv), tip_k), tip_off));
	// Tip temp with cold-junction compensation, assume the board temperature is
	// the same as the cold junction, on an external handle this is not always true
	tip_temp_c = I16_FP_EMA_K4(tip_temp_c, tt_now + temp_c);
}


// coroutine to draw the "main" ui which draws the current tip temperature, power
// pwm state, etc.
coro_state_t cs_ui;
coroutine c_draw_main_ui(coro_state_t *state)
{
	static struct timer t;
	coro_begin(state);

	for(;;) {
		// Wait for the next frame, roughly 30 fps
		timer_set(&t, 33);
		coro_wait_until(state, timer_expired(&t));

		// Draw UI
		u8g2_ClearBuffer(u8g2);
		u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
		draw_temp(u8g2, x_off+0, y_off+0, tip_temp_c, true);
		u8g2_DrawStr(u8g2, x_off+32, y_off+6, "A:");
		u8g2_DrawStr(u8g2, x_off+42, y_off+6, u16toa((current_ma+500)/1000));
		u8g2_DrawStr(u8g2, x_off+60, y_off+6, "V:");
		u8g2_DrawStr(u8g2, x_off+70, y_off+6, u16toa((vbus_mv+500)/1000));
		uint8_t p = (power*100)/pd_profile.set_power;
		uint8_t w = (uint16_t)(p*54)/100;
		u8g2_DrawBox(u8g2, x_off+42, y_off+14, w, 5);
	}

	coro_end();
}


// coroutine to update the tip temperature, vcc and compute the new pid value
// this has to be called periodically but only with pwm disabled
coro_state_t cs_duty;
coroutine c_update_duty(coro_state_t *state)
{
	static struct timer t;
	coro_begin(state);

	for(;;) {
		timer_set(&t, 50);
		coro_wait_until(state, timer_expired(&t));

		pwm_set(0);
		Delay_Ms(TURN_OFF_DELAY);
		update_tip_and_vcc();
		if (enabled) {
			duty = pid((int16_t)pd_profile.set_temp - tip_temp_c, pd_profile.max_duty);
		} else {
			duty = 0;
		}
	}

	coro_end();
}


__attribute__((noreturn)) int main(void)
{
	setup();

	coro_init(&cs_ui);
	coro_init(&cs_duty);

	u8g2_ClearBuffer(u8g2);
	u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
	u8g2_DrawStr(u8g2, x_off+0, y_off+7, "Negotiating...");
	u8g2_SendBuffer(u8g2);

	// Init USBPD
	bool has_pd = pd_negotiate(eUSBPD_VCC_3V3);
	if (has_pd == false) {
		while (true) {
			// No Power Delivery, maybe we are attached to a computer, so poll the input
			poll_input();

			update_power_and_temperature();
			update_tip_and_vcc();

			u8g2_ClearBuffer(u8g2);
			u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
			u8g2_DrawStr(u8g2, x_off+0, y_off+7, "Negotiation FAILED");
			u8g2_DrawStr(u8g2, x_off+0, y_off+14, USBPD_ResultToStr(pd_get_result()));
			u8g2_DrawStr(u8g2, x_off+0, y_off+21, u16toa(vcc_mv));
			u8g2_DrawStr(u8g2, x_off+0, y_off+28, u16toa(temp_c));
			u8g2_DrawStr(u8g2, x_off+30, y_off+28, u16toa(tip_temp_c));
			u8g2_SendBuffer(u8g2);
		}
	} else {
		pd_get_profile(&pd_profile, 100);

		// TODO: let the user decide the power profile
		pd_profile.set_temp = 320;
		pd_profile.set_power = 95; // Slightly below max power to avoid overloading
		pd_profile.tip_r = 2500; // TODO: tip check and resistance calculator
		pd_profile.max_duty = MIN(
			(100*(u32)isqrt((
				MIN(pd_profile.set_power, pd_profile.power_avail)*pd_profile.tip_r)/1000) * 1000) / pd_profile.voltage
			, 100);
	}


	enum {
		STATE_MENU,
		STATE_HEATING,
	} state = STATE_MENU;

	/* ============================== Main Loop =============================== */
	struct button btn;
	button_init(&btn, true);
	for (;;) {
		button_update(&btn, funDigitalRead(PIN_BTN));

		update_power_and_temperature();

		c_update_duty(&cs_duty);

		switch (state) {
		case STATE_MENU:
			u8g2_ClearBuffer(u8g2);
			u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
			u8g2_DrawStr(u8g2, x_off+0, y_off+7, "TEMP:");
			u8g2_DrawStr(u8g2, x_off+25, y_off+7, u8g2_u16toa(pd_profile.set_temp, 4));

			if (encoder != 0) {
				pd_profile.set_temp += encoder >= ENCODER_FAST ? 20 : encoder;
				encoder = 0;
				if (pd_profile.set_temp < MIN_TIP_SET_TEMP) pd_profile.set_temp = MIN_TIP_SET_TEMP;
				if (pd_profile.set_temp > MAX_TIP_SET_TEMP) pd_profile.set_temp = MAX_TIP_SET_TEMP;
			}

			if (button_falling(btn)) {
				state = STATE_HEATING;
				enabled = false;
			}
			break;
		case STATE_HEATING:
			c_draw_main_ui(&cs_ui);

			if (enabled) {
				funDigitalWrite(PIN_12V, 1);

				// Safety logic
				if (current_ma > pd_profile.max_current + pd_profile.max_current/10) {
					enabled = false;
				}
				if (temp_c > MAX_BOARD_TEMP) {
					enabled = false;
				}
				if (tip_temp_c > MAX_TIP_TEMP) {
					enabled = false;
				}

				const uint16_t tim_max = FUNCONF_SYSTEM_CORE_CLOCK / PWM_FREQ_HZ - 1;
				pwm_set(((u32)duty*tim_max)/100);
				u8g2_DrawBox(u8g2, x_off+92, y_off+0, 4, 4);
			} else {
				funDigitalWrite(PIN_12V, 0);
			}

			// move to menu mode when encoder is turned
			if (encoder != 0) {
				state = STATE_MENU;
				funDigitalWrite(PIN_12V, 0);
				pwm_set(0);
			}

			// Check button to toggle enable
			if (button_falling(btn)) {
				enabled = !enabled;
				if (enabled) {
					pwm_on();
				} else {
					pwm_off();
				}
			}
			break;
		}
		u8g2_SendBuffer(u8g2);
	}
}
