COMP(term);

HAL_PIN(wave0) = 0.0;
HAL_PIN(gain0) = 10.0;
HAL_PIN(offset0) = 0.0;

HAL_PIN(wave1) = 0.0;
HAL_PIN(gain1) = 10.0;
HAL_PIN(offset1) = 0.0;

HAL_PIN(wave2) = 0.0;
HAL_PIN(gain2) = 10.0;
HAL_PIN(offset2) = 0.0;

HAL_PIN(wave3) = 0.0;
HAL_PIN(gain3) = 10.0;
HAL_PIN(offset3) = 0.0;

HAL_PIN(wave4) = 0.0;
HAL_PIN(gain4) = 10.0;
HAL_PIN(offset4) = 0.0;

HAL_PIN(wave5) = 0.0;
HAL_PIN(gain5) = 10.0;
HAL_PIN(offset5) = 0.0;

HAL_PIN(wave6) = 0.0;
HAL_PIN(gain6) = 10.0;
HAL_PIN(offset6) = 0.0;

HAL_PIN(wave7) = 0.0;
HAL_PIN(gain7) = 10.0;
HAL_PIN(offset7) = 0.0;

HAL_PIN(send_step) = 10.0;

HAL_PIN(buf_use) = 0.0;

HAL_PIN(fault) = 0.0;
HAL_PIN(con) = 0.0;

HAL_PIN(tim2_rv) = 420.0;

#define TERM_BUF_SIZE 24

MEM(float w0[TERM_BUF_SIZE]);
MEM(float w1[TERM_BUF_SIZE]);
MEM(float w2[TERM_BUF_SIZE]);
MEM(float w3[TERM_BUF_SIZE]);
MEM(float w4[TERM_BUF_SIZE]);
MEM(float w5[TERM_BUF_SIZE]);
MEM(float w6[TERM_BUF_SIZE]);
MEM(float w7[TERM_BUF_SIZE]);

MEM(unsigned int read_pos) = 0;
MEM(unsigned int write_pos) = 0;
MEM(unsigned int send_counter) = 0;

MEM(unsigned int last_fault_sent) = 0; //save last error message
MEM(uint32_t state) = HAL_OK;
MEM(uint16_t ee_error);


int load(){
   if(hal.rt_state != RT_STOP || hal.frt_state != FRT_STOP){
      return(-4);
   }
   if(ee_error != 8){
      return -1;
   }
   typedef union{
      float f;
      uint16_t byte[2];
   }param_t;
   param_t param;
   uint16_t address = 0;
   uint16_t lo;
   uint16_t hi;
   uint16_t elo;
   uint16_t ehi;
   for(int i = 0; i < hal.hal_pin_count; i++){
      if(address >= NB_OF_VAR){
         return -2;
      }
      char name[6];
      strncpy(name,hal.hal_pins[i]->name,5);
      name[5] =  '\0';
      if(!strcmp(name, "conf0")){
         elo = EE_ReadVariable(address,&lo);
         ehi = EE_ReadVariable(address+1,&hi);
         if(elo == 0 && ehi == 0){
            param.byte[0] = lo;
            param.byte[1] = hi;
            hal.hal_pins[i]->value = param.f;
         }else{
            return -3;
         }
         address+=2;
         Wait(1);
      }
   }
   //update cmd/fb links
   update_cmd();
   update_fb();
   return 0;
}


INIT(
   FLASH_Unlock();
   ee_error = EE_Init();
   FLASH_Lock();
   int ret = load();
   if(ret){//flash load error
      hal.hal_state = CONFIG_LOAD_ERROR;
   }
);

RT(
   TIM2->ARR = (int)CLAMP(PIN(tim2_rv), 1, 1500);
   if(send_counter++ >= PIN(send_step) - 1){
      w0[write_pos] = PIN(wave0);
      w1[write_pos] = PIN(wave1);
      w2[write_pos] = PIN(wave2);
      w3[write_pos] = PIN(wave3);
      w4[write_pos] = PIN(wave4);
      w5[write_pos] = PIN(wave5);
      w6[write_pos] = PIN(wave6);
      w7[write_pos] = PIN(wave7);

      write_pos++;
      write_pos %= TERM_BUF_SIZE;
      send_counter = 0;
   }
);

