#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "main.h"

#define DEF_F_BAUD 19200
#define DEF_BAUD 64 * F_CPU / 16 / DEF_F_BAUD

#define ADC_MUXPOS_CARTRIDGE_RESISTANCE ADC_MUXPOS_AIN4_gc
#define ADC_MUXPOS_CARTRIDGE_CURRENT ADC_MUXPOS_AIN7_gc
#define ADC_MUXPOS_CARTRIDGE_TEMPERATURE ADC_MUXPOS_AIN8_gc
#define ADC_MUXPOS_V_IN ADC_MUXPOS_AIN9_gc

#define BUFFER_SIZE (64)

volatile uint8_t debugRxStateMachine = 0x00;
volatile uint8_t debugRxBuffer[BUFFER_SIZE];
volatile uint8_t debugRxBufferIndex = 0;
volatile uint8_t debugRxFrameLength = 0;

volatile uint8_t debugTxStateMachine = 0x00;
volatile uint8_t debugTxBuffer[BUFFER_SIZE];
volatile uint8_t debugTxBufferIndex = 0;
volatile uint8_t debugTxFrameLength = 0;

volatile uint8_t adc_channel = 0;
volatile uint8_t adc_counter = 0;

volatile uint16_t cartridge_temperature_data[8];
volatile uint8_t cartridge_temperature_data_pointer = 0;

//~ volatile uint8_t out_data[32];
//~ volatile uint8_t out_data_pointer = 0;

volatile uint16_t sleep_kp_value = 0;
volatile uint16_t sleep_counter = 0;
volatile uint8_t sleep_state = 1;
volatile uint16_t out_avg = 0;
volatile int8_t out_avg_counter = 0;

#define Kp 30
#define Kd 0
#define Ki 0.5
#define Kit 400
#define errSumMax 1 / Ki * Kit

volatile float controllerKp = 0;
volatile float controllerKpBuff = Kp;
volatile uint8_t controllerKpBuffFlag = 1;

volatile float controllerKd = 0;
volatile float controllerKdBuff = Kd;
volatile uint8_t controllerKdBuffFlag = 1;

volatile float controllerKi = 0;
volatile float controllerKiBuff = Ki;
volatile uint8_t controllerKiBuffFlag = 1;

volatile float controllerKit = 0;
volatile float controllerKitBuff = Kit;
volatile uint8_t controllerKitBuffFlag = 1;

volatile float controllerErrSumMax = 0;

volatile float tSet = 0;
volatile float tSetBuff = 200;
volatile uint8_t tSetBuffFlag = 1;

#define OUT_MAX 800

volatile float tAct = 0;
volatile float err = 0;
volatile float errAlt = 0;
volatile float errSum = 0;
volatile float outOrig = 0;
volatile uint16_t out = 0;
volatile uint16_t outOld = 0;
volatile uint8_t outSettlingCounter = 0;
volatile uint8_t outSettlingState = 0;

volatile uint8_t debugDataEnable = 0;

void DebugAppendData(uint8_t data)
{
  if (debugTxStateMachine == 0x00)
  {
    if (debugTxFrameLength < BUFFER_SIZE)
    {
      debugTxBuffer[debugTxFrameLength] = data;
      
      debugTxFrameLength++;
    }
  }
}

void DebugSendData(void)
{
  if (debugTxStateMachine == 0x00)
  {
    debugTxStateMachine = 0x01;
    
    while ((USART0.STATUS & 0b00100000) == 0) {}
    
    USART0.CTRLA |= 0b00100000;
    
    USART0.TXDATAL = 0xff;
  }
}

