// PicoBasic und TestLab

#include "debug.h"

uint16_t code[256] = {0x4900};
uint8_t ram[512];
//uint8_t ram2[512];
uint8_t a = 0, b = 0, c = 0, d = 0;
uint8_t pw1=0;
uint8_t pw2=0;
uint16_t ad;
uint16_t rx = 1000;
char ch;
uint16_t adr = 0;
uint16_t pdat = 0;
uint16_t return_adr[10];
uint16_t return_nr = 0;
uint16_t kom = 0;
uint16_t dat = 0;
uint32_t warten, weiter, millis;
u8 flash = 0;
u16 flash_adr = 0;
u8 ram_adr = 0;
u8 opv = 1;
u32 akk1=0, akk2=0, phase1=0, phase2=0;
u8 IRQ_on = 0;
u32 dF = 1;
u8 gen1 = 0;
u8 gen2 = 0;
volatile uint32_t interrupt_count = 0;
volatile uint32_t pulse_count = 0;
volatile uint32_t pulse_last = 0;
volatile uint32_t pulse_high = 0;

GPIO_InitTypeDef GPIO_InitStructure={0};
const uint8_t sin_table[256] = 
{128,  131,  134,  137,  140,  143,  146,  149,  152,  156,  159,  162,  165,  168,  171,  173,  
176,  179,  182,  185,  188,  190,  193,  196,  199,  201,  204,  206,  209,  211,  213,  216,  
218,  220,  222,  224,  226,  228,  230,  232,  234,  235,  237,  239,  240,  242,  243,  244,  
246,  247,  248,  249,  250,  251,  251,  252,  253,  253,  254,  254,  255,  255,  255,  255,  
255,  255,  255,  255,  255,  254,  254,  253,  253,  252,  251,  251,  250,  249,  248,  247,  
246,  244,  243,  242,  240,  239,  237,  235,  234,  232,  230,  228,  226,  224,  222,  220,  
218,  216,  213,  211,  209,  206,  204,  201,  199,  196,  193,  191,  188,  185,  182,  179,  
176,  174,  171,  168,  165,  162,  159,  156,  152,  149,  146,  143,  140,  137,  134,  131,  
128,  124,  121,  118,  115,  112,  109,  106,  103,  100,  96,  93,  90,  87,  84,  82,  79,  
76,  73,  70,  67,  65,  62,  59,  57,  54,  51,  49,  46,  44,  42,  39,  37,  35,  33,  31,  
29,  27,  25,  23,  21,  20,  18,  16,  15,  13,  12,  11,  9,  8,  7,  6,  5,  4,  4,  3,  2,  
2,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  2,  2,  3,  4,  4,  5,  6,  7,  8,  9,  
11,  12,  13,  15,  16,  18,  20,  21,  23,  25,  27,  29,  31,  33,  35,  37,  39,  42,  44,  
46,  49,  51,  54,  56,  59,  62,  64,  67,  70,  73,  76,  79,  81,  84,  87,  90,  93,  96,  
99,  103,  106,  109,  112,  115,  118,  121,  124, }; 

/*********************************************************************
 * @fn      TIM1_UP_IRQHandler
 *
 * @brief   This function handles TIM1 UP exception.
 *
 *
 * @return  none
 */
void TIM1_UP_IRQHandler(void) __attribute__((interrupt("WCH-Interrupt-fast")));
void TIM1_UP_IRQHandler(void)
{
    if(TIM_GetITStatus(TIM1, TIM_IT_Update)==SET)
    {
      if(phase1 > 0){
         akk1 += phase1;
         TIM1->CH1CVR = sin_table[akk1 >> 24];
      }
      if(phase2 > 0){
        akk2 += phase2;          
        TIM2->CH2CVR = sin_table[akk2 >> 24];    
      }
   }
    TIM_ClearITPendingBit( TIM1, TIM_IT_Update );
}

void TIM1_PWMOut_Init(u16 arr, u16 psc, u16 ccp)
{
    GPIO_InitTypeDef GPIO_InitStructure={0};
    TIM_OCInitTypeDef TIM_OCInitStructure={0};
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure={0};

    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOD | RCC_APB2Periph_TIM1 , ENABLE );
    RCC_APB1PeriphClockCmd( RCC_APB1Periph_TIM2, ENABLE );

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 ;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_Init( GPIOD, &GPIO_InitStructure );

    TIM_TimeBaseInitStructure.TIM_Period = arr;
    TIM_TimeBaseInitStructure.TIM_Prescaler = psc;
    TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit( TIM1, &TIM_TimeBaseInitStructure);

    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = ccp;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
    TIM_OC1Init( TIM1, &TIM_OCInitStructure );

    TIM_CtrlPWMOutputs(TIM1, ENABLE );
    TIM_OC1PreloadConfig( TIM1, TIM_OCPreload_Disable );
    TIM_ARRPreloadConfig( TIM1, ENABLE );
    TIM_Cmd( TIM1, ENABLE );
}