NRT(

   void bootloader(){
      *((unsigned long *)0x2001C000) = 0xDEADBEEF;
      NVIC_SystemReset();
   }

   void reset(){
      NVIC_SystemReset();
   }

   void hal_info(){
      printf_("######## hal info ########\n");
      printf_("#pins %i\n", hal.hal_pin_count);
      printf_("#comps %i\n", hal.comp_count);
      printf_("link errors %i\n", hal.link_errors);
      printf_("pin errors %i\n", hal.pin_errors);
      printf_("comp errors %i\n", hal.comp_errors);
      printf_("set errors %i\n", hal.set_errors);
      printf_("get errors %i\n", hal.get_errors);
      printf_("foo0.bar:  %f\n", get_hal_pin("foo0.bar"));
      printf_("error_name: %s\n",hal.error_name);
      float pe = get_hal_pin("net0.rt_period");
      float ct = get_hal_pin("net0.rt_calc_time");
      if(pe > 0.0){
         printf_("rt time: %f/%fs", ct, pe);
         printf_("=%f%%\n",(ct/pe)*100);
      }
      pe = get_hal_pin("net0.frt_period");
      ct = get_hal_pin("net0.frt_calc_time");
      if(pe > 0.0){
         printf_("frt time: %f/%fs", ct, pe);
         printf_("=%f%%\n",(ct/pe)*100);
      }
      pe = get_hal_pin("net0.nrt_period");
      ct = get_hal_pin("net0.nrt_calc_time");
      if(pe > 0.0){
         printf_("nrt time: %f/%fs", ct, pe);
         printf_("=%f%%\n",(ct/pe)*100);
      }
      switch(hal.rt_state){
         case RT_STOP:
            printf_("rt state:  STOP\n");
            break;
         case RT_SLEEP:
            printf_("rt state:  SLEEP\n");
            break;
         case RT_CALC:
            printf_("rt state:  CALC\n");
            break;
      }
      switch(hal.frt_state){
         case FRT_STOP:
            printf_("frt state:  STOP\n");
            break;
         case FRT_SLEEP:
            printf_("frt state:  SLEEP\n");
            break;
         case FRT_CALC:
            printf_("frt state:  CALC\n");
            break;
      }
      switch(hal.hal_state){
         case HAL_OK:
            printf_("HAL state:  HAL_OK\n");
            break;
         case RT_TOO_LONG:
            printf_("HAL state:  RT_TOO_LONG\n");
            break;
         case FRT_TOO_LONG:
            printf_("HAL state:  FRT_TOO_LONG\n");
            break;
         case MISC_ERROR:
            printf_("HAL state:  MISC_ERROR\n");
            break;
         case MEM_ERROR:
            printf_("HAL state:  MEM_ERROR\n");
            break;
         case CONFIG_LOAD_ERROR:
            printf_("HAL state:  CONFIG_LOAD_ERROR\n");
            break;
         case CONFIG_ERROR:
            printf_("HAL state:  CONFIG_ERROR\n");
            break;
      }
      uint32_t p = 0;
      printf_("active rt funcs(%u):\n", hal.rt_func_count);
      for(int i = 0; i < hal.rt_func_count; i++){
         p = (uint32_t)hal.rt[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->rt){
               printf_("   %s%u.rt(%f)\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance,  hal.hal_pins[hal.hal_comps[i]->hal_pin_start_index + 2]->source->source->value);
               break;
            }
         }
      }
      printf_("\nactive frt funcs(%u):\n", hal.frt_func_count);
      for(int i = 0; i < hal.frt_func_count; i++){
         p = (uint32_t)hal.frt[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->frt){
               printf_("   %s%u.frt(%f)\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance, hal.hal_pins[hal.hal_comps[i]->hal_pin_start_index + 3]->source->source->value);
               break;
            }
         }
      }
      printf_("\nactive rt_init funcs(%u):\n", hal.rt_init_func_count);
      for(int i = 0; i < hal.rt_init_func_count; i++){
         p = (uint32_t)hal.rt_init[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->rt_init){
               printf_("   %s%u.rt_init\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance);
               break;
            }
         }
      }
      printf_("\nactive rt_deinit funcs(%u):\n", hal.rt_deinit_func_count);
      for(int i = 0; i < hal.rt_deinit_func_count; i++){
         p = (uint32_t)hal.rt_deinit[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->rt_deinit){
               printf_("   %s%u.rt_deinit\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance);
               break;
            }
         }
      }
      printf_("\nactive nrt_init funcs(%u):\n", hal.nrt_init_func_count);
      for(int i = 0; i < hal.nrt_init_func_count; i++){
         p = (uint32_t)hal.nrt_init[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->nrt_init){
               printf_("   %s%u.nrt_init\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance);
               break;
            }
         }
      }
      printf_("\nactive nrt funcs(%u):\n", hal.nrt_func_count);
      for(int i = 0; i < hal.nrt_func_count; i++){
         p = (uint32_t)hal.nrt[i];
         for(int i = 0; i < hal.comp_count; i++){
            if(p == (uint32_t)hal.hal_comps[i]->nrt){
               printf_("   %s%u.nrt\n", hal.hal_comps[i]->name, hal.hal_comps[i]->instance);
               break;
            }
         }
      }
      printf_("\n");
   }

   void about(){
      printf_("######## software info ########\n");
      printf_(
          "%s v%i.%i.%i %s\n",
          version_info.product_name,
          version_info.major,
          version_info.minor,
          version_info.patch,
          version_info.git_version
      );
      printf_("Branch %s\n",version_info.git_branch);
      printf_("Compiled %s %s ",version_info.build_date, version_info.build_time);
      printf_("by %s on %s\n",version_info.build_user, version_info.build_host);
      printf_("GCC        %s\n",__VERSION__);
      printf_("newlib     %s\n",_NEWLIB_VERSION);
      printf_("CMSIS      %i.%i\n",__CM4_CMSIS_VERSION_MAIN,__CM4_CMSIS_VERSION_SUB);
      printf_("StdPeriph  %i.%i.%i\n",__STM32F4XX_STDPERIPH_VERSION_MAIN,__STM32F4XX_STDPERIPH_VERSION_SUB1,__STM32F4XX_STDPERIPH_VERSION_SUB2);
      uint32_t crc = crc32_init();
      crc = crc32_update(crc, (void*)0x08010000, version_info.image_size);
      crc = crc32_finalize(crc);
      printf_("size: %u crc:%x\n",version_info.image_size,version_info.image_crc);
      if(crc == 0)
         printf_("crc ok!\n");
      else
         printf_("crc error!:%x\n",crc);
      
      printf_("######## Bootloader info ########\n");
      printf_(
          "%s v%i.%i.%i %s\n",
          bt_version_info->product_name,
          bt_version_info->major,
          bt_version_info->minor,
          bt_version_info->patch,
          bt_version_info->git_version
      );
      printf_("Branch %s\n",bt_version_info->git_branch);
      printf_("Compiled %s %s ",bt_version_info->build_date, bt_version_info->build_time);
      printf_("by %s on %s\n",bt_version_info->build_user, bt_version_info->build_host);

      printf_("start:%p ,size:%p ,end%p \n",&_binary_stm32f103_main_bin_start,&_binary_stm32f103_main_bin_size,&_binary_stm32f103_main_bin_end);
   }

   void start(){
      printf_("starting hal ... ");
      start_hal();
      printf_("done\n");
   }

   void stop(){
      printf_("stopping hal ... ");
      stop_hal();
      printf_("done\n");
   }

   void sysinfo(){
      printf_("######## sysinfo ########\n");
      extern char _etext;    // end address of the .text section
      extern char _sidata;   // start address of the initialization values of the .data section
      extern char _sdata;    // start address of the .data section
      extern char _edata;    // end address of the .data section
      extern char _sbss;     // start address of the .bss section
      extern char _ebss;     // end address of the .bss section
      //extern char _snoinit;  // start address of the .noinit section
      //extern char _enoinit;  // end address of the .noinit section
      extern char _end;      // end address of the .bss section
      extern char _estack;   // initial value of the stack pointer

      extern char *__brkval;

      RCC_ClocksTypeDef  RCC_ClocksStatus;
      RCC_GetClocksFreq(&RCC_ClocksStatus);

      printf_("HSE_VALUE        = %uHz\n", HSE_VALUE);
      printf_("SYSCLK_Frequency = %uHz\n", RCC_ClocksStatus.SYSCLK_Frequency );
      printf_("HCLK_Frequency   = %uHz\n", RCC_ClocksStatus.HCLK_Frequency   );
      printf_("PCLK1_Frequency  = %uHz\n", RCC_ClocksStatus.PCLK1_Frequency  );
      printf_("PCLK2_Frequency  = %uHz\n", RCC_ClocksStatus.PCLK2_Frequency  );
      extern void *g_pfnVectors;
      printf("vtor %lu\n",(uint32_t)&g_pfnVectors);
      //*****************************************************************
      printf_("RCC->CSR = %x\n", (unsigned int)RCC->CSR);
      printf_("_etext   = %p\n", &_etext  );
      printf_("_sidata  = %p\n", &_sidata );
      printf_("_sdata   = %p\n", &_sdata  );
      printf_("_edata   = %p\n", &_edata  );
      printf_("_sbss    = %p\n", &_sbss   );
      printf_("_ebss    = %p\n", &_ebss   );
      // printf_("_snoinit = %p\n", &_snoinit);
      // printf_("_enoinit = %p\n", &_enoinit);
      printf_("_end     = %p\n", &_end    );
      printf_("_estack  = %p\n", &_estack );

      printf_("heap avail       = %uB\n", (uint32_t)((char*)__get_MSP() - __brkval));
   }

   void listhal(){
      for(int i = 0; i < hal.hal_pin_count; i++){
         printf_("%s <= %s = %f\n", hal.hal_pins[i]->name, hal.hal_pins[i]->source->name, hal.hal_pins[i]->source->source->value);
         Wait(1);
      }
   }

   void save(){
      if(ee_error != 8){
         printf_("flash error:%i\n",ee_error);
         return;
      }
      typedef union{
         float f;
         uint16_t byte[2];
      }param_t;
      param_t param;
      uint16_t elo;
      uint16_t ehi;
      uint16_t address = 0;
      FLASH_Unlock();
      for(int i = 0; i < hal.hal_pin_count; i++){
         if(address >= NB_OF_VAR){
            printf_("NB_OF_VAR too small\n");
            FLASH_Lock();
            return;
         }
         char name[6];
         strncpy(name,hal.hal_pins[i]->name,5);
         name[5] =  '\0';
         if(!strcmp(name, "conf0")){
            param.f = hal.hal_pins[i]->source->source->value;
            //printf_("param: %s=%f address:%i\n",hal.hal_pins[i]->name,param.f,address);
            elo = EE_WriteVariable(address,param.byte[0]);
            ehi = EE_WriteVariable(address+1,param.byte[1]);
            if(elo != 8 || ehi != 8){
               printf_("error writing to %i: error%i,%i\n",address,elo,ehi);
               break;
            }
            address+=2;
         }
      }
      FLASH_Lock();
      printf_("done\n");
   }

   void conf(){
      for(int i = 0; i < hal.hal_pin_count; i++){
         char name[6];
         strncpy(name,hal.hal_pins[i]->name,5);
         name[5] =  '\0';
         if(!strcmp(name, "conf0")){
            printf_("%s = %f\n", hal.hal_pins[i]->name, hal.hal_pins[i]->value);
         }
      }
   }

   void load_cmd(){
      int ret = load();
      if(ret == -1){
         printf_("flash error:%i\n",ee_error);
         return;
      }else if(ret == -2){
         printf_("NB_OF_VAR too small\n");
         return;
      }else if(ret == -3){
         printf_("flash read error\n");
      }else if(ret == -4){
         printf_("hal running error\n");
      }else{
         printf_("done\n");
      }
   }

   void help(){
      printf_("######## HAL cheat sheet ########\n");
      printf_("pin name: <comp name><comp instance number>.<pin name> (e.g. pid0.enable)\n");
      printf_("show pin value and source: pid0.enable (answer: pid0.enable <= fault0.enable_pid = 1.000000)\n");
      printf_("set pin: pid0.enable = 1.5 (answer: OK pid0.enable = 1.500000)\n");
      printf_("link pin: pid0.enable = fault0.enable_pid (answer: OK pid0.enable <= fault0.enable_pid = 1.000000)\n");
      printf_("unlink pin: pid0.enable = pid0.enable (answer: OK pid0.enable <= pid0.enable = 1.000000)\n");
      printf_("show hal info: hal\n");
      printf_("show sys info: sysinfo\n");
      printf_("show software version: about\n");
      printf_("stop realtime hal: stop\n");
      printf_("start realtime hal: start\n");
      printf_("change rt priority: pid0.rt_prio = 6 (-1 = disable, 0 = highest, stop and start hal to apply)\n");
      printf_("change frt priority: pid0.frt_prio = -1 (-1 = disable, 0 = highest, stop and start hal to apply)\n");
      printf_("reset cpu: reset\n");
      printf_("print help: help\n");
      printf_("list all hal pins: list\n");
      printf_("list all conf pins: conf\n");
      printf_("save conf: save\n");
      printf_("load conf: load\n");
   }

   void cmd(char * s){
      if(!strcmp(s, "bootloader")){
         bootloader();
      }
      else if(!strcmp(s, "start")){
         start();
      }
      else if(!strcmp(s, "stop")){
         stop();
      }
      else if(!strcmp(s, "hal")){
         hal_info();
      }
      else if(!strcmp(s, "about")){
         about();
      }
      else if(!strcmp(s, "sysinfo")){
         sysinfo();
      }
      else if(!strcmp(s, "reset")){
         reset();
      }
      else if(!strcmp(s, "list")){
         listhal();
      }
      else if(!strcmp(s, "help")){
         help();
      }
      else if(!strcmp(s, "save")){
         save();
      }
      else if(!strcmp(s, "load")){
         load_cmd();
      }
      else if(!strcmp(s, "conf")){
         conf();
      }
      else{
         printf_("not found: %s\n",s);
      }
   }

   void hal_error_sender(){
      if(hal.hal_state != state){
         switch(hal.hal_state){
            case HAL_OK:
               printf_("HAL state:  HAL_OK\n");
               break;
            case RT_TOO_LONG:
               printf_("HAL state:  RT_TOO_LONG\n");
               break;
            case FRT_TOO_LONG:
               printf_("HAL state:  FRT_TOO_LONG\n");
               break;
            case MISC_ERROR:
               printf_("HAL state:  MISC_ERROR\n");
               break;
            case MEM_ERROR:
               printf_("HAL state:  MEM_ERROR\n");
               break;
            case CONFIG_LOAD_ERROR:
               printf_("HAL state:  CONFIG_LOAD_ERROR\n");
               break;
            case CONFIG_ERROR:
               printf_("HAL state:  CONFIG_ERROR\n");
               break;
         }
      }
   }

   void fault_sender(){

     int flt = (int)PIN(fault);

     if(flt != last_fault_sent){
       last_fault_sent = flt;
       switch(flt){
         case STATE_DISABLED:
           printf_("INFO: Disabled \n");
         break;

         case STATE_RESET:
           printf_("INFO: Reset \n");
         break;

         case STATE_FB_ERROR:
           printf_("ERROR: Feedback \n");
         break;

         case STATE_SAT_ERROR:
           printf_("ERROR: Saturation \n");
         break;

         case STATE_OVR_CURR:
           printf_("ERROR: Overcurrent \n");
         break;

         case STATE_POS_ERROR:
           printf_("ERROR: Position \n");
         break;

         case STATE_OVR_TEMP:
           printf_("ERROR: Overtemperture \n");
         break;

         case STATE_OVR_VOLT:
           printf_("ERROR: Overvoltage \n");
         break;

         case STATE_ENABLED:
           printf_("INFO: Enabled \n");
         break;

         case STATE_PHASING:
           printf_("INFO: Phasing \n");
         break;

         default:
         break;
       }
     }
   }


   int e = 0;
   unsigned char buf[8 + 2];

   buf[0] = 255;
   buf[8 + 1] = 0;

   float o0 = PIN(offset0);
   float o1 = PIN(offset1);
   float o2 = PIN(offset2);
   float o3 = PIN(offset3);
   float o4 = PIN(offset4);
   float o5 = PIN(offset5);
   float o6 = PIN(offset6);
   float o7 = PIN(offset7);
   float g0 = PIN(gain0);
   float g1 = PIN(gain1);
   float g2 = PIN(gain2);
   float g3 = PIN(gain3);
   float g4 = PIN(gain4);
   float g5 = PIN(gain5);
   float g6 = PIN(gain6);
   float g7 = PIN(gain7);


   unsigned int wp = write_pos;
   unsigned int bc = 0;

   while(read_pos != wp){
      bc++;
      e = (w0[read_pos] + o0) * g0 + 128;
      buf[1] = CLAMP(e,1,254);

      e = (w1[read_pos] + o1) * g1 + 128;
      buf[2] = CLAMP(e,1,254);

      e = (w2[read_pos] + o2) * g2 + 128;
      buf[3] = CLAMP(e,1,254);

      e = (w3[read_pos] + o3) * g3 + 128;
      buf[4] = CLAMP(e,1,254);

      e = (w4[read_pos] + o4) * g4 + 128;
      buf[5] = CLAMP(e,1,254);

      e = (w5[read_pos] + o5) * g5 + 128;
      buf[6] = CLAMP(e,1,254);

      e = (w6[read_pos] + o6) * g6 + 128;
      buf[7] = CLAMP(e,1,254);

      e = (w7[read_pos] + o7) * g7 + 128;
      buf[8] = CLAMP(e,1,254);

      read_pos++;
      read_pos %= TERM_BUF_SIZE;

      buf[8 + 1] = 0;

      if(UB_USB_CDC_GetStatus()==USB_CDC_CONNECTED){
         PIN(con) = 1.0;
         UB_USB_CDC_SendString((char*)buf, NONE);
      }else{
         PIN(con) = 0.0;
      }
   }

   PIN(buf_use) = bc;



   if(UB_USB_CDC_GetStatus()==USB_CDC_CONNECTED && systime >= 1000){

      char source[APP_TX_BUF_SIZE];
      char sink[APP_TX_BUF_SIZE];

      fault_sender();
      hal_error_sender();

      int i = -1;
      char rx_buf[APP_TX_BUF_SIZE];
      if(UB_USB_CDC_ReceiveString(rx_buf)==RX_READY){
         i = sscanf_(rx_buf, "%N = %N",sink ,source);
      }
      if(i == 2){ // read hal pin
         if(is_hal_pin(sink)){
            printf_("%s <= %s = %f\n", sink, find_hal_pin(sink)->source->name, get_hal_pin(sink));
         }else{
            cmd(rx_buf);
         }
      }
      else if(i == 5){// link hal pin
         if(is_hal_pin(source) && is_hal_pin(sink)){
            link_hal_pins(source, sink);
            printf_("OK %s <= %s = %f\n", sink, source, get_hal_pin(sink));
         }
         else if(is_hal_pin(sink)){// set hal pin
            set_hal_pin(sink, strtof(source,NULL));
            printf_("OK %s = %f\n", sink, get_hal_pin(sink));
         }
         else{// hal pin not found
            printf_("not found: %s\n", sink);
         }
      }
   }

);

ENDCOMP;