#include "main.h"
#include "stm32g4xx_hal_gpio.h"
#include "big_vars.h"
#include "stdbool.h"
#include "PW_SX_V1.00_lib/PW_SX_V1.00_lib.hpp"

#include "string.h"
#include <stdio.h>


#ifndef __6EDL7141_DRIVER_HPP
#define __6EDL7141_DRIVER_HPP

#include "stm32g4xx.h"


typedef unsigned char __u8;
typedef unsigned short __u16;
typedef unsigned long __u32;
typedef __u32 canid_t;
typedef uint8_t byte;

#ifdef __cplusplus
extern "C" {
#endif

#define PC3_ON 	GPIOC->BSRR = (uint32_t) GPIO_PIN_3;
#define PC3_OFF	GPIOC->BRR  = (uint32_t) GPIO_PIN_3;

#define PA11_ON  GPIOA->BSRR = (uint32_t) GPIO_PIN_11;
#define PA11_OFF GPIOA->BRR  = (uint32_t) GPIO_PIN_11;

// ###############################################################
// ############## USE MACROS WITH ';' AT THE END!!!! #############
// ###############################################################
#define ADC_TEST_ON  //PC3_ON
#define ADC_TEST_OFF //PC3_OFF

#define TEST_1_ON 	//PC3_ON
#define TEST_1_OFF	//PC3_OFF

#define TEST_HALL_ON 	PC3_ON
#define TEST_HALL_OFF	PC3_OFF

#define TEST_TIME_TIM8CC_ON		PC3_ON//PA11_ON
#define TEST_TIME_TIM8CC_OFF	PC3_OFF//PA11_OFF

#define TEST_BOUNCE_ON 	//PC3_ON
#define TEST_BOUNCE_OFF	//PC3_OFF

#define TEST_TIM8_COM_ON 	//PC3_ON
#define TEST_TIM8_COM_OFF	//PC3_OFF

#define ADC_TEST_2_ON
#define ADC_TEST_2_OFF

#define TEST_WAVEJUMP_ON PA11_ON
#define TEST_WAVEJUMP_OFF PA11_OFF


#define CONTROLLER_TASKS_INTERVAL 100

typedef enum
{
	MOTOR_UNDEFINED = 0U,
	MOTOR_STOPPED,		// Does not turn, or turnes slowly which is considered stopped
	MOTOR_STARTED,		// Has not completed a full electrical rotation yet
	MOTOR_RUNNING,		// Has at least completet a full electrical rotation, and is actively beeing driven
	MOTOR_FADING			/* Has completed a full electrical roation, but has not slowed down enough to be considered beeing stopped (TODO: maybe add speacial case,
   	  	  	  	  	  	 	 where the motor is completely stopped, but the bike has been rolling constantly without the possibility of the motor beeing driven
   	  	  	  	  	  	 	 backwards, by pushing the bike backwards. - Maybe this case is unnessary, if hallstate determination is used everytime)
	 */
} Motor_State;


void EXTI2_IRQHandler(void);
void TIM8_CC_IRQHandler(void);

void controllerTasks(void);
void loadNextSectorSixStep();

//bool motorRunning = false;

void drvFaultHandler();
void startTimer8();
void initHallTim1AndDriverTim8();
void setDuty(uint32_t newValue);

void updateDuty();



void switchToSinModeReqFunc();
void switchToSinusoidalMode();
void switchToSixStepMode();


void TIM1_CC_IRQHandler();
void TIM1_TRG_COM_TIM17_IRQHandler();
void TIM8_TRG_COM_IRQHandler();

void TIM8_UP_IRQHandler();
void TIM8_CC_IRQHandler();
void ADC1_2_IRQHandler();
void loadSector1(void);
void startMotor(void);
void stopMotor(void);

void DMA1_Channel2_IRQHandler();

void initADC_1(void);
void initADC_2(void);
void initADCCommon(void);
void initDMAADC2_Regular(void);
void initDMAADC1_Injected(void);
void initDMAADC2_Injected(void);

void initTestTim2(void);

void gpioInitDriver(void);

int determineSector(void);

void readDataSPIDriver(byte address, byte *data);
void sendDataDriver(byte address, byte *data);
void initSPIDriver(void);

void startDriver(void);

void stopDriver(void);

void jumpStartDriver(void);

void writeDataSPIDriver(byte address, byte firstByte, byte secondByte);

void initTimer8PWM(void );
void setupDriver(void );

void loadNextSector(void);
void switchToSectorOFF(void);

void readAndPrintDrvRegister(byte address);

void readAndPrintDrvAllRegisters(void);

void initNFaultInterrupt(void);

void readAndPrintDrvStatusRegisters(void);
void readAndPrintDrvRegisterHex(void);




#ifdef __cplusplus
}
#endif






#endif //__6EDL7141_DRIVER_HPP



uint32_t micros(void);
uint32_t millis(void);


volatile byte requestedDuty = 0;		// From pedal power of user
volatile byte outputDuty = 0;

volatile unsigned long lastHallIntervalTics = 1;	// 1 to not get hardfault when dividing through it (cant divide through 0)


volatile byte elecRevCount = 0;


byte motorTimeoutCount = 0;

volatile bool sinusoidalMode = false;


volatile int motorRequest = MOTOR_UNDEFINED;
volatile int motorSTATE = MOTOR_STOPPED;
volatile byte firstSectorOnStart = 0;

#define PSEUDO_ROTATION_ANGLE 384
#define PSEUDO_SECTOR_ANGLE (PSEUDO_ROTATION_ANGLE / 6)

// Angle in electrical angle (384 is 360 degrees !)
uint16_t motorAngle = 0;

//TODO: make sure, it goes never above PSEUDO_SECTOR_ANGLE? or maybe let it go above (is needed?)?
uint8_t advanceAngle = 16;
uint16_t advanceTime = 0;

uint16_t angleTable[] = {0,64,128,192,256,320};

static uint8_t SinusoidalWaveTable[384] = {
		1,2,4,5,6,8,9,10,12,13,14,16,17,18,20,21,22,24,25,26,28,29,30,32,33,34,36,37,38,40,41,42,44,45,46,47,49,50,51,53,54,55,57,58,59,61,62,63,65,66,67,69,70,71,73,74,75,77,78,79,81,
		82,83,85,86,87,89,90,91,93,94,95,97,98,99,101,102,103,105,106,107,109,110,111,113,114,115,117,118,119,121,122,123,125,126,127,129,130,131,132,134,135,136,138,139,140,142,143,
		144,146,147,148,150,151,152,154,155,156,158,159,160,162,163,164,166,167,168,170,171,172,174,175,176,178,179,180,182,183,184,186,187,188,190,191,192,194,195,196,198,199,200,202,
		203,204,206,207,208,210,211,212,214,215,216,217,219,220,221,223,224,225,227,228,229,231,232,233,235,236,237,239,240,241,243,244,245,247,248,249,251,252,253,255,253,252,251,249,
		248,247,245,244,243,241,240,239,237,236,235,233,232,231,229,228,227,225,224,223,221,220,219,217,216,215,214,212,211,210,208,207,206,204,203,202,200,199,198,196,195,194,192,191,
		190,188,187,186,184,183,182,180,179,178,176,175,174,172,171,170,168,167,166,164,163,162,160,159,158,156,155,154,152,151,150,148,147,146,144,143,142,140,139,138,136,135,134,132,
		131,130,129,127,126,125,123,122,121,119,118,117,115,114,113,111,110,109,107,106,105,103,102,101,99,98,97,95,94,93,91,90,89,87,86,85,83,82,81,79,78,77,75,74,73,71,70,69,67,66,
		65,63,62,61,59,58,57,55,54,53,51,50,49,47,46,45,44,42,41,40,38,37,36,34,33,32,30,29,28,26,25,24,22,21,20,18,17,16,14,13,12,10,9,8,6,5,4,2,1,0,
};