void TIM2_PWMOut_Init(u16 arr, u16 psc, u16 ccp)
{
    GPIO_InitTypeDef GPIO_InitStructure={0};
    TIM_OCInitTypeDef TIM_OCInitStructure={0};
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure={0};

    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOD | RCC_APB2Periph_TIM1 , ENABLE );
    RCC_APB1PeriphClockCmd( RCC_APB1Periph_TIM2, ENABLE );

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3 ;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_Init( GPIOD, &GPIO_InitStructure );

    TIM_TimeBaseInitStructure.TIM_Period = arr;
    TIM_TimeBaseInitStructure.TIM_Prescaler = psc;
    TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit( TIM2, &TIM_TimeBaseInitStructure);

    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = ccp;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
    TIM_OC2Init( TIM2, &TIM_OCInitStructure );

    TIM_CtrlPWMOutputs(TIM2, ENABLE );
    TIM_OC2PreloadConfig( TIM2, TIM_OCPreload_Disable );
    TIM_ARRPreloadConfig( TIM2, ENABLE );
    TIM_Cmd( TIM2, ENABLE );
}

void Tim1_int_Init( u16 arr, u16 psc)
{
    NVIC_InitTypeDef NVIC_InitStructure={0};
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure={0};
    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOD | RCC_APB2Periph_TIM1 , ENABLE );

    TIM_TimeBaseInitStructure.TIM_Period = arr;
    TIM_TimeBaseInitStructure.TIM_Prescaler = psc;
    TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 80;
    TIM_TimeBaseInit( TIM1, &TIM_TimeBaseInitStructure);

    TIM_ClearITPendingBit( TIM1, TIM_IT_Update );

    NVIC_InitStructure.NVIC_IRQChannel =TIM1_UP_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority =0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority =1;
    NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
    NVIC_Init(&NVIC_InitStructure);
    TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
}

void TIM1_2_PWMOut_Init(u16 arr, u16 psc, u16 ccp)
{
    GPIO_InitTypeDef GPIO_InitStructure={0};
    TIM_OCInitTypeDef TIM_OCInitStructure={0};
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure={0};

    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOD | RCC_APB2Periph_TIM1 , ENABLE );
    RCC_APB1PeriphClockCmd( RCC_APB1Periph_TIM2, ENABLE );

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_Init( GPIOD, &GPIO_InitStructure );

    TIM_TimeBaseInitStructure.TIM_Period = arr;
    TIM_TimeBaseInitStructure.TIM_Prescaler = psc;
    TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit( TIM2, &TIM_TimeBaseInitStructure);
    TIM_TimeBaseInit( TIM1, &TIM_TimeBaseInitStructure);

    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = ccp;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
    TIM_OC2Init( TIM2, &TIM_OCInitStructure );
    TIM_OC1Init( TIM1, &TIM_OCInitStructure );

    TIM_CtrlPWMOutputs(TIM1, ENABLE );
    TIM_CtrlPWMOutputs(TIM2, ENABLE );
    TIM_OC1PreloadConfig( TIM1, TIM_OCPreload_Disable );
    TIM_OC1PreloadConfig( TIM2, TIM_OCPreload_Disable );
    TIM_ARRPreloadConfig( TIM1, ENABLE );
    TIM_ARRPreloadConfig( TIM2, ENABLE );
    TIM_Cmd( TIM1, ENABLE );
    TIM_Cmd( TIM2, ENABLE );
}

void IRQ_Init(void)
{
  NVIC_InitTypeDef NVIC_InitStructure={0};
  TIM_ClearITPendingBit( TIM1, TIM_IT_Update );
  NVIC_InitStructure.NVIC_IRQChannel =TIM1_UP_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority =0;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority =1;
  NVIC_InitStructure.NVIC_IRQChannelCmd =ENABLE;
  NVIC_Init(&NVIC_InitStructure);
  TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
  //phase1 = 14496;
 // phase2 = phase1*2;
  IRQ_on = 1;
}