ISR(USART0_RXC_vect)
{
  uint8_t data = USART0.RXDATAL;
  
  switch (debugRxStateMachine)
  {
    case 0x00:
      if (data == 0xff)
      {
        debugRxStateMachine = 0x01;
      }
      break;
    
    case 0x01:
      if (data == 0xf1)
      {
        debugRxStateMachine = 0x02;
      }
      else
      {
        debugRxStateMachine = 0x00;
      }
      break;
    
    case 0x02:
      debugRxFrameLength = data;
      
      if (debugRxFrameLength <= BUFFER_SIZE)
      {
        debugRxStateMachine = 0x03;
      }
      else
      {
        debugRxStateMachine = 0xff;
      }
      break;
    
    case 0x03:
      if (debugRxBufferIndex < BUFFER_SIZE)
      {
        debugRxBuffer[debugRxBufferIndex] = data;
        
        debugRxBufferIndex++;
        
        if (debugRxBufferIndex >= debugRxFrameLength)
        {
          debugRxBufferIndex = 0;
          
          debugRxStateMachine = 0x04;
        }
      }
      else
      {
        debugRxStateMachine = 0xff;
      }
      break;
      
    default:
      break;
  }
}

ISR(USART0_DRE_vect)
{
  switch (debugTxStateMachine)
  {
    case 0x01:
      debugTxStateMachine = 0x02;
      
      USART0.TXDATAL = 0xf1;
      break;
    
    case 0x02:
      debugTxStateMachine = 0x03;
      
      debugTxBufferIndex = 0;
      
      USART0.TXDATAL = debugTxFrameLength;
      break;
    
    case 0x03:
      if (debugTxBufferIndex == debugTxFrameLength - 1)
      {
        debugTxStateMachine = 0x04;
      }
      
      USART0.TXDATAL = debugTxBuffer[debugTxBufferIndex];
      
      debugTxBufferIndex++;
      break;
    
    case 0x04:
      USART0.CTRLA &= 0b11011111;
      
      debugTxFrameLength = 0;
      
      debugTxStateMachine = 0;
      break;
    
    default:
      break;
  }
}