/*
 * Timer 8 is APB2 => 85MHz
 * 35kHz and 1/12 prescaler -> ~200 tics for one pwm cycle
 * ARR is counted up and down, so just half the tics for calculations
 *
 */
// Prescaler 12 for 34kHz
#define TIM_8_PSC 12
#define PWM_HALF_CYCLE_TICS 100	// DO NOT CHANGE without changing DUTY_TO_TICS, as it has been made  1:1 to not calculate unessary
#define DUTY_TO_TICS(duty) ((duty))// * PWM_HALF_CYCLE_TICS) / 100)
#define ADC_TRIG_PRE_COUNT 1

void setDuty(uint32_t newValue){
	requestedDuty = newValue;
}

float speedLimitFactor = 0.0;

/*
 * Used for regulation only
 */
void updateDuty(){
	speedLimitFactor = millis() % 4000 / 4000.0;	//TODO: debug remove implement different
	outputDuty = (byte) (requestedDuty * speedLimitFactor);	// TODO: make regulationFactor to not affect duty (or effect less) on low requested duty. This would
	if(sinusoidalMode == false){
		WRITE_REG(TIM8->CCR1, DUTY_TO_TICS(outputDuty));
		WRITE_REG(TIM8->CCR2, DUTY_TO_TICS(outputDuty));
		WRITE_REG(TIM8->CCR3, DUTY_TO_TICS(outputDuty));
	}
}

void initTimer8PWM(){
	__HAL_RCC_TIM8_CLK_ENABLE();		// Enable clock

	CLEAR_REG(TIM8->AF1);				// Disable BKIN input
	CLEAR_REG(TIM8->AF2);

	SET_BIT(TIM8->CR1, 	TIM_CR1_ARPE | 	// ARR register is bufferd
			TIM_CR1_CMS_1 	// center aligned up/down (interrupt on counting up - for ADC trigger only?)
	);

	//TODO: check entries again for pwm
	SET_BIT(TIM8->CR2, 	TIM_CR2_MMS2_2 | TIM_CR2_MMS2_1 | TIM_CR2_MMS2_0 |// tim_oc4refc or tim_oc6refc rising edges generate pulses on	tim_trgo2 for ADC trigger
			TIM_CR2_CCPC |	// Preload enable
			TIM_CR2_CCUS);

	//TIMx_ARR -1 ist max counter (Counter is counted up and down, so PWM has half the tics)
	WRITE_REG(TIM8->ARR, PWM_HALF_CYCLE_TICS); 	// Counter TOP
	WRITE_REG(TIM8->CCR4, ADC_TRIG_PRE_COUNT);	// compare value 4 (ADC trigger)


	TIM8->PSC = TIM_8_PSC - 1;

	//TODO: DEBUG REMOVE
	setDuty(2);
	//	WRITE_REG(TIM8->CCR1, DUTY_TO_TICS(10));	// compare value 1
	//	WRITE_REG(TIM8->CCR2, DUTY_TO_TICS(10));	// compare value 2
	//	WRITE_REG(TIM8->CCR3, DUTY_TO_TICS(10));	// compare value 3


	// ***************************************************************
	// ****** ATTENTION! TIM CH 1 is PH 3 and CH 3 is PH 1 !!!  ******
	// ***************************************************************

	SET_BIT(TIM8->CCMR1, 	TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1	| // pwm output compare polarity channel 1 and 2 (pwm mode 1)
			TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 |
			TIM_CCMR1_OC1PE  | TIM_CCMR1_OC2PE); 	// preload enable ccr 1 + 2
	// Default is OUTPUT !!!
	// CC1S = 0 > channel output (CC1 as OUTPUT)

	SET_BIT(TIM8->CCMR2, 	TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1	| // pwm output compare polarity channel 3 and 4 (pwm mode 1)
			TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1 |
			TIM_CCMR2_OC3PE  | TIM_CCMR2_OC4PE); 	// preload enable ccr 3 + 4



	// Timer 1 trigger out comes in on itr0 (Only trgo is possible!)
	//TIM8->SMCR itr0 is default 0!


	//TODO: debug remove
	//SET_BIT(TIM8->CCER, TIM_CCER_CC4E);

	//	TIM8->DIER |= TIM_DIER_CC4IE;	// Enable interrupts (from ADC trigger) for kind of sine wave generation
	//TIM8->CR1 |= TIM_CR1_URS;	does it work with update from timer 1?// Only update interrupt on overflow and DMA TODO: does it work with interrupt

	SET_BIT(TIM8->BDTR, TIM_BDTR_OSSR); // inactive state high or low
	CLEAR_BIT(TIM8->CCER, TIM_CCER_CC1P);// oc1 polarity active high
	CLEAR_BIT(TIM8->CCER, TIM_CCER_CC2P);// oc2 polarity active high
	CLEAR_BIT(TIM8->CCER, TIM_CCER_CC3P);// oc3 polarity active high
	// Polarity ONLY for pins, has no effect on trigger lines!!!
}


/*
 * Exiting interrupt with if(true)+(2 NOP´s) is about 20ns, Checking for "! (TIM8->SR & TIM_SR_COMIF)" adds 120ns!
 * TODO: Debug remove
 */
void TIM8_TRG_COM_IRQHandler(){
	//TEST_TIM8_COM_ON
	if(! (TIM8->SR & TIM_SR_COMIF)){	// Return if no flag is set (prevents false triggers because of latency in the bus)
		__ASM volatile ("NOP");
		__ASM volatile ("NOP");
		TEST_TIM8_COM_OFF;
		return;
	}

	//print((uint32_t) (determineSector()));


	TIM8->SR = ~TIM_SR_COMIF;	// DONT send "&=" ReadModifyWrite is bad!
	__ASM volatile ("NOP");
	__ASM volatile ("NOP");
	TEST_TIM8_COM_OFF;
}


void startTimer8(){
	__disable_irq();

	SET_BIT(TIM8->EGR , 1);				// Send event, to load all register via Update event (UG)

	SET_BIT(TIM8->CR1, TIM_CR1_CEN); 	// Counter enable
	switchToSectorOFF();				// Dont drive any outputs yet!
	SET_BIT(TIM8->BDTR, TIM_BDTR_MOE); 	// Connect signals to outputs (When CCxE and CCxNE are set - see also OSSR and OSSI)
	WRITE_REG(TIM8->SR, 0); 			// Clear any int flags
	__enable_irq();						// TODO: set old state
}


volatile byte loadedSector = 0;
volatile byte sector = 1;

// ***************************************************************
// ****** ATTENTION! TIM CH 1 is PH 3 and CH 3 is PH 1 !!!  ******
// ***************************************************************
// Forcing low is really needed, to bring the level down quickly. But the deactivation of its channel has to happen at a later stage to have a sharp edge
// Configuration is only preloaded! Activated through com update via TIM 1 trigger.

// TODO: Get rid of soft switchting, and use complimentary switching: https://www.youtube.com/watch?v=QgcAJpRFj1U&list=PLaBr_WzeIAixidGwqfcrQlwKZX4RZ2E7D&index=7