void USART1_CFG(void)
{
    GPIO_InitTypeDef  GPIO_InitStructure = {0};
    USART_InitTypeDef USART_InitStructure = {0};

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD | RCC_APB2Periph_USART1, ENABLE);

    // USART1 TX-->D.5   RX-->D.6
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_30MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOD, &GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOD, &GPIO_InitStructure);

    USART_InitStructure.USART_BaudRate = 1000000;
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
    USART_InitStructure.USART_StopBits = USART_StopBits_1;
    USART_InitStructure.USART_Parity = USART_Parity_No;
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;

    USART_Init(USART1, &USART_InitStructure);
    USART_Cmd(USART1, ENABLE);
}

void ADC_Config(void) {
    ADC_InitTypeDef ADC_InitStructure = {0};
    GPIO_InitTypeDef GPIO_InitStructure = {0};

    // Enable ADC and GPIO clocks
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOD, ENABLE);
    RCC_ADCCLKConfig(RCC_PCLK2_Div4);  

    // Configure ADC Channel 0 (PC0) as analog input
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(GPIOD, &GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_2;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // ADC basic configuration
    ADC_DeInit(ADC1);
    ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
    ADC_InitStructure.ADC_ScanConvMode = DISABLE;       // Single channel
    ADC_InitStructure.ADC_ContinuousConvMode = DISABLE; // Single conversion
    ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
    ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
    ADC_InitStructure.ADC_NbrOfChannel = 1;
    ADC_Init(ADC1, &ADC_InitStructure);
    ADC_Cmd(ADC1, ENABLE);
    
    // ADC calibration
    ADC_ResetCalibration(ADC1);
    while(ADC_GetResetCalibrationStatus(ADC1));
    ADC_StartCalibration(ADC1);
    while(ADC_GetCalibrationStatus(ADC1));
    
  //  OPA_InitTypeDef OPA_InitStructure = {0};
  //  OPA_InitStructure.PSEL = CHP1;
  //  OPA_InitStructure.NSEL = CHN1;
  //  OPA_Init( &OPA_InitStructure );
  //  OPA_Cmd( ENABLE );
}

int getchar(void) {
    while((USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET)){}      
    return (USART_ReceiveData(USART1));
}

void DMA_Tx_Init(DMA_Channel_TypeDef *DMA_CHx, u32 ppadr, u32 memadr, u16 bufsize, u16 channum)
{
    ADC_InitTypeDef  ADC_InitStructure = {0};
    GPIO_InitTypeDef GPIO_InitStructure = {0};
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_ADC1, ENABLE);
    RCC_ADCCLKConfig(RCC_PCLK2_Div4);      
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_1;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    ADC_DeInit(ADC1);
    ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
    if (channum==1){ADC_InitStructure.ADC_ScanConvMode = DISABLE;}
    if (channum>1){ADC_InitStructure.ADC_ScanConvMode = ENABLE;}
    ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
    ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
    ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
    ADC_InitStructure.ADC_NbrOfChannel = channum;
    ADC_Init(ADC1, &ADC_InitStructure);

    ADC_Calibration_Vol(ADC1, ADC_CALVOL_50PERCENT);
    ADC_DMACmd(ADC1, ENABLE);
    ADC_Cmd(ADC1, ENABLE);

    ADC_ResetCalibration(ADC1);
    while(ADC_GetResetCalibrationStatus(ADC1));
    ADC_StartCalibration(ADC1);
    while(ADC_GetCalibrationStatus(ADC1));

    DMA_InitTypeDef DMA_InitStructure = {0};
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
    DMA_DeInit(DMA_CHx);
    DMA_InitStructure.DMA_PeripheralBaseAddr = ppadr;
    DMA_InitStructure.DMA_MemoryBaseAddr = memadr;
    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
    DMA_InitStructure.DMA_BufferSize = bufsize;
    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
    DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
    DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
    DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
    DMA_Init(DMA_CHx, &DMA_InitStructure);
}

void TIM1_ETRClockMode1_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure = {0};
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 | RCC_APB2Periph_GPIOD, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOD, &GPIO_InitStructure);

    TIM_CounterModeConfig(TIM1, TIM_CounterMode_Up);
    TIM_SetAutoreload(TIM1, 0xFFFF);
    TIM_ETRClockMode1Config(TIM1, TIM_ExtTRGPSC_OFF, TIM_ExtTRGPolarity_Inverted, 0x0);
    /* GPIOD2 Input as TIM Clock Source */
    TIM_TIxExternalClockConfig(TIM1, TIM_TIxExternalCLK1Source_TI1, TIM_ICPolarity_Rising, 0x00);
    TIM_Cmd(TIM1, ENABLE);
}