ISR(ADC0_RESRDY_vect)
{
  //~ uint16_t adc_value = ADC0.RES >> 5;
  uint16_t adc_value = ADC0.RES;
  
  switch (adc_channel)
  {
    case 100:
      cartridge_temperature_data[cartridge_temperature_data_pointer] = adc_value;
      
      cartridge_temperature_data_pointer++;
      cartridge_temperature_data_pointer &= 7;
      
      uint16_t cartridge_temperature = 0;
      
      for (uint8_t i = 0; i < 8; i++)
      {
        cartridge_temperature += cartridge_temperature_data[i];
      }
      
      cartridge_temperature >>= 3;
      
      //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
      
      if (tSetBuffFlag > 0)
      {
        tSet = tSetBuff;
        tSetBuffFlag = 0;
        
        if (tSet > 250)
        {
          sleep_state = 0;
          sleep_counter = 0;
        }
        else
        {
          sleep_state++;
        }
      }
      
      
      if (controllerKpBuffFlag > 0)
      {
        controllerKp = controllerKpBuff;
        controllerKpBuffFlag = 0;
        
        sleep_kp_value = (uint16_t) (controllerKp * 2.0);
      }
      
      if (controllerKdBuffFlag > 0)
      {
        controllerKd = controllerKdBuff;
        controllerKdBuffFlag = 0;
      }
      
      if (controllerKiBuffFlag > 0)
      {
        if (controllerKiBuff == 0)
        {
          controllerKiBuff = 1;
        }
        
        controllerKi = controllerKiBuff;
        controllerErrSumMax = 1 / controllerKi * controllerKit;
        controllerKiBuffFlag = 0;
      }
      
      if (controllerKitBuffFlag > 0)
      {
        controllerKit = controllerKitBuff;
        controllerErrSumMax = 1 / controllerKi * controllerKit;
        controllerKitBuffFlag = 0;
      }
      
      float m = 0.4833149757, b = 35.1689715661;
      
      tAct = ((float) cartridge_temperature) * m + b;
      
      err = tSet - tAct;
      errSum = errSum + err;
      if (errSum < -controllerErrSumMax)
      {
        errSum = -controllerErrSumMax;
      }
      if (errSum > controllerErrSumMax)
      {
        errSum = controllerErrSumMax;
      }
      
      outOrig = controllerKp * err + controllerKi * errSum + controllerKd * (err - errAlt);
      errAlt = err;
      
      outOld = out;
      
      if (outOrig < 0)
      {
        out = 0;
      }
      else if (outOrig > OUT_MAX)
      {
        out = OUT_MAX;
      }
      else
      {
        out = (uint16_t) outOrig;
      }
      
      TCA0.SINGLE.CMP0BUF = out;
      
      //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
      
      if (out == 0 || out == OUT_MAX)
      {
        outSettlingState = 0;
        outSettlingCounter = 0;
      }
      else
      {
        if (outSettlingState == 0)
        {
          outSettlingCounter++;
          
          int16_t outDiff = (int16_t) (out - outOld);
          
          if (outDiff > 25)
          {
            outSettlingCounter = 0;
          }
          
          if (outDiff < -25)
          {
            outSettlingCounter = 0;
          }
          
          if (outSettlingCounter > 180)
          {
            outSettlingState = 1;
          }
        }
      }
      
      if (outSettlingState == 0)
      {
        out_avg = out;
      }
      else
      {
        if (out > out_avg)
        {
          if (out < out_avg + sleep_kp_value)
          {
            out_avg_counter++;
          }
          //~ out_avg_counter++;
        }
        else if (out < out_avg)
        {
          out_avg_counter--;
        }
        
        if (out_avg_counter > 10)
        {
          if (out_avg < 800)
          {
            out_avg++;
          }
          
          out_avg_counter = 0;
        }
        else if (out_avg_counter < -3)
        {
          if (out_avg > 0)
          {
            out_avg--;
          }
          
          out_avg_counter = 0;
        }
        
        if (sleep_state == 0)
        {
          sleep_counter++;
        }
        
        if (out > out_avg + sleep_kp_value)
        {
          sleep_counter = 0;
          
          if (sleep_state > 0)
          {
            tSetBuff = 330;
            tSetBuffFlag++;
            
            sleep_state = 0;
          }
        }
        
        if (sleep_counter > 2350)
        {
          tSetBuff = 200;
          tSetBuffFlag++;
          
          sleep_counter = 0;
          
          sleep_state++;
        }
      }
      
      //~ if (adc_counter > 4)
      //~ {
        //~ out_data[out_data_pointer] = (uint8_t) (out >> 2);
        
        //~ out_data_pointer++;
        //~ out_data_pointer &= 31;
        
        //~ out_avg = 0;
        
        //~ for (uint8_t i = 0; i < 32; i++)
        //~ {
          //~ out_avg += out_data[i];
        //~ }
        
        //~ out_avg >>= 3;
      //~ }
      
      //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
      
      if (adc_counter > 8)
      {
        adc_counter = 0;
        
        if (debugDataEnable > 0)
        {
          DebugAppendData(0x8d);
          
          DebugAppendData((uint8_t) (cartridge_temperature >> 8));
          DebugAppendData((uint8_t) (cartridge_temperature));
          
          uint32_t value = *(uint32_t*) &tAct;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          value = *(uint32_t*) &tSet;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          value = *(uint32_t*) &err;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          value = *(uint32_t*) &errSum;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          value = *(uint32_t*) &outOrig;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          DebugAppendData((uint8_t) ((out & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (out & 0x00FF));
          
          DebugAppendData((uint8_t) ((out_avg & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (out_avg & 0x00FF));
          
          DebugAppendData((uint8_t) ((sleep_counter & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (sleep_counter & 0x00FF));
          
          DebugSendData();
        }
        else
        {
          DebugAppendData(0x8c);
          
          uint32_t value = *(uint32_t*) &tAct;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          value = *(uint32_t*) &tSet;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          DebugSendData();
        }
      }
      else
      {
        adc_counter++;
      }
      break;
    
    default:
      break;
  }
  
  
  //~ if (adc_channel < 100)
  //~ {
    //~ DebugAppendData(0x11);
    //~ DebugAppendData(adc_channel);
    //~ DebugAppendData((uint8_t) (adc_value >> 8));
    //~ DebugAppendData((uint8_t) (adc_value));
    
    //~ DebugSendData();
  //~ }
  //~ else
  //~ {
    
  //~ }
  
  adc_channel = 0;
}

ISR(TCA0_CMP1_vect)
{
  TCA0.SINGLE.INTFLAGS = TCA_SINGLE_CMP1_bm;
  
  VREF.CTRLA = VREF_ADC0REFSEL_1V5_gc | VREF_DAC0REFSEL_0V55_gc;
  ADC0.MUXPOS = ADC_MUXPOS_CARTRIDGE_TEMPERATURE;
  _delay_us(10);
  ADC0.COMMAND = ADC_STCONV_bm;
  adc_channel = 100;
}

int main(void)
{
  CPU_CCP = CCP_IOREG_gc;
  //~ CLKCTRL.MCLKCTRLB = CLKCTRL_PDIV_4X_gc | CLKCTRL_PEN_bm;
  CLKCTRL.MCLKCTRLB = 0;
  
  /*
   * PA2: power_mosfet
   * PA4: cartridge_resistance_measure (AIN4)
   * PA7: cartridge_current_measure (AIN7)
   */
  
  PORTA.DIR = 0b00000100;
  PORTA.OUT = 0b00000000;
  //~ PORTA.INTFLAGS = ;
  //~ PORTA.PIN0CTRL = ;
  //~ PORTA.PIN1CTRL = ;
  //~ PORTA.PIN2CTRL = ;
  //~ PORTA.PIN3CTRL = ;
  PORTA.PIN4CTRL = PORT_ISC_INPUT_DISABLE_gc;
  //~ PORTA.PIN5CTRL = ;
  //~ PORTA.PIN6CTRL = ;
  PORTA.PIN7CTRL = PORT_ISC_INPUT_DISABLE_gc;
  
  /*
   * PB2: TxD
   * PB3: RxD
   * PB4: v_in_measure (AIN9)
   * PB5: cartridge_temperature_measure (AIN8)
   */
  
  PORTB.DIR = 0b00000100;
  PORTB.OUT = 0b00001000;
  //~ PORTB.INTFLAGS = ;
  //~ PORTB.PIN0CTRL = ;
  //~ PORTB.PIN1CTRL = ;
  //~ PORTB.PIN2CTRL = ;
  //~ PORTB.PIN3CTRL = ;
  PORTB.PIN4CTRL = PORT_ISC_INPUT_DISABLE_gc;
  PORTB.PIN5CTRL = PORT_ISC_INPUT_DISABLE_gc;
  //~ PORTB.PIN6CTRL = ;
  //~ PORTB.PIN7CTRL = ;
  
  /*
   * PC0: measure_mosfet
   */
  
  PORTC.DIR = 0b00000001;
  PORTC.OUT = 0b00000000;
  //~ PORTC.INTFLAGS = ;
  //~ PORTC.PIN0CTRL = ;
  //~ PORTC.PIN1CTRL = ;
  //~ PORTC.PIN2CTRL = ;
  //~ PORTC.PIN3CTRL = ;
  //~ PORTC.PIN4CTRL = ;
  //~ PORTC.PIN5CTRL = ;
  
  // configure USART
  
  USART0.BAUD = DEF_BAUD;
  USART0.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_SBMODE_1BIT_gc | USART_CHSIZE_8BIT_gc;
  USART0.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_RXMODE_NORMAL_gc;
  USART0.CTRLA = USART_RXCIE_bm | USART_RS485_OFF_gc;
  
  //~ while (RTC.STATUS > 0) {} // Wait for all register to be synchronized
  
  //~ RTC.CLKSEL = 0b00000000;
  //~ //RTC.CNTL = 0;
  //~ //RTC.CNTH = 0;
  //~ //RTC.CMPL = 10;
  //~ //RTC.CMPH = 0;
  //~ RTC.PERL = 0;
  //~ RTC.PERH = 134;
  //~ RTC.INTCTRL = 0b00000010;
  //~ RTC.CTRLA = 0b10000001;
  
  // configure VREF
  
  VREF.CTRLA = VREF_ADC0REFSEL_0V55_gc | VREF_DAC0REFSEL_0V55_gc;
  VREF.CTRLB = VREF_ADC0REFEN_bm | VREF_DAC0REFEN_bm;
  
  // configure ADC
  
  ADC0.CTRLB = ADC_SAMPNUM_ACC1_gc;
  ADC0.CTRLC = ADC_REFSEL_INTREF_gc | ADC_PRESC_DIV256_gc;
  ADC0.CTRLD = ADC_INITDLY_DLY0_gc;
  ADC0.CTRLE = ADC_WINCM_NONE_gc;
  ADC0.MUXPOS = ADC_MUXPOS_CARTRIDGE_RESISTANCE;
  ADC0.INTCTRL = ADC_RESRDY_bm;
  ADC0.CTRLA = ADC_ENABLE_bm;
  
  // configure DAC
  
  DAC0.DATA = 112;
  DAC0.CTRLA = DAC_RUNSTDBY_bm |DAC_ENABLE_bm;
  
  // configure AC0
  
  AC0.MUXCTRLA = AC_MUXPOS_PIN0_gc | AC_MUXNEG_DAC_gc;
  AC0.CTRLA = AC_RUNSTDBY_bm | AC_ENABLE_bm;
  //~ AC0.CTRLA = AC_RUNSTDBY_bm | AC_OUTEN_bm | AC_LPMODE_bm | AC_ENABLE_bm;
  
  // configure TCA0
  
  TCA0.SINGLE.CTRLB = TCA_SINGLE_WGMODE_SINGLESLOPE_gc;
  TCA0.SINGLE.INTCTRL = TCA_SINGLE_CMP1EN_bm;
  TCA0.SINGLE.CNT = 0;
  TCA0.SINGLE.PER = 976;
  TCA0.SINGLE.CMP0BUF = 0;
  TCA0.SINGLE.CMP1BUF = 900;
  
  // configure TCB0
  
  TCB0.CTRLB = TCB_ASYNC_bm | TCB_CCMPEN_bm | TCB_CNTMODE_SINGLE_gc;
  TCB0.EVCTRL = TCB_CAPTEI_bm;
  TCB0.CCMP = 50;
  TCB0.CNT = 0;
  
  // configure CCL
  
  CCL.SEQCTRL0 = CCL_SEQSEL_RS_gc;
  
  //~ CCL.LUT0CTRLB = CCL_INSEL1_AC0_gc | CCL_INSEL0_TCA0_gc;
  //~ CCL.TRUTH0 = 0b00001010;
  //~ CCL.LUT0CTRLB = CCL_INSEL1_TCB0_gc;
  //~ CCL.LUT0CTRLB = CCL_INSEL0_TCA0_gc;
  CCL.LUT0CTRLB = CCL_INSEL1_TCB0_gc | CCL_INSEL0_TCA0_gc;
  CCL.LUT0CTRLC = CCL_INSEL2_FEEDBACK_gc;
  CCL.TRUTH0 = 0b00000010;
  CCL.LUT0CTRLA = CCL_EDGEDET_bm | CCL_ENABLE_bm;
  
  //~ CCL.LUT1CTRLB = CCL_INSEL1_AC0_gc | CCL_INSEL0_TCA0_gc;
  //~ CCL.TRUTH1 = 0b00001101;
  CCL.LUT1CTRLB = CCL_INSEL1_AC0_gc | CCL_INSEL0_TCA0_gc;
  CCL.TRUTH1 = 0b00001101;
  //~ CCL.LUT1CTRLA = CCL_EDGEDET_bm | CCL_FILTSEL_SYNCH_gc | CCL_ENABLE_bm;
  CCL.LUT1CTRLA = CCL_ENABLE_bm;
  
  CCL.CTRLA = CCL_ENABLE_bm;
  
  // configure EVSYS
  
  EVSYS.ASYNCCH0 = EVSYS_ASYNCCH0_CCL_LUT0_gc;
  EVSYS.ASYNCCH1 = EVSYS_ASYNCCH1_AC0_OUT_gc;
  
  EVSYS.ASYNCUSER_EVOUT0 = EVSYS_ASYNCUSER_ASYNCCH0_gc;
  EVSYS.ASYNCUSER_TCB0 = EVSYS_ASYNCUSER_ASYNCCH1_gc;
  //~ EVSYS.ASYNCUSER_CCL_LUT0EV0 = EVSYS_ASYNCUSER_ASYNCCH2_gc;
  
  PORTMUX.CTRLA = PORTMUX_EVOUT0_bm;
  
  sei();
  
  TCB0.CTRLA = TCB_ENABLE_bm;
  TCA0.SINGLE.CTRLA = TCA_SINGLE_CLKSEL_DIV1024_gc | TCA_SINGLE_ENABLE_bm;
  
  //~ _delay_ms(2000);
  
  //~ DebugAppendData(0xab);
  //~ DebugAppendData(0xcd);
  //~ DebugAppendData(0xef);
  //~ DebugSendData();
  
  uint32_t value;
  
  while(1)
  {
    /*
     * debug protocol: 0xff, 0xf1, [length_of_data], [data]
     */
    
    if (debugRxStateMachine == 0x04)
    {
      switch (debugRxBuffer[0])
      {
        //~ case 0x10:
          //~ DebugAppendData(debugRxBuffer[0]);
          
          //~ if (adc_channel == 0)
          //~ {
            //~ switch (debugRxBuffer[1])
            //~ {
              //~ case 1:
                //~ VREF.CTRLA = VREF_ADC0REFSEL_0V55_gc | VREF_DAC0REFSEL_0V55_gc;
                //~ ADC0.MUXPOS = ADC_MUXPOS_CARTRIDGE_RESISTANCE;
                //~ _delay_us(10);
                //~ ADC0.COMMAND = ADC_STCONV_bm;
                //~ adc_channel = 1;
                //~ break;
              
              //~ case 2:
                //~ VREF.CTRLA = VREF_ADC0REFSEL_0V55_gc | VREF_DAC0REFSEL_0V55_gc;
                //~ ADC0.MUXPOS = ADC_MUXPOS_CARTRIDGE_CURRENT;
                //~ _delay_us(10);
                //~ ADC0.COMMAND = ADC_STCONV_bm;
                //~ adc_channel = 2;
                //~ break;
              
              //~ case 3:
                //~ VREF.CTRLA = VREF_ADC0REFSEL_0V55_gc | VREF_DAC0REFSEL_0V55_gc;
                //~ ADC0.MUXPOS = ADC_MUXPOS_CARTRIDGE_TEMPERATURE;
                //~ _delay_us(10);
                //~ ADC0.COMMAND = ADC_STCONV_bm;
                //~ adc_channel = 3;
                //~ break;
              
              //~ case 4:
                //~ VREF.CTRLA = VREF_ADC0REFSEL_4V34_gc | VREF_DAC0REFSEL_0V55_gc;
                //~ ADC0.MUXPOS = ADC_MUXPOS_V_IN;
                //~ _delay_us(10);
                //~ ADC0.COMMAND = ADC_STCONV_bm;
                //~ adc_channel = 4;
                //~ break;
              
              //~ default:
                //~ break;
            //~ }
            
            //~ DebugAppendData(adc_channel);
          //~ }
          //~ else
          //~ {
            //~ DebugAppendData(255);
          //~ }
          //~ break;
        
        //~ case 0x20:
          //~ DebugAppendData(debugRxBuffer[0]);
          
          //~ if (debugRxBuffer[1] > 0)
          //~ {
            //~ PORTC.OUTSET = 0b00000001;
            
            //~ DebugAppendData(1);
          //~ }
          //~ else
          //~ {
            //~ PORTC.OUTCLR = 0b00000001;
            
            //~ DebugAppendData(0);
          //~ }
          //~ break;
        
        case 0x33:
          DebugAppendData(debugRxBuffer[0]);
          
          debugDataEnable = 1;
          break;
        
        case 0x34:
          DebugAppendData(debugRxBuffer[0]);
          
          debugDataEnable = 0;
          break;
        
        case 0x91:
          DebugAppendData(debugRxBuffer[0]);
          
          uint16_t receivedTemperatureSet = (uint16_t) (debugRxBuffer[1] * 256 + debugRxBuffer[2]);
          
          if (receivedTemperatureSet > 400)
          {
            receivedTemperatureSet = 400;
          }
          
          tSetBuff = (float) receivedTemperatureSet;
          tSetBuffFlag++;
          
          value = *(uint32_t*) &tSetBuff;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          break;
        
        case 0xa0:
          DebugAppendData(debugRxBuffer[0]);
          
          value = ((uint32_t) debugRxBuffer[1]) << 24 | ((uint32_t) debugRxBuffer[2]) << 16 | ((uint32_t) debugRxBuffer[3]) << 8 | ((uint32_t) debugRxBuffer[4]);
          controllerKpBuff = *(float*) &value;
          controllerKpBuffFlag++;
          
          value = *(uint32_t*) &controllerKpBuff;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          break;
        
        case 0xa1:
          DebugAppendData(debugRxBuffer[0]);
          
          value = ((uint32_t) debugRxBuffer[1]) << 24 | ((uint32_t) debugRxBuffer[2]) << 16 | ((uint32_t) debugRxBuffer[3]) << 8 | ((uint32_t) debugRxBuffer[4]);
          controllerKiBuff = *(float*) &value;
          controllerKiBuffFlag++;
          
          value = *(uint32_t*) &controllerKiBuff;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          break;
        
        case 0xa2:
          DebugAppendData(debugRxBuffer[0]);
          
          value = ((uint32_t) debugRxBuffer[1]) << 24 | ((uint32_t) debugRxBuffer[2]) << 16 | ((uint32_t) debugRxBuffer[3]) << 8 | ((uint32_t) debugRxBuffer[4]);
          controllerKdBuff = *(float*) &value;
          controllerKdBuffFlag++;
          
          value = *(uint32_t*) &controllerKdBuff;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          break;
        
        case 0xa3:
          DebugAppendData(debugRxBuffer[0]);
          
          value = ((uint32_t) debugRxBuffer[1]) << 24 | ((uint32_t) debugRxBuffer[2]) << 16 | ((uint32_t) debugRxBuffer[3]) << 8 | ((uint32_t) debugRxBuffer[4]);
          controllerKitBuff = *(float*) &value;
          controllerKitBuffFlag++;
          
          value = *(uint32_t*) &controllerKitBuff;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          break;
        
        case 0xf0:
          DebugAppendData(debugRxBuffer[0]);
          
          value = *(uint32_t*) &tSet;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          value = *(uint32_t*) &controllerKp;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          value = *(uint32_t*) &controllerKi;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          value = *(uint32_t*) &controllerKd;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          
          value = *(uint32_t*) &controllerKit;
          DebugAppendData((uint8_t) ((value & 0xFF000000) >> 24));
          DebugAppendData((uint8_t) ((value & 0xFF0000) >> 16));
          DebugAppendData((uint8_t) ((value & 0xFF00) >> 8));
          DebugAppendData((uint8_t) (value & 0x00FF));
          break;
        
        default:
          DebugAppendData(0xff);
          
          DebugAppendData(0xde);
          DebugAppendData(0xad);
          DebugAppendData(0xbe);
          DebugAppendData(0xef);
          break;
      }
      
      DebugSendData();
      
      debugRxFrameLength = 0;
      debugRxStateMachine = 0x00;
    }
  }
}