//################### PH 1 ######################
#define REF_1_FORCE_LOW  	MODIFY_REG(TIM8->CCMR2, TIM_CCMR2_OC3M_Msk, TIM_CCMR2_OC3M_2)
#define REF_1_FORCE_HIGH  	MODIFY_REG(TIM8->CCMR2, TIM_CCMR2_OC3M_Msk, TIM_CCMR2_OC3M_2 |					 TIM_CCMR2_OC3M_0)
#define REF_1_PWM			MODIFY_REG(TIM8->CCMR2, TIM_CCMR2_OC3M_Msk, TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1)
#define HS_1_ENABLE			  SET_BIT(TIM8->CCER, TIM_CCER_CC3E)
#define HS_1_DISABLE		CLEAR_BIT(TIM8->CCER, TIM_CCER_CC3E)
#define LS_1_ENABLE			  SET_BIT(TIM8->CCER, TIM_CCER_CC3NE)
#define LS_1_DISABLE		CLEAR_BIT(TIM8->CCER, TIM_CCER_CC3NE)

//################### PH 2 ######################
#define REF_2_FORCE_LOW  	MODIFY_REG(TIM8->CCMR1, TIM_CCMR1_OC2M_Msk, TIM_CCMR1_OC2M_2)
#define REF_2_FORCE_HIGH  	MODIFY_REG(TIM8->CCMR1, TIM_CCMR1_OC2M_Msk, TIM_CCMR1_OC2M_2 |					 TIM_CCMR1_OC2M_0)
#define REF_2_PWM			MODIFY_REG(TIM8->CCMR1, TIM_CCMR1_OC2M_Msk, TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1)
#define HS_2_ENABLE			  SET_BIT(TIM8->CCER, TIM_CCER_CC2E)
#define HS_2_DISABLE		CLEAR_BIT(TIM8->CCER, TIM_CCER_CC2E)
#define LS_2_ENABLE			  SET_BIT(TIM8->CCER, TIM_CCER_CC2NE)
#define LS_2_DISABLE		CLEAR_BIT(TIM8->CCER, TIM_CCER_CC2NE)

//################### PH 3 ######################
#define REF_3_FORCE_LOW  	MODIFY_REG(TIM8->CCMR1, TIM_CCMR1_OC1M_Msk, TIM_CCMR1_OC1M_2)
#define REF_3_FORCE_HIGH  	MODIFY_REG(TIM8->CCMR1, TIM_CCMR1_OC1M_Msk, TIM_CCMR1_OC1M_2 |					 TIM_CCMR1_OC1M_0)
#define REF_3_PWM			MODIFY_REG(TIM8->CCMR1, TIM_CCMR1_OC1M_Msk, TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1)
#define HS_3_ENABLE			  SET_BIT(TIM8->CCER, TIM_CCER_CC1E)
#define HS_3_DISABLE		CLEAR_BIT(TIM8->CCER, TIM_CCER_CC1E)
#define LS_3_ENABLE			  SET_BIT(TIM8->CCER, TIM_CCER_CC1NE)
#define LS_3_DISABLE		CLEAR_BIT(TIM8->CCER, TIM_CCER_CC1NE)



//################### PH 1  SIX STEP ######################
#define SIX_STEP_PRELOAD_HS_1_PWM 					REF_1_PWM; 			HS_1_ENABLE;
#define SIX_STEP_PRELOAD_HS_1_LOW					REF_1_FORCE_LOW;

#define SIX_STEP_PRELOAD_LS_1_PWM					REF_1_PWM;			LS_1_ENABLE;
#define SIX_STEP_PRELOAD_LS_1_HIGH  	HS_1_DISABLE;	REF_1_FORCE_HIGH; 	LS_1_ENABLE;	// Disable HS here to get sharp edges on it. Has to be forced low some time before disabling it.
#define SIX_STEP_PRELOAD_LS_1_LOW					REF_1_FORCE_LOW;	LS_1_DISABLE;

//################### PH 2  SIX STEP ######################
#define SIX_STEP_PRELOAD_HS_2_PWM 					REF_2_PWM; 			HS_2_ENABLE;
#define SIX_STEP_PRELOAD_HS_2_LOW					REF_2_FORCE_LOW;

#define SIX_STEP_PRELOAD_LS_2_PWM					REF_2_PWM;			LS_2_ENABLE;
#define SIX_STEP_PRELOAD_LS_2_HIGH  	HS_2_DISABLE;	REF_2_FORCE_HIGH; 	LS_2_ENABLE;	// Disable HS here to get sharp edges on it. Has to be forced low some time before disabling it.
#define SIX_STEP_PRELOAD_LS_2_LOW					REF_2_FORCE_LOW;	LS_2_DISABLE;

//################### PH 3  SIX STEP ######################
#define SIX_STEP_PRELOAD_HS_3_PWM 					REF_3_PWM; 			HS_3_ENABLE;
#define SIX_STEP_PRELOAD_HS_3_LOW					REF_3_FORCE_LOW;

#define SIX_STEP_PRELOAD_LS_3_PWM					REF_3_PWM;			LS_3_ENABLE;
#define SIX_STEP_PRELOAD_LS_3_HIGH  	HS_3_DISABLE;	REF_3_FORCE_HIGH; 	LS_3_ENABLE;	// Disable HS here to get sharp edges on it. Has to be forced low some time before disabling it.
#define SIX_STEP_PRELOAD_LS_3_LOW					REF_3_FORCE_LOW;	LS_3_DISABLE;



//TODO: disabling preload is needed while changing preload values? Think about throughshoot!
void loadSectorSixStep1(){
	SET_BIT(  TIM8->SMCR, TIM_SMCR_SMS_2); 		//TODO: debug remove  // with clear below: reset counter and prescaler on trigger input (The PWM pulse will not move then anymore)
	SIX_STEP_PRELOAD_HS_1_PWM
	SIX_STEP_PRELOAD_LS_1_PWM
	SIX_STEP_PRELOAD_HS_3_LOW
	SIX_STEP_PRELOAD_LS_3_LOW
	loadedSector = 1;
}

void loadSectorSixStep2(){
	CLEAR_BIT(  TIM8->SMCR, TIM_SMCR_SMS_2); 	//TODO: debug remove  // with set above: reset counter and prescaler on trigger input (The PWM pulse will not move then anymore)
	SIX_STEP_PRELOAD_LS_2_LOW
	SIX_STEP_PRELOAD_LS_3_HIGH
	loadedSector = 2;
}

void loadSectorSixStep3(){
	SIX_STEP_PRELOAD_HS_1_LOW
	SIX_STEP_PRELOAD_LS_1_LOW
	SIX_STEP_PRELOAD_HS_2_PWM
	SIX_STEP_PRELOAD_LS_2_PWM
	loadedSector = 3;
}

void loadSectorSixStep4(){
	SIX_STEP_PRELOAD_LS_1_HIGH
	SIX_STEP_PRELOAD_LS_3_LOW
	loadedSector = 4;
}

void loadSectorSixStep5(){
	SIX_STEP_PRELOAD_HS_2_LOW
	SIX_STEP_PRELOAD_LS_2_LOW
	SIX_STEP_PRELOAD_HS_3_PWM
	SIX_STEP_PRELOAD_LS_3_PWM
	loadedSector = 5;
}

void loadSectorSixStep6(){
	SIX_STEP_PRELOAD_LS_1_LOW
	SIX_STEP_PRELOAD_LS_2_HIGH
	loadedSector = 6;

}


//################### PH 1 SINUSOIDAL ######################
#define SINUSOID_PRELOAD_HS_1_PWM 					REF_1_PWM; 			HS_1_ENABLE;
#define SINUSOID_PRELOAD_HS_1_LOW					REF_1_FORCE_LOW;

#define SINUSOID_PRELOAD_LS_1_PWM					REF_1_PWM;			LS_1_ENABLE;
#define SINUSOID_PRELOAD_LS_1_HIGH  					REF_1_FORCE_HIGH; 	LS_1_ENABLE;	// Disable HS here to get sharp edges on it. Has to be forced low some time before disabling it.
#define SINUSOID_PRELOAD_LS_1_LOW					REF_1_FORCE_LOW;

//################### PH 2 SINUSOIDAL ######################
#define SINUSOID_PRELOAD_HS_2_PWM 					REF_2_PWM; 			HS_2_ENABLE;
#define SINUSOID_PRELOAD_HS_2_LOW					REF_2_FORCE_LOW;

#define SINUSOID_PRELOAD_LS_2_PWM					REF_2_PWM;			LS_2_ENABLE;
#define SINUSOID_PRELOAD_LS_2_HIGH  					REF_2_FORCE_HIGH; 	LS_2_ENABLE;	// Disable HS here to get sharp edges on it. Has to be forced low some time before disabling it.
#define SINUSOID_PRELOAD_LS_2_LOW					REF_2_FORCE_LOW;

//################### PH 3 SINUSOIDAL ######################
#define SINUSOID_PRELOAD_HS_3_PWM 					REF_3_PWM; 			HS_3_ENABLE;
#define SINUSOID_PRELOAD_HS_3_LOW					REF_3_FORCE_LOW;

#define SINUSOID_PRELOAD_LS_3_PWM					REF_3_PWM;			LS_3_ENABLE;
#define SINUSOID_PRELOAD_LS_3_HIGH  					REF_3_FORCE_HIGH; 	LS_3_ENABLE;	// Disable HS here to get sharp edges on it. Has to be forced low some time before disabling it.
#define SINUSOID_PRELOAD_LS_3_LOW					REF_3_FORCE_LOW;


void loadSectorSinusoid1(){
	SET_BIT(  TIM8->SMCR, TIM_SMCR_SMS_2); 		//TODO: debug remove  // with clear below: reset counter and prescaler on trigger input (The PWM pulse will not move then anymore)
	loadedSector = 1;
}

void loadSectorSinusoid2(){
	CLEAR_BIT(  TIM8->SMCR, TIM_SMCR_SMS_2); 	//TODO: debug remove  // with set above: reset counter and prescaler on trigger input (The PWM pulse will not move then anymore)
	loadedSector = 2;
}

void loadSectorSinusoid3(){
	loadedSector = 3;

}

void loadSectorSinusoid4(){
	loadedSector = 4;
}

void loadSectorSinusoid5(){
	loadedSector = 5;
}

void loadSectorSinusoid6(){
	loadedSector = 6;
}


void switchToSectorOFF(){
	HS_1_DISABLE;
	LS_1_DISABLE;
	HS_2_DISABLE;
	LS_2_DISABLE;
	HS_3_DISABLE;
	LS_3_DISABLE;

	SET_BIT(TIM8->EGR, TIM_EGR_COMG); // update immediately
	sector = 0;
	loadedSector = 0;
	motorSTATE = MOTOR_STOPPED;
	elecRevCount = 0;
}


int currentduty = 10;


void loadNextSectorSixStep(){
	// TODO: maybe we want to advance earlier than the current sektor? ( switch(step+1) )
	switch(sector){		// DONT use state++, as this requires a read of state and thus is slower!
	case 0: loadSectorSixStep1(); break;	// After motor has been stopped
	case 1: loadSectorSixStep2(); break;
	case 2: loadSectorSixStep3(); break;
	case 3: loadSectorSixStep4(); break;
	case 4: loadSectorSixStep5(); break;
	case 5: loadSectorSixStep6(); break;
	case 6: loadSectorSixStep1(); break;
	//case 7: loadSectorSixStep2(); break; // Only in case of advance before a step (see above (step + 1)
	default: break;
	}
	//SET_BIT(TIM8->EGR, TIM_EGR_COMG); // update
}

void loadNextSectorSinusoid(){
	// TODO: maybe we want to advance earlier than the current sektor? ( switch(step+1) )
	switch(sector){		// DONT use state++, as this requires a read of state and thus is slower!
	case 0: loadSectorSinusoid1(); break;	// After motor has been stopped
	case 1: loadSectorSinusoid2(); break;
	case 2: loadSectorSinusoid3(); break;
	case 3: loadSectorSinusoid4(); break;
	case 4: loadSectorSinusoid5(); break;
	case 5: loadSectorSinusoid6(); break;
	case 6: loadSectorSinusoid1(); break;
	//case 7: loadSectorSinusoid2(); break; // Only in case of advance before a step (see above (step + 1)
	default: break;
	}
	//SET_BIT(TIM8->EGR, TIM_EGR_COMG); // update
}

uint32_t DWT_Delay_Init(void)
{
	/* Disable TRC */
	CoreDebug->DEMCR &= ~CoreDebug_DEMCR_TRCENA_Msk; // ~0x01000000;
	/* Enable TRC */
	CoreDebug->DEMCR |=  CoreDebug_DEMCR_TRCENA_Msk; // 0x01000000;

	/* Disable clock cycle counter */
	DWT->CTRL &= ~DWT_CTRL_CYCCNTENA_Msk; //~0x00000001;
	/* Enable  clock cycle counter */
	DWT->CTRL |=  DWT_CTRL_CYCCNTENA_Msk; //0x00000001;

	/* Reset the clock cycle counter value */
	DWT->CYCCNT = 0;

	/* 3 NO OPERATION instructions */
	__ASM volatile ("NOP");
	__ASM volatile ("NOP");
	__ASM volatile ("NOP");

	/* Check if clock cycle counter has started */
	if(DWT->CYCCNT)
	{
		return 0; /*clock cycle counter started*/
	}
	else
	{
		return 1; /*clock cycle counter not started*/
	}
}

// APB2 (85Mhz)
// Longest interval for one hall sensor cycle (falling edge to falling edge) ~25ms (6ms for turning very slow - 25ms just stopped)
// 25 ms / 65535 = T -> freq = 1/T -> temp_presc = clock APB2 / freq = 32 ->
// There are 3 hallsensors -> prec = temp_presc / 3  = ...
/*
 * Different approach:
 * Least resolution needed for sector signal (signal of any 3 hall signals) -> 120 per sector (60° of electr. rotation)
 * See Paperwork "Hallsensors 15.04.2024 - 19:55"
 */

#define APB_FREQ_MHZ 85

#define TIM1_PRESC 200	// - 1 is subtracted later!

#define NS_TO_TIM1_CLOCK(ns) (APB_FREQ_MHZ * ns 		/ TIM1_PRESC / 1000)
#define US_TO_TIM1_CLOCK(us) (APB_FREQ_MHZ * us 		/ TIM1_PRESC)
#define MS_TO_TIM1_CLOCK(ms) (APB_FREQ_MHZ * ms * 1000 	/ TIM1_PRESC)

#define TIM1_CLOCKS_TO_US(clks) (clks * TIM1_PRESC / APB_FREQ_MHZ)

/*
 * Minimum pedal revolutions per minute that the motor runs at.
 */
#define MIN_RUN_PEDAL_RPM 10

// SINGLE HALL PULSES PER PEDAL REVOLUTION
#define SING_H_PULS_PEDAL_REV 300

// COMBINED HALL EDGES PER ELEC ROATION
#define COMB_H_EDGES_ELEC_ROT 6

// Minimum hall frequency (Hz) (all 3 sensors combined rising and falling edge) which is considered showing a running motor
#define COMB_HALL_MIN_FREQ  ( (uint32_t)COMB_H_EDGES_ELEC_ROT * SING_H_PULS_PEDAL_REV * MIN_RUN_PEDAL_RPM / 60)

// 1400 tics at 3.3ms (COMB_HALL_MIN_FREQ)
//#define MAX_TIMER_1_TICS_SECTOR_RUNNING (COMB_HALL_MIN_FREQ / COMB_H_EDGES_ELEC_ROT) // (Max timer 1 tics/s for one sector and the motor to be defined as running)
#define MAX_TIMER_1_TICS_SECTOR_RUNNING 4096 // Max value for a running motor (for sinusoidal generation)

// Micros interval of COMB_HALL_MIN_FREQ (minimum speed of motor -> minimum frequency -> is maximum interval)
#define COMBINED_HALL_EDGES_MAX_INTERVAL_MICROS ( 1000uL * 1000 / (COMB_HALL_MIN_FREQ) )



void startMotor(){
	// Detect current sector
	sector = determineSector() - 1;	// -1 as loading and update would load the "next" sector

	if(sector > 6){	// error from determineSector() - underflow of datatype (~255)
		//TODO Error:
		while(1);
	}

	if(sector == 0)
		sector = 1;

	// Load next sector
	if(sinusoidalMode)
		loadNextSectorSinusoid();
	else
		loadNextSectorSixStep();


	//setDuty(20); // TODO: add from torc sensor AND push function

	// Set status
	firstSectorOnStart = sector;
	motorSTATE = MOTOR_STARTED;
	motorTimeoutCount = 0;

	// Energize phases
	SET_BIT(TIM8->EGR, TIM_EGR_COMG); // update to activate output

	// TODO: Reset variables ?
}

/* Needed for push function, as an accidental release of the push button should not turn off the motor immediately.
 * It should fade out over 1 revolution or time x
 */
void stopMotor(){
	motorSTATE = MOTOR_FADING;
}


uint32_t rampTimeMillis = 0;

// DONT GO BELOW CONTROLLER_TASKS_INTERVAL or we get divide by zero!
#define RAMP_UP_TIME_MILLIS 200
#define RAMP_UP_DUTY_STEP (100 / (RAMP_UP_TIME_MILLIS / CONTROLLER_TASKS_INTERVAL))

byte rampUpDuty = 0;

// TODO: clean up those values with "regulationFactor" - Have proper seperated variables
byte commandedDuty = 0; // From torc sensor and formula/setting, limited by max current / temperature of FETs


void rampUp(){
	rampUpDuty += RAMP_UP_DUTY_STEP;
	setDuty(rampUpDuty);
}

void rampDown(){
	//setDuty((uint32_t) (currentDuty * 0.9));
}


void controllerTasks(){
	// ############# Request state ###############

	setDuty(100);	//TODO: debug set back to rampup


	updateDuty();

	// Advanceangle calculation
	advanceTime = 200; //us
	advanceAngle = US_TO_TIM1_CLOCK(advanceTime) * 64 / lastHallIntervalTics;
}





void sinusoidalHallInterrupt(){
	//	TEST_HALL_ON;

	// Hall external triggered
	if(TIM1->SR & TIM_SR_CC1IF){
		TIM1->SR = ~TIM_SR_CC1IF;
		sector = loadedSector;		// Output is loaded on cnt 0 on timer 1

		if(sector == 1){		//TODO: debug remove
			TEST_HALL_ON;
			delayMicroseconds(1);
			TEST_HALL_OFF;
		}

		lastHallIntervalTics = TIM1->CCR1;		// read Capture register (resets interrupt flag) and TODO: calculate interval

		if(motorSTATE > MOTOR_STOPPED){
			TEST_HALL_ON
			//ITM_SendChar(elecRevCount);
			if(sector == 0)
				elecRevCount++;	//TODO: overflow occurs!
			loadNextSectorSinusoid();
		}
	}

	// COM timer 8 update trigger
	if(TIM1->SR & TIM_SR_CC2IF){
		//TEST_HALL_ON
		TIM1->SR = ~TIM_SR_CC2IF;
	}

	// Interrupt flag is reset by reading CCR1 capture register!
	TEST_HALL_OFF;
}

void sixStepHallInterrupt(){


	// Hall external triggered
	if(TIM1->SR & TIM_SR_CC1IF){
		if(sector == 1){		//TODO: debug remove
			TEST_HALL_ON;
			delayMicroseconds(1);
			TEST_HALL_OFF;
		}
		lastHallIntervalTics = TIM1->CCR1;	// read Capture register (resets interrupt flag) and TODO: calculate interval

		// preload new CCR2 value (delay for COM -> timer 8) -> It does actually delay +(50-75ns)!
		WRITE_REG(TIM1->CCR2, (uint32_t) (lastHallIntervalTics - lastHallIntervalTics * advanceAngle / PSEUDO_SECTOR_ANGLE )); 	// Delay value for PWM change trigger TODO: shouldnt it be "time"-advance??

		//		WRITE_REG(TIM1->CCR2, (uint32_t) (lastHallIntervalTics * 33 / 100));	//TODO: Debug replace with US_TO_TIM1_CLOCK below
		//		WRITE_REG(TIM1->CCR2, (uint32_t) (lastHallIntervalTics - US_TO_TIM1_CLOCK(100)) );	// Delay value for PWM change trigger


		if(motorSTATE > MOTOR_STOPPED){
			//TEST_HALL_ON
			elecRevCount++;
			loadNextSectorSixStep();
		}
		//TODO: if(running fast)
	}

	// COM timer 8 update trigger
	if(TIM1->SR & TIM_SR_CC2IF){
		//TEST_HALL_ON
		TIM1->SR = ~TIM_SR_CC2IF;
		sector = loadedSector;
	}


	// Interrupt flag is reset by reading CCR1 capture register!
}



/* Interrupt is called 240-360ns after edge on input without filter 07.04.2024 - 18:42
 * Interrupt is called ~1.4us after edge on input with filter 07.04.2024 - 22:50
 * Takes 1.4 us to execute 18.04.2024 - 18:42
 * 		 3.1 us  21.04.2024 - 16:37
 * Execution every 833us at 80 rpm pedals => 0.17% processing load
 *
 * Cadence of 120U/min is 600 Hallpulses/s on one hall sensor (1 hallsensor - 1.666ms - electrical rotation).
 * 555.55us between each sector signal/change comes out at 0.25% processing load
 *
 * Magnetfield leads 60-120 degrees for optimal performance in hall configuration. https://de.mathworks.com/discovery/bldc-motor-control.html
 *
 * TODO: do not determine Sectors when rotating fast, since there will be no errors from turning backwards.
 */
void TIM1_CC_IRQHandler(){
	__disable_irq();
	//	TEST_HALL_ON;

	if(sinusoidalMode)
		sinusoidalHallInterrupt();
	else
		sixStepHallInterrupt();

	// Timer 1 overflow - Only when no hall edges detected
	if(TIM1->SR & TIM_SR_CC3IF){
		//TEST_HALL_ON;
		TIM1->SR = ~TIM_SR_CC3IF;
		motorTimeoutCount++;
		delayMicroseconds(8);	//TODO: untoggle comment


		if(motorTimeoutCount >= 6)	// Motor turned to slow, or did not start turning much at all
			switchToSectorOFF();
	}



	TEST_HALL_OFF;
	__enable_irq();
}

void switchToSinusoidalMode(){
	/* TODO: implement switching
	 * Sinusoidal table fur jede mode?
	 */

	// If already activated, return.
	if(sinusoidalMode)
		return;

	__disable_irq(); // TIM1_CC_IRQHandler may run into sixStepHallInterrupt() and mess up the PWM output configuration
	//SET_BIT(TIM8->CR1, TIM_CR1_UDIS);
	//TODO: Läuft falsch. Ein oder zwei Kanäle eingefroren (4 Ausgänge)

	//	// Enable is included in PWM
	SIX_STEP_PRELOAD_HS_1_PWM;
	SIX_STEP_PRELOAD_HS_2_PWM;
	SIX_STEP_PRELOAD_HS_3_PWM;

	SIX_STEP_PRELOAD_LS_1_PWM;
	SIX_STEP_PRELOAD_LS_2_PWM;
	SIX_STEP_PRELOAD_LS_3_PWM;
	//delayMicroseconds(200);					// A delay is needed, otherwise outputs might not work (are frozen) https://community.st.com/t5/stm32-mcus-products/generating-pwm-modulated-sinewaves-fails-sometimes-on-some/m-p/720279#M260860



	sinusoidalMode = true;
	TIM8->DIER |= TIM_DIER_CC4IE;			// Enable interrupts (from ADC trigger) for kind of sine wave generation
	WRITE_REG(TIM1->CCR2, (uint32_t) 0);	// Update of timer 8 values immediately (through trigger line)
	//	CLEAR_BIT(TIM8->CR1, TIM_CR1_UDIS);
	__enable_irq();

}

void switchToSixStepMode(){

	sinusoidalMode = false;
	TIM8->DIER = ~TIM_DIER_CC4IE;	// Disable interrupts (from ADC trigger) for kind of sine wave generation
}



uint16_t lastDuty = 0;
uint16_t lastMotorAngle = 0;
uint16_t lastRawDeltaAngle = 0;
uint16_t lastHTics = 0;

uint16_t dutys[400] = {0};
int indexs = 0;

int debugCount = 0;

// calculate new PWM duty to "build" a sinewave.+
// 1.5us duration - 15.05.2024 - 22:33
void TIM8_CC_IRQHandler(){
	TEST_TIME_TIM8CC_ON;
	//	__disable_irq();

	// Calculation of PWM values on every ADC (current measure) trigger / pwm pulse
	if(TIM8->SR & TIM_SR_CC4IF){
		TEST_TIME_TIM8CC_ON;
		TIM8->SR = ~TIM_SR_CC4IF;

		motorAngle = 64 * TIM1->CNT / lastHallIntervalTics;
		if(motorAngle > 64)
			motorAngle = 64;
		motorAngle += angleTable[sector - 1];

		// Discard everything, as counter timer 1 has been reset, and values are not valid. ( Validated here, sinc there is a bit of a lag until the flag is set)
		if(TIM1->SR & TIM_SR_CC1IF)
			return;

		motorAngle += advanceAngle;

		if(motorAngle >= PSEUDO_ROTATION_ANGLE)
			motorAngle -= PSEUDO_ROTATION_ANGLE;

		//		if(motorAngle < 6 && sector == 6){
		//			__ASM volatile ("NOP");
		//		}

		// TODO: uint8 differs to uint16 at the others???
		WRITE_REG(TIM8->CCR3, DUTY_TO_TICS( (uint8_t)( ((uint16_t)outputDuty * (uint16_t)SinusoidalWaveTable[motorAngle]) >> 8 )));

		if (motorAngle < 128){
			WRITE_REG(TIM8->CCR2, DUTY_TO_TICS((uint16_t)(((uint16_t)outputDuty * (uint16_t)SinusoidalWaveTable[motorAngle + 256]) >> 8)));
		}
		else {
			WRITE_REG(TIM8->CCR2, DUTY_TO_TICS((uint16_t)(((uint16_t)outputDuty * (uint16_t)SinusoidalWaveTable[motorAngle - 128]) >> 8)));
		}
		if (motorAngle >= 256){
			WRITE_REG(TIM8->CCR1, DUTY_TO_TICS( (uint16_t)(((uint16_t)outputDuty * (uint16_t)SinusoidalWaveTable[motorAngle -256]) >> 8)));
		}
		else {
			WRITE_REG(TIM8->CCR1, DUTY_TO_TICS( (uint16_t)(((uint16_t)outputDuty * (uint16_t)SinusoidalWaveTable[motorAngle + 128]) >> 8)));
		}

		// ***************************************************************
		// ****** ATTENTION! TIM CH 1 is PH 3 and CH 3 is PH 1 !!!  ******
		// ***************************************************************
		//		WRITE_REG(TIM8->CCR3, DUTY_TO_TICS(dutyCycleA));
		//		WRITE_REG(TIM8->CCR2, DUTY_TO_TICS(dutyCycleB));
		//		WRITE_REG(TIM8->CCR1, DUTY_TO_TICS(dutyCycleC));
		//
		//		if(millis() > 7000 && lastDuty != 0 && /*lastDuty != 95 &&*/ abs((int16_t)lastDuty - (int16_t)dutyCycleA) > 2){
		//			TEST_WAVEJUMP_ON
		//			for(int i = 0; i < 10; i++)
		//				__ASM volatile ("NOP");
		//			TEST_WAVEJUMP_OFF
		//		}
		//
		//		lastDuty = dutyCycleA;
	}

	//	__enable_irq();
	TEST_TIME_TIM8CC_OFF;
}



/*
 * byte index starts from 0!
 */
int hallStates[6][3] ={
		{1,0,0},	// sector 1 (hall 1, hall 2, ...)
		{1,1,0},	// sector 2
		{1,1,1},	// ...
		{0,1,1},
		{0,0,1},
		{0,0,0}
};

/*
 * MEASURE TIME OUTSIDE. "If" conditions confuses when forgotten to address!
 * 1.5us 21.04.2024 - 16:24
 */
int determineSector(){
	int idr_C = GPIOC->IDR;

	int hallState_1 = (idr_C & GPIO_PIN_0) > 0 ? 1 : 0;
	int hallState_2 = (idr_C & GPIO_PIN_1) > 0 ? 1 : 0;
	int hallState_3 = (idr_C & GPIO_PIN_2) > 0 ? 1 : 0;

	for(int i = 0; i < 6; i++){
		if(		hallStates[i][0] == hallState_1 &&
				hallStates[i][1] == hallState_2 &&
				hallStates[i][2] == hallState_3){
			__ASM volatile ("NOP");
			return i+1;	// byte index starts from 0!
		}
	}
	return 0;	// TODO: Error on hall sensors
}


/*
 * When timer 1 overflowed, motor is not turning, or very slow (not really).
 */
void TK_TIM1_UP_IRQ(){
	TIM1->SR = ~TIM_SR_UIF;
	//	lastHallIntervalTics = 0;
	//	motorState = MOTOR_STOPPED;
}


/**
 * Timer for hallsensors (Rotor speed for FOC later)
 * Delayed execution of interrupt because of filter!!!
 * APB2 85MHz
 */