void SysTick_Handler(void) __attribute__((interrupt("WCH-Interrupt-fast")));
void SysTick_Handler(void)
{
    pulse_count = TIM1->CNT;
    interrupt_count++;
    if (pulse_count < pulse_last) {pulse_high++;}
    pulse_last = pulse_count;
    if (interrupt_count==1000){
        printf("%d\r\n", (pulse_high << 16) | pulse_count);
        pulse_high = 0;
        pulse_last = 0;
        interrupt_count = 0;
        SysTick->CNT = 0;
        TIM1->CNT=0; 
    }   
    SysTick->SR = 0;
}


void Serial_flush(void){
Delay_Ms(1);
USART_ReceiveData(USART1);
Delay_Ms(1);
USART_ReceiveData(USART1);
Delay_Ms(1);
USART_ReceiveData(USART1);
}


uint16_t getint(void){
  char c;
  uint16_t n=0;
  while((USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET)){}      
  c=(USART_ReceiveData(USART1));
  while (c > 13){
    if (c>47 && c<58){
      n=n*10;
      n+=c-48;
    }
    while((USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET)){}      
    c=(USART_ReceiveData(USART1));
   }
   return n;
}

u16 Get_ADC_Val(u8 ch)
{
    u16 val;
    ADC_RegularChannelConfig(ADC1, ch, 1, ADC_SampleTime_241Cycles);
    ADC_SoftwareStartConvCmd(ADC1, ENABLE);
    while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC));
    val = ADC_GetConversionValue(ADC1);
    return val;
}

void Clock48MHzHSE(void)
{ if ((RCC->CTLR & RCC_HSERDY) == 0){ 
    RCC->CFGR0 = 0x04000000;  //Default + Clk out     
    RCC->APB2PCENR |= RCC_APB2Periph_GPIOD;
    GPIOD->CFGLR&=(~0xF0);
    GPIOD->CFGLR|=0x80;
    GPIOD->BSHR =0x2;
    RCC->APB2PCENR |= RCC_AFIOEN;
    AFIO->PCFR1 |= (1<<15);
    RCC->CTLR |= RCC_HSEON;
    RCC->CTLR &= ~RCC_PLLON; 
    Delay_Ms(1);
    if ((RCC->CTLR & RCC_HSERDY) == 0){ 
      //RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC));
      RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Mul2);  //RC
    }
    else{
      RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSE_Mul2);  //Quarz
    }
    RCC->CTLR |= RCC_PLLON;
    while((RCC->CTLR & RCC_PLLRDY) == 0) {}
    RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW));
    RCC->CFGR0 |= (uint32_t)RCC_SW_PLL;
    while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08){}
  }  
}