void initHallTim1AndDriverTim8(){
	__disable_irq();

	__HAL_RCC_TIM1_CLK_ENABLE();

	SET_BIT(  TIM1->CR2, 	TIM_CR2_TI1S);		// config all 3 hall signals (PC0-2) via XOR to timer 1
	WRITE_REG(TIM1->ARR, 	0xFFFF); 			// Max value (is reset by input signal)
	WRITE_REG(TIM1->CCR3,	0x1A90);			// Timeout hall input (update does not work as it fires interrupt on timer reset) -> 0x1A90 = 16ms
	WRITE_REG(TIM1->PSC, 	TIM1_PRESC - 1);	// Mind the 1 that is added in the uC !

	SET_BIT(  TIM1->CCMR1,  TIM_CCMR1_CC1S_0);// |
	//							TIM_CCMR1_CC2S_0);	// channel is configured as input, tim_ic1 is mapped on tim_ti1, etc...
	//	SET_BIT(  TIM1->CCMR2,  TIM_CCMR2_CC3S_0);	// As above
	SET_BIT(  TIM1->CCMR1,	TIM_CCMR1_OC2M_2 |
			TIM_CCMR1_OC2M_1 |
			TIM_CCMR1_OC2M_0); 	// PWM mode 2 - In upcounting, channel 1 is inactive as long as TIMx_CNT<TIMx_CCR1 else active.
	SET_BIT(  TIM1->CCMR2,	TIM_CCMR2_OC3M_1 |
			TIM_CCMR2_OC3M_0);

	SET_BIT(  TIM1->CR1,  	TIM_CR1_CKD_0);		// Prescaler (clk/2) for filter and deadtime
	SET_BIT(  TIM1->CCMR1, 	TIM_CCMR1_IC1F_1); 	// filter if needed

	SET_BIT(  TIM1->CR2, 	TIM_CR2_MMS_2 |
			TIM_CR2_MMS_0);		// tim_oc2refc signal is used as trigger output (tim_trgo -> itr0 for timer 8)

	SET_BIT(  TIM1->DIER,   TIM_DIER_CC1IE); 	// enable interrupt on hall signal (is XOR with 2 and 3
	SET_BIT(  TIM1->DIER,   TIM_DIER_CC2IE); 	// enable interrupt on COM trigger for timer 8 (activates the preloaded configuration)
	SET_BIT(  TIM1->DIER,   TIM_DIER_CC3IE);	// Enable Interrupt for timeout of hall sensors (motor stopped)
	SET_BIT(  TIM1->DIER,   TIM_DIER_UIE); 		// enable interrupt on counter overflow

	SET_BIT(  TIM1->CCER,   TIM_CCER_CC1E |
			TIM_CCER_CC1NP |
			TIM_CCER_CC1P);		// capture enable and Rising and falling edge

	SET_BIT(  TIM1->SMCR, TIM_SMCR_SMS_2); 		// reset counter and prescaler on trigger input
	SET_BIT(  TIM1->SMCR, TIM_SMCR_TS_2);		// trigger on tim_ti1 -> tim_ti1f_ed


	SET_BIT(  TIM1->CR1,    TIM_CR1_CEN); 		// enable counter timer 1
	NVIC_EnableIRQ(TIM1_CC_IRQn);				// enable interrupt

	//	OC1M is preploaded
	// slave mod reset input bei tim_ti1f_ed
	// capture/compare channel 1 is configured in capture mode, capture signal is tim_trc


	initTimer8PWM();

	startTimer8();

	__enable_irq();

	//SET_BIT(TIM1->EGR, TIM_EGR_CC1G);	//force interrupt
}


//void jumpStartDriver(){
//	GPIO_InitTypeDef my_GPIO_InitStruct = {0};
//	// PC6-8
//	// PC10-12
//
//	my_GPIO_InitStruct.Pin = 	GPIO_PIN_6 |
//								GPIO_PIN_7 |
//								GPIO_PIN_8 |
//								GPIO_PIN_10 |
//								GPIO_PIN_11 |
//								GPIO_PIN_12;
//	my_GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
//	my_GPIO_InitStruct.Pull = GPIO_NOPULL;
//	HAL_GPIO_Init(GPIOC, &my_GPIO_InitStruct);
//
//	GPIOC->BSRR |= my_GPIO_InitStruct.Pin;
//	delayMicroseconds(2);
//	GPIOC->BRR |= my_GPIO_InitStruct.Pin;
//
//
//}


__weak void printBin(byte b){};

void readAndPrintDrvRegisterBin(byte address){
	byte data[2] = {0};
	readDataSPIDriver(address, data);

	printBin(address);
	printBin(data[0]);
	printBin(data[1]);
}







void gpioInitMisc(){
	GPIO_InitTypeDef my_GPIO_InitStruct = {0};

	// Enable Throttle pin (And other debug testsignals)
	my_GPIO_InitStruct.Pin = GPIO_PIN_3;
	my_GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	my_GPIO_InitStruct.Pull = GPIO_PULLUP;
	HAL_GPIO_Init(GPIOC, &my_GPIO_InitStruct);

	my_GPIO_InitStruct.Pin = GPIO_PIN_11 | GPIO_PIN_12;
	my_GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
	my_GPIO_InitStruct.Pull = GPIO_PULLUP;
	HAL_GPIO_Init(GPIOA, &my_GPIO_InitStruct);

	// Temperature FETs
	my_GPIO_InitStruct.Pin = GPIO_PIN_7;
	my_GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
	my_GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOA, &my_GPIO_InitStruct);

	// Csens A-C (C is different!)
	my_GPIO_InitStruct.Pin = GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_6;
	my_GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
	my_GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOA, &my_GPIO_InitStruct);

	// Battery voltage
	my_GPIO_InitStruct.Pin = GPIO_PIN_4;
	my_GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
	my_GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOC, &my_GPIO_InitStruct);

	// Button sens voltage
	my_GPIO_InitStruct.Pin = GPIO_PIN_5;
	my_GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
	my_GPIO_InitStruct.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOA, &my_GPIO_InitStruct);

	// TIM 3 Inputs (Pedal rot - PB5 / Torc PWM - PB1)
	my_GPIO_InitStruct.Pin = GPIO_PIN_5 | GPIO_PIN_1;
	my_GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
	my_GPIO_InitStruct.Pull = GPIO_PULLUP;
	HAL_GPIO_Init(GPIOB, &my_GPIO_InitStruct);
	GPIOB->AFR[0] |= (	GPIO_AFRL_AFSEL1_1 |  	// AF2 (all)
			GPIO_AFRL_AFSEL5_1);

	/*Configure Torc com (PA9 TX - PA10 RX) (USART1) */
	my_GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_10;
	my_GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
	my_GPIO_InitStruct.Pull = GPIO_NOPULL;
	my_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	my_GPIO_InitStruct.Alternate = GPIO_AF7_USART3;
	HAL_GPIO_Init(GPIOA, &my_GPIO_InitStruct);


	//	// BEGIN EEPROM I2C_3
	//	my_GPIO_InitStruct.Pin = GPIO_PIN_9;
	//	my_GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
	//	my_GPIO_InitStruct.Pull = GPIO_NOPULL;
	//	my_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	//	my_GPIO_InitStruct.Alternate = GPIO_AF8_I2C3;	//SDA
	//	HAL_GPIO_Init(GPIOC, &my_GPIO_InitStruct);
	//
	//	// Continued EEPROM I2C_3
	//	my_GPIO_InitStruct.Pin = GPIO_PIN_8;
	//	my_GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
	//	my_GPIO_InitStruct.Pull = GPIO_NOPULL;
	//	my_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	//	my_GPIO_InitStruct.Alternate = GPIO_AF2_I2C3;	//SCL
	//	HAL_GPIO_Init(GPIOA, &my_GPIO_InitStruct);
	//	// END EEPROM I2C_3

	// User button TODO: DEBUG remove
	my_GPIO_InitStruct.Pin = GPIO_PIN_13;
	my_GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
	my_GPIO_InitStruct.Pull = GPIO_NOPULL;
	my_GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
	my_GPIO_InitStruct.Alternate = 0;
	HAL_GPIO_Init(GPIOC, &my_GPIO_InitStruct);

}

uint32_t lastRequestMillis = 0;

void userButtonFunc(){
	__disable_irq();

	if(motorSTATE == MOTOR_STOPPED || motorSTATE == MOTOR_FADING)
		motorRequest = MOTOR_STARTED;
	else
		motorRequest = MOTOR_STOPPED;

	__enable_irq();
}


#define TICS_PER_TIMER_OVERFLOW 0xFFFF
#define TIM16_PRESC 1



void setup(){ /* Set Interrupt Group Priority */
	HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);

	/* Use SysTick as time base source and configure 1ms tick (default clock after Reset is HSI) */
	HAL_InitTick(TICK_INT_PRIORITY);

	__HAL_RCC_SYSCFG_CLK_ENABLE();
	__HAL_RCC_PWR_CLK_ENABLE();

	/** Disable the internal Pull-Up in Dead Battery pins of UCPD peripheral*/
	HAL_PWREx_DisableUCPDDeadBattery();

	RCC_OscInitTypeDef RCC_OscInitStruct = {0};
	RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

	/** Configure the main internal regulator output voltage
	 */
	HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);

	/** Initializes the RCC Oscillators according to the specified parameters
	 * in the RCC_OscInitTypeDef structure.
	 */
	RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
	RCC_OscInitStruct.HSEState = RCC_HSE_ON;
	RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
	RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
	RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV6;
	RCC_OscInitStruct.PLL.PLLN = 85;
	RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
	RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
	RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
	if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
	{
		Error_Handler();
	}

	/** Initializes the CPU, AHB and APB buses clocks
	 */
	RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
			|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
	RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
	RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
	RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
	RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV4;

	if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
	{
		Error_Handler();
	}

	GPIO_InitTypeDef GPIO_InitStruct = {0};

	/* GPIO Ports Clock Enable */
	__HAL_RCC_GPIOA_CLK_ENABLE();
	__HAL_RCC_GPIOB_CLK_ENABLE();
	__HAL_RCC_GPIOC_CLK_ENABLE();
	__HAL_RCC_GPIOD_CLK_ENABLE();
	__HAL_RCC_GPIOE_CLK_ENABLE();
	__HAL_RCC_GPIOF_CLK_ENABLE();


	__HAL_RCC_TIM16_CLK_ENABLE();
	TIM16->PSC = TIM16_PRESC - 1; 				// true prescaler 85 is value (85-1)
	TIM16->CR1 |= TIM_CR1_CEN | TIM_CR1_URS;					// Enable counter
	TIM16->ARR = TICS_PER_TIMER_OVERFLOW - 1;
	TIM16->DIER |= TIM_DIER_UIE;				// Comp 1 interrupt enable

	//NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn);

	DWT_Delay_Init();



	// activate clock to access registers
	SET_BIT(RCC->APB1ENR1, RCC_APB1ENR1_USART3EN);




	// Init all interrupts (102 of them in STM32) at a medium priority (default is all high) (prio 0-15 / 0 is highest)
	for(int i = 0; i < 102; i++){
		NVIC_SetPriority((IRQn_Type) i, 6);
	}


	NVIC_SetPriority(TIM8_UP_IRQn, 1);		// set higher priority
	NVIC_SetPriority(TIM8_CC_IRQn, 1);		// set higher priority
	NVIC_SetPriority(TIM1_CC_IRQn, 2);		// set higher priority


	GPIO_InitTypeDef my_GPIO_InitStruct = {0};

	// **************** PWM *********************
	// PC6-8 (High side)
	// PC10-12 (Low side)
	my_GPIO_InitStruct.Pin = 	GPIO_PIN_6 |
			GPIO_PIN_7 |
			GPIO_PIN_8 |
			GPIO_PIN_9 |	//TODO: debug remove test for tim 8 CCR4
			GPIO_PIN_10 |
			GPIO_PIN_11 |
			GPIO_PIN_12;

	my_GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
	my_GPIO_InitStruct.Pull = GPIO_PULLDOWN;	// Pulldown, to never create an opportunity of shootthrough. Actually not used by timer, directly, but may be used by software (intentionally or accidently).
	my_GPIO_InitStruct.Speed = GPIO_SPEED_LOW; // Low = 270V/us / fast = 570V/us
	HAL_GPIO_Init(GPIOC, &my_GPIO_InitStruct);

	// Alternative function HIGH register
	GPIOC->AFR[0] |= (	GPIO_AFRL_AFSEL6_2 |  	// AF4 (all)
			GPIO_AFRL_AFSEL7_2);
	// Alternative function LOW register
	GPIOC->AFR[1] |= (	GPIO_AFRH_AFSEL8_2 |	// AF4 (all)
			GPIO_AFRH_AFSEL9_2 | 	//TODO: Debug remove
			GPIO_AFRH_AFSEL10_2 |
			GPIO_AFRH_AFSEL11_2 |
			GPIO_AFRH_AFSEL12_2);


	// Hallsensors (Tim1 capture)
	my_GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2;
	my_GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
	my_GPIO_InitStruct.Pull = GPIO_PULLUP;	// TODO: The hall sensor does not put out 5V on its own????
	HAL_GPIO_Init(GPIOC, &my_GPIO_InitStruct);
	GPIOC->AFR[0] |= (	GPIO_AFRL_AFSEL0_1 |  	// AF2 (all)
			GPIO_AFRL_AFSEL1_1 |
			GPIO_AFRL_AFSEL2_1);



	delay(100);

	initHallTim1AndDriverTim8();

	NVIC_EnableIRQ(TIM8_TRG_COM_IRQn);
	NVIC_EnableIRQ(TIM8_CC_IRQn);	// For sinusoidal generation
	NVIC_EnableIRQ(TIM8_UP_IRQn);	// For sinusoidal generation


	NVIC_EnableIRQ(TIM1_TRG_COM_TIM17_IRQn);


	TIM8->DIER |= TIM_DIER_COMIE;	// Enable interrupt on COM //TODO: put in init timer8 !


}

	uint32_t buttonTimePressed = 0;

	bool buttonOnce = false;

	uint32_t lastControllerTasksMicros = 0;

	// Loop interval 2.5us (no CAN connected) 12.04.2024 09:10
	void loop(){

		unsigned long now = micros();
		if(now - lastControllerTasksMicros >= CONTROLLER_TASKS_INTERVAL * 1000){
			controllerTasks(); // changes only TIM8->CCR1,2,3

			lastControllerTasksMicros = now;
		}

		if(micros() > 1000*1000 && buttonOnce == false){
			// ATTENTION: has chopped up motorangle after beeing executed over and over again
			loadNextSectorSixStep();


			//setDuty(20); // TODO: add from torc sensor AND push function

			// Set status
			motorSTATE = MOTOR_STARTED;

			// Energize phases
			SET_BIT(TIM8->EGR, TIM_EGR_COMG); // update to activate output
			buttonOnce = true;
		}


		if(millis() > 2000){
			//motorRunning = true;
			//setDuty(20);
			switchToSinusoidalMode();
		}
	}