void extensions2(void){
  if (b==1){FLASH_Unlock(); FLASH_ErasePage (0x08003C00);flash_adr = 0;}  //Flash loeschen
  if (b==2){flash_adr = 0;}                                               //Flash-Adr = 0
  if (b==3){a = *(__IO uint16_t *)(0x08003C00 + flash_adr*2), flash_adr++; flash_adr &= 0x1FF;} //Lesen
  if (b==4){FLASH_ProgramHalfWord(0x08003C00 + flash_adr*2, a);flash_adr++; flash_adr &= 0x1FF;} //Schreiben 
  if (b==5){    //Port PP 30 MHz
    GPIO_InitTypeDef GPIO_InitStructure = {0};
    GPIO_InitStructure.GPIO_Pin = a;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_30MHz;
    GPIO_Init(GPIOC, &GPIO_InitStructure); 
  }
  if (b==6){    //Port OD 30 MHz
    GPIO_InitTypeDef GPIO_InitStructure = {0};
    GPIO_InitStructure.GPIO_Pin = a;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_30MHz;
    GPIO_Init(GPIOC, &GPIO_InitStructure); 
    if (a==0){
      GPIO_InitStructure.GPIO_Pin = 12;
      GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
      GPIO_Init(GPIOD, &GPIO_InitStructure);      
    }
  }
  if (b==7){    //OPV aktiv
    OPA_InitTypeDef OPA_InitStructure = {0};
    OPA_InitStructure.PSEL = CHP1;
    OPA_InitStructure.NSEL = CHN1;
    OPA_Init( &OPA_InitStructure );
    OPA_Cmd(ENABLE);
    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOD, ENABLE );
    GPIO_InitTypeDef GPIO_InitStructure = {0};
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_7;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOD, &GPIO_InitStructure);  
    opv = 1;
  }
  if (b==8){    //Ohm an AD2(A0)
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_Init(GPIOA, &GPIO_InitStructure); 
    u32 u = Get_ADC_Val(0);
    u32 r = u * 43500 / (1023-u);
    printf("%d R   \r\n",r);
  }
  if (b==9){    //pF an AD2(A0)
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
    GPIO_Init(GPIOA, &GPIO_InitStructure); 
    GPIOA->OUTDR = 0;    //Pulldown
    GPIOA->OUTDR = 4;    //Pullup
    u32 count = 0;
    while ((GPIOA->INDR & 4)==0) {count++;} 
    printf("%d pF\r\n", (count-1) * 14);
    GPIOA->OUTDR = 0;    //Pulldown
  }
  if (b==10){     //DMA-AD1 a, PA1
    DMA_Tx_Init(DMA1_Channel1, (u32)&ADC1->RDATAR, (u32)ram, 252, 1);   
    DMA_Cmd(DMA1_Channel1, ENABLE);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
    ADC_SoftwareStartConvCmd(ADC1, ENABLE);
    while ((DMA1->INTFR & DMA1_IT_TC1) == 0)
    ADC_SoftwareStartConvCmd(ADC1, DISABLE);
    for (u16 n=0;n<252;n++){printf("%d\r\n",(ram[n*2]>>2) +(ram[n*2+1]<<6));}  
    ADC_Config();
  }
  if (b==11){     //AD-DMA Zweikanal
    DMA_Tx_Init(DMA1_Channel1, (u32)&ADC1->RDATAR, (u32)ram, 254, 2);   
    DMA_Cmd(DMA1_Channel1, ENABLE);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
    ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 2, ADC_SampleTime_15Cycles);
    ADC_SoftwareStartConvCmd(ADC1, ENABLE);
    while ((DMA1->INTFR & DMA1_IT_TC1) == 0)
    ADC_SoftwareStartConvCmd(ADC1, DISABLE);
    for (u16 n=0;n<126;n++){
      u16 d1 = (ram[n*4]>>2) +(ram[n*4+1]<<6);
      u16 d2 = (ram[n*4+2]>>2) +(ram[n*4+3]<<6);
      u16 d3 = (ram[n*4+4]>>2) +(ram[n*4+5]<<6);
      u16 d4 = (ram[n*4+6]>>2) +(ram[n*4+7]<<6);
      printf("%d\r\n", d1);
      printf("%d\r\n", d2);
      printf("%d\r\n", (d1+d3)/2);
      printf("%d\r\n", (d2+d4)/2);
    }  
    ADC_Config();
  }
  if (b==12){     //Frequenz an PD2
    //SystemCoreClockUpdate();
    TIM1_ETRClockMode1_Init();
    NVIC_EnableIRQ(SysTick_IRQn);
    SysTick->SR &= ~(1 << 0);
    SysTick->CMP = 48000-1 ; //Millisekunde
    SysTick->CNT = 0;
    SysTick->CTLR = 0xF;
    while (USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET);
    SysTick->CTLR &= ~(1 << 1);
  }
  if (b==13){opv = 0;}                    //Reset aktivieren
  if (b==14){for (u16 n=0;n<256;n++){ram[n]=0; ram_adr=0;}} //Loeschen
  if (b==15){ram_adr = 0;}                //RAM-Adr = 0
  if (b==16){a = ram[ram_adr];ram_adr++;} //Lesen
  if (b==17){ram[ram_adr]= a;ram_adr++;}  //Schreiben 
  if (b==18){Clock48MHzHSE();}            //Quarz einschalten   
}

void extensions(void){   
  if (dat==240){      //PWM1 Freq 
    TIM1->PSC= a-1; 
  }
  if (dat==241){      //keine Uref                           
    TIM2->PSC= a-1;   //PWM2 Freq 
  }
  if (dat==242){            //Hispeed Scope
    for (uint16_t n=0;n<256;n++){ram[n]=Get_ADC_Val(7) >>2;}
  }
  if (dat==243){            //Zweikanal Scope
    for (u16 n=0;n<256;n++){ram[n*2]=Get_ADC_Val(7) >>2;
      ram[n*2+1]=Get_ADC_Val(1) >>2;}
    for (u16 n=0;n<500;n++){printf("%d\r\n",ram[n]);}
  }
  if (dat==244){            //Sinustabelle
    for (uint16_t n=0;n<256;n++){ram[n]=sin_table[n];}
  }
  if (dat==245){            //DDS + Scope
    uint8_t akku = 0;
    for (u8 m=0;m<5;m++){      
      for (u16 n=0;n<255;n++){
        TIM1->CH1CVR = sin_table[akku];
        akku += a;
        ram[n]=Get_ADC_Val(7) >>2;
      }
    }
    for (uint16_t n=0;n<250;n++){printf("%d\r\n",ram[n]);}
  }
  if (dat==246){      //Sweep
    uint8_t akku = 0;
    for (uint16_t n=0;n<512;n++){
      TIM1->CH1CVR = sin_table[akku];
      akku = akku +a + (n>>4);
      ram[n]=Get_ADC_Val(7) >>2;
      Delay_Us(11);
    }  
    for (uint16_t n=0;n<500;n++){printf("%d\r\n",ram[n]);} 
  }
  if (dat==247){} 
  if (dat==248){} 
  if (dat==249){} 
  if (dat==250){extensions2();}  // Extensions2
  if (dat==251){ //Generator 1 FSYS / B.A
    u16 teiler = (b<<8) | a;
    if (gen1 == 0){TIM1_PWMOut_Init( 1, teiler-1, 1 );gen1 = 1;}
    else {TIM1->PSC = teiler-1;}
    }
  if (dat==252){ //Generator 2 FSYS / B.A
    u16 teiler = (b<<8) | a;
    if (gen2 == 0){TIM2_PWMOut_Init( 1, teiler-1, 1 );gen2 = 1;}
    else {TIM2->PSC = teiler-1;}
  } 
  if (dat==253){dF= a;} //Faktor der Frequenzuebergabe
  if (dat==254){        //PWM1 DDS A*dF
    phase1 = a * 22906 * dF;  
    if ((phase1==0) & (phase2 == 0)){TIM_ITConfig(TIM1, TIM_IT_Update, DISABLE);IRQ_on =0;}
    if ((phase1>0) | (phase2>0)) {IRQ_Init();}
  }
  if (dat==255){    //PWM2 DDS A*dF
    phase2 = a * 22906 * dF;  
    if ((phase1==0) & (phase2 == 0)) {TIM_ITConfig(TIM1, TIM_IT_Update, DISABLE);IRQ_on =0;}
    if ((phase1>0) | (phase2>0)) {IRQ_Init();}   
  }
}



  void k1(){a = dat;adr++;}  
  void k2(){b = dat;adr++;} 
  void k3(){c = dat;adr++;} 
  void k4(){d = dat;adr++;} 
  void np(){adr++;}    
  void k8(){GPIOC->OUTDR = dat;adr++;} //Pout dat
  void k9(){GPIOC->CFGLR =0x44444444; 
    GPIO_InitStructure.GPIO_Pin = dat;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_Init(GPIOC, &GPIO_InitStructure);adr++;} //Pdir dat
  void k10(){GPIO_InitStructure.GPIO_Pin = dat;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_Init(GPIOC, &GPIO_InitStructure);adr++;} //Pullup dat  
  void k11(){GPIO_InitStructure.GPIO_Pin = dat;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
    GPIO_Init(GPIOC, &GPIO_InitStructure);adr++;} //Ppulldown dat  
  void k16(){TIM1->CH1CVR = dat;adr++;} //PWM1 
  void k17(){TIM2->CH2CVR = dat;adr++;} //PWM2 
  void k18(){a &= dat;adr++;} // A = A AND dat
  void k19(){a |= dat;adr++;} // A = A Or dat
  void k24(){Delay_Us(dat);adr++;} // Delay ?s
  void k25(){warten = dat;adr++;}         // Dekay ms
  void k26(){warten = dat*1000;adr++;}    // Delay s
  void k27(){warten = dat*60000;adr++;}   // Delay min
  void k32(){adr = dat;} // Goto L 
  void k33(){adr++;if (dat<240){return_nr++;return_adr[return_nr]=adr; adr = dat;}
      if (dat>239){extensions();}} // Gosub L 
  void k34(){adr++;if (a == b){adr = dat;};} // If A = B Goto L
  void k35(){adr++;if (a > b){adr = dat;};} // If A > B Goto L
  void k36(){adr++;if (a < b){adr = dat;};} // If A < B Goto L
  void k37(){adr++;if (c > 0){adr = dat;}; c--;} // C*Goto L 
  void k38(){adr++;if (d > 0){adr = dat;}; d--;} // D*Goto L   
  void k40(){a += 1;adr++;} // A = A + 1
  void k41(){a -= 1;adr++;} // A = A - 1
  void k42(){a += b;adr++;} // A = A + B
  void k43(){a -= b;adr++;} // A = A - B
  void k44(){a = a*b;adr++;} // A = A * B
  void k45(){a /= b;adr++;} // A = A / B
  void k46(){a &= b;adr++;} // A = A AND B
  void k47(){a |= b;adr++;} // A = A OR B
  void k48(){a = a^b;adr++;} // A = A XOR B
  void k49(){a = a << 1;adr++;} // A = Shl 1
  void k50(){a = a >> 1;adr++;} // A = Shr 1
  void k51(){a = ~a;adr++;} // A = NOT A
  void k52(){b = a;adr++;} // B = A
  void k53(){a = b;adr++;} // A = B
  void k54(){c = a;adr++;} // C = A
  void k55(){a = c;adr++;} // A = C
  void k56(){d = a;adr++;} // D = A
  void k57(){a = d;adr++;} // A = D
  void k58(){a = ram[b];b++;adr++;} // A = [B+] 
  void k59(){ram[b] = a;b++;adr++;} // [B+] = A  
  void k60(){a = Get_ADC_Val(7) >>2;adr++;} // A = AD0   
  void k61(){a = Get_ADC_Val(1) >>2;adr++;} // A = AD1   
  void k62(){a = Get_ADC_Val(0) >>2;adr++;} // A = AD2  
  void k63(){a = GPIOC->INDR;adr++;} // A = Pin   
  void k64(){a = 1 & (GPIOC->INDR);adr++;} // A = Pin0   
  void k65(){if(rx < 1000){a = rx; rx = 1000;}adr++;} // Input A    
  void k66(){printf("%d\r\n",a);adr++;} // Print A   
  void k67(){TIM1->CH1CVR = a; pw1=a;adr++;} // PWM1 = A  
  void k68(){TIM2->CH2CVR = a; pw2=a;adr++;} // PWM2 = A  
  void k69(){GPIOC->OUTDR = a;adr++;} // Pout = A 
  void k72(){adr++;adr = return_adr[return_nr]; return_nr--;} // Ret  
  void k73(){adr++;adr--;} // End  

    void (*befehl[76])() = {
        np, k1, k2, k3, k4, np, np, np, 
        k8, k9, k10, k11, np, np, np, np,
        k16, k17, k18, k19, np, np, np, np,
        k24, k25, k26, k27, np, np, np, np,
        k32, k33, k34, k35, k36, k37, k38, np,
        k40, k41, k42, k43, k44, k45, k46, k47,
        k48, k49, k50, k51, k52, k53, k54, k55,
        k56, k57, k58, k59, k60, k61, k62, k63,
        k64, k65, k66, k67, k68, k69, np, np,
        k72, k73, np};


  void d65(){printf("%d\r\n",a);}  //A
  void d66(){printf("%d\r\n",b);}  //B
  void d67(){printf("%d\r\n",c);}  //C
  void d68(){printf("%d\r\n",d);}  //D
  void d69(){printf("%d\r\n",Get_ADC_Val(7)>>2);}  //E  AD0
  void d70(){printf("%d\r\n",Get_ADC_Val(1)>>2);}  //F  AD1
  void d71(){printf("%d\r\n",Get_ADC_Val(0)>>2);}  //G  AD2
  void d72(){printf("%d\r\n",GPIOC->INDR);} //H  Pin
  void d73(){weiter = 0xFFFFFFFF;} //I  Stop!
  void d74(){weiter = 0;} //J  Go!
  void d75(){GPIOC->CFGLR =0x44444444; 
    GPIO_InitStructure.GPIO_Pin = getint();
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_Init(GPIOC, &GPIO_InitStructure);adr++;}  //K  DIR
  void d76(){GPIOC->OUTDR = getint();} //L  OUT
  void d77(){GPIO_InitStructure.GPIO_Pin = getint();
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_Init(GPIOC, &GPIO_InitStructure);adr++;}  //M  Pullup
  void d78(){GPIO_InitStructure.GPIO_Pin = getint();
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
    GPIO_Init(GPIOC, &GPIO_InitStructure);adr++;} //N  Pulldown
  void d79(){u16 n = getint(); TIM1->CH1CVR = n; pw1=n;} //O  PWM1
  void d80(){u16 n = getint(); TIM2->CH2CVR = n; pw2=n;} //P  PWM2
  void d81(){for(int j=0; j<256; j++){u16 n = getint();ram[j]=n;}} //Q  RAM fuellen
  void d82(){a = getint();} //R  A=
  void d83(){b = getint();} //S  B=
  void d84(){c = getint();} //T  C=
  void d85(){d = getint();} //U  D=
  void d86(){u16 n = getint(); TIM1->PSC= n-1; TIM2->PSC= n-1; } //V  PWM-Vorteiler

  void (*direkt[22])() = {
  d65, d66, d67, d68, d69, d70, d71, d72, 
  d73, d74, d75, d76, d77, d78, d79, d80,
  d81, d82, d83, d84, d85, d86};
  

int main(void){
  GPIO_InitTypeDef GPIO_InitStructure = {0};
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_7;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
  GPIO_Init(GPIOD, &GPIO_InitStructure); 
  //SystemCoreClockUpdate();
  Delay_Init();
  USART_Printf_Init(115200);
  SystemCoreClockUpdate();
  RCC_AdjustHSICalibrationValue(0x0D);
  Delay_Ms(1);
  
  if ((FLASH_GetUserOptionByte() & 255) != 223){  // nur wenn RST-Pin noch aktiv
    FLASH_Unlock();
    FLASH_EraseOptionBytes();
    FLASH_UserOptionByteConfig(OB_IWDG_SW, OB_STDBY_NoRST, OB_RST_NoEN, OB_PowerON_Start_Mode_USER);
  }

/*
  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
  u32 HSEStatus = RCC->CTLR & (1 << 17); // Bit 17: HSERDY
  if (HSEStatus == 0) { //Quarz schwingt nicht
    RCC->CFGR0 &= ~(1 << 16); //  HSI als PLL-Quelle
    RCC->CTLR |= (1 << 24); // PLL einschalten
    while (!(RCC->CTLR & (1 << 25))); // Warten, bis PLL bereit
    RCC->CFGR0 = (RCC->CFGR0 & ~0x3) | 0x2;  //PLL als Systemtakt
    while ((RCC->CFGR0 & 0xC) != 0x8);
    RCC->CTLR &= ~((uint32_t)RCC_HSEON); //HSE aus
    AFIO->PCFR1 &= ~(1<<15); //PA12 als IO-Ports
    RCC->APB2PCENR &= ~RCC_AFIOEN; //Quarzoszillator abschalten
    RCC_AdjustHSICalibrationValue(0x0D);
  }
*/

  TIM1_2_PWMOut_Init(255, 0, 0 );
  TIM1_PWMOut_Init(255, 0, 0 );
  TIM2_PWMOut_Init(255, 0, 0 );
  ADC_Config();
  USART1_CFG();
  for (adr = 0; adr < 255; adr++) {  //Daten aus dem Flash laden
    u16 eedat = *(__IO uint16_t *)(0x08003800 + adr*2) ;  
    if (eedat>0x4900){eedat=0x4900;}        
    code[adr] = eedat;
  } 
  adr=0;
  Serial_flush();
  millis = 0;
  SysTick->CNT = 0;
  SysTick->CTLR = 0x1;
  warten = 0;
  weiter = 0; 

  while(1){
    if (USART_GetFlagStatus(USART1, USART_FLAG_RXNE) != RESET){
      ch = getchar();
      if (ch>47 && ch<58){
        rx = ch - 48;
        while (ch!=13){
          if (USART_GetFlagStatus(USART1, USART_FLAG_RXNE) != RESET){
            ch = getchar();
            if (ch>47 && ch<58){
              rx = rx * 10;
              rx = rx + ch - 48;
              rx = rx & 255;
            }
          }
        }
        printf("%d\r\n",rx);
      }
           
      if (ch==112){   //p
        uint16_t n = getint();
        for (uint16_t i=0;i<n;i++){
          uint16_t pdat = getint();
          code[i] = pdat;
        }
        Serial_flush();
        adr = 0; 
        a=0; b=0; c=0; d=0;
        return_nr = 0;
        rx = 1000;
        weiter = 0; //keine Wartezeit
        warten = 0;    
        millis = 0;   
        SysTick->CNT = 0;  
        SysTick->CTLR = 0x1;     
      }

      if (ch==101){   //e
        uint16_t n = getint();
        for (uint16_t i=0;i<n;i++){
          uint16_t pdat = getint();
          code[i] = pdat;
        }
        FLASH_Unlock();
        FLASH_ErasePage (0x08003800);
        for (uint16_t i=0;i<n;i++){
          FLASH_ProgramHalfWord(0x08003800 + i*2, code[i]);
        }
        adr = 0; 
        a=0; b=0; c=0; d=0;
        return_nr = 0;
        rx = 1000;
        weiter = 0; //keine Wartezeit
        warten = 0;
      }

      if (ch>64 && ch<88){    //A bis V Direktkommandos
        direkt[ch-65]();
      }
    }
    if (opv==0){
      while ((GPIOD->INDR & 1) == 0){adr=0;}  //PicoBasic Reset
      if ((GPIOD->INDR & 128) == 0){PFIC->SCTLR |= (1 << 31);}  //Conroller Reset
    }
    if  (SysTick->CNT > 6000){SysTick->CNT -= 6000; millis++;}
    if(warten>0){weiter = millis + warten; warten = 0;}    
    if(millis >= weiter)     
    {
      kom = code[adr] >> 8;
      dat = code[adr] & 255;
      befehl[kom]();
    }
  }
}
  

                