#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif


// Adjustment FREQ
#define BIAS_FREQ   130



#define voices                  4
#define nokey                 255
#define shift_tone_sector_0    0
#define shift_tone_sector_1    16
#define shift_tone_sector_2    32

#define RANGE_SEL        (RANGE << 12)
#define GATE_MODE_SEL    ((1 << GATE_MODE) << 13)GATE_MODE_SEL

#define KBD_PIN          PIND
#define KBD_PIN_B        PINB
#define KBD_DDR          DDRD
#define KBD_PORT         PORTD
#define OUT_PORT         PORTB
#define GATE_PORT        PORTB
#define SPI_PORT         PORTB

#define OUT_DCO1_SQR     OCR1A
#define OUT_DCO1_TRI     OCR1B
#define OUT_DCO2_SQR     OCR0A
#define OUT_DCO2_TRI     OCR0B

//#define LFO_1            REG_16[0]
//#define LFO_FREQ         REG_16[1]
//#define DETUNE           REG_16[2]
//#define RELEASE_VCA      REG_16[3]
//#define LFO_1            REG_16[4]
//#define LFO_2            REG_16[5]
//#define ATTACK_VCA       REG_16[6]
//#define LFO_2            REG_16[7]
#define AMP_VCA          0x0040
#define GATE_PIN         0x0005
#define GATE_ENABLE      0x0001
#define GATE_WIDTH       0x07D0
#define KBD_PIN_MASK     0x000F  //0b00001111
#define KBD_SHIFT        0x0000
#define SECTOR_0_PIN     0x0004
#define SECTOR_1_PIN     0x0007
#define SECTOR_2_PIN     0x0000
#define KEY_NOTE_ST      0x0000  // SECTOR 2:  [G#3 - F#4]
#define KEY_NOTE_END     0x000A  // SECTOR 2
#define KEY_FORM_DCO_1   0x000B
#define KEY_FORM_DCO_2   0x000C
#define KEY_FORM_LFO     0x000D
#define KEY_RANGE_DCO_1  0x000E
#define KEY_ENV_MODE     0x000F
#define SPI_DATA_PIN     0x0003
#define SPI_LATCH_PIN    0x0004

int8_t   LFO_1 = 0;
int8_t   LFO_2 = 0;
int8_t   LFO_1_pred  = 0;
int8_t   LFO_2_pred  = 0;
uint16_t   DETUNE      = 0;
uint16_t   DETUNE_pred = 0;
int8_t   FORM_1[256];
int8_t   FORM_2[256];
int8_t   FORM_LFO[256];
uint8_t  keypressed;
uint8_t  keypressed_pred;
uint8_t  keyreleased;
uint8_t  KBD;
uint8_t  RANGE;
uint8_t  GATE_MODE;
uint8_t  SPI_clk;
uint8_t  sel_1;
uint8_t  sel_2;
uint8_t  sel_lfo;
uint8_t  REG_16[8];
uint8_t  AMP_1[voices];
uint8_t  AMP_2[voices];
uint8_t  iADSR[voices];
uint8_t  amp_base[voices];
uint8_t  keych[voices];
uint8_t  tch[voices];
uint16_t KBD_state_0;
uint16_t KBD_state_1;
uint16_t KBD_state_2;
uint16_t SPI_data;
uint16_t SPI_data_pred;
uint16_t LFO_OSC;
uint16_t OSC_1;
uint16_t OSC_2;
uint16_t FREQ_1;
uint16_t FREQ_2;
uint16_t system_clk;
uint16_t clk_gate;

//                         C      C#     D      D#     E      F      F#     G      G#     A      A#     H
uint16_t tone_inc[96] = { 34,    36,    38,    41,    43,    46,    48,    51,    54,    58,    61,    65,    //  0
                          69,    73,    77,    82,    86,    92,    97,    103,   109,   115,   122,   129,   //  1
                          137,   145,   154,   163,   173,   183,   194,   206,   218,   231,   244,   259,   //  2
                          274,   291,   308,   326,   346,   366,   388,   411,   435,   461,   489,   518,   //  3
                          549,   581,   616,   652,   691,   732,   776,   822,   871,   923,   978,   1036,  //  4
                          1097,  1163,  1232,  1305,  1383,  1465,  1552,  1644,  1742,  1845,  1955,  2071,  //  5
                          2195,  2325,  2463,  2610,  2765,  2930,  3104,  3288,  3484,  3691,  3910,  4143,  //  6
                          4389,  4650,  4927,  5220,  5530,  5859,  6207,  6577,  6968,  7382,  7821,  8286,  //  7
                        };
/*
  int8_t FORM_SAW[256] = {
  0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x8B, 0x8C, 0x8D, 0x8E, 0x8F,
  0x90, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A, 0x9B, 0x9C, 0x9D, 0x9E, 0x9F,
  0xA0, 0xA1, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8, 0xA9, 0xAA, 0xAB, 0xAC, 0xAD, 0xAE, 0xAF,
  0xB0, 0xB1, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xBB, 0xBC, 0xBD, 0xBE, 0xBF,
  0xC0, 0xC1, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xCB, 0xCC, 0xCD, 0xCE, 0xCF,
  0xD0, 0xD1, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9, 0xDA, 0xDB, 0xDC, 0xDD, 0xDE, 0xDF,
  0xE0, 0xE1, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9, 0xEA, 0xEB, 0xEC, 0xED, 0xEE, 0xEF,
  0xF0, 0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA, 0xFB, 0xFC, 0xFD, 0xFE, 0xFF,
  0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
  0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1A, 0x1B, 0x1C, 0x1D, 0x1E, 0x1F,
  0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x2B, 0x2C, 0x2D, 0x2E, 0x2F,
  0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x3B, 0x3C, 0x3D, 0x3E, 0x3F,
  0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x4B, 0x4C, 0x4D, 0x4E, 0x4F,
  0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5A, 0x5B, 0x5C, 0x5D, 0x5E, 0x5F,
  0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6A, 0x6B, 0x6C, 0x6D, 0x6E, 0x6F,
  0x70, 0x71, 0x72, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A, 0x7B, 0x7C, 0x7D, 0x7E, 0x7F
  };

  int8_t FORM_RAW[256] = {
  0x7F, 0x7E, 0x7D, 0x7C, 0x7B, 0x7A, 0x79, 0x78, 0x77, 0x76, 0x75, 0x74, 0x73, 0x72, 0x71, 0x70,
  0x6F, 0x6E, 0x6D, 0x6C, 0x6B, 0x6A, 0x69, 0x68, 0x67, 0x66, 0x65, 0x64, 0x63, 0x62, 0x61, 0x60,
  0x5F, 0x5E, 0x5D, 0x5C, 0x5B, 0x5A, 0x59, 0x58, 0x57, 0x56, 0x55, 0x54, 0x53, 0x52, 0x51, 0x50,
  0x4F, 0x4E, 0x4D, 0x4C, 0x4B, 0x4A, 0x49, 0x48, 0x47, 0x46, 0x45, 0x44, 0x43, 0x42, 0x41, 0x40,
  0x3F, 0x3E, 0x3D, 0x3C, 0x3B, 0x3A, 0x39, 0x38, 0x37, 0x36, 0x35, 0x34, 0x33, 0x32, 0x31, 0x30,
  0x2F, 0x2E, 0x2D, 0x2C, 0x2B, 0x2A, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x20,
  0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11, 0x10,
  0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00,
  0xFF, 0xFE, 0xFD, 0xFC, 0xFB, 0xFA, 0xF9, 0xF8, 0xF7, 0xF6, 0xF5, 0xF4, 0xF3, 0xF2, 0xF1, 0xF0,
  0xEF, 0xEE, 0xED, 0xEC, 0xEB, 0xEA, 0xE9, 0xE8, 0xE7, 0xE6, 0xE5, 0xE4, 0xE3, 0xE2, 0xE1, 0xE0,
  0xDF, 0xDE, 0xDD, 0xDC, 0xDB, 0xDA, 0xD9, 0xD8, 0xD7, 0xD6, 0xD5, 0xD4, 0xD3, 0xD2, 0xD1, 0xD0,
  0xCF, 0xCE, 0xCD, 0xCC, 0xCB, 0xCA, 0xC9, 0xC8, 0xC7, 0xC6, 0xC5, 0xC4, 0xC3, 0xC2, 0xC1, 0xC0,
  0xBF, 0xBE, 0xBD, 0xBC, 0xBB, 0xBA, 0xB9, 0xB8, 0xB7, 0xB6, 0xB5, 0xB4, 0xB3, 0xB2, 0xB1, 0xB0,
  0xAF, 0xAE, 0xAD, 0xAC, 0xAB, 0xAA, 0xA9, 0xA8, 0xA7, 0xA6, 0xA5, 0xA4, 0xA3, 0xA2, 0xA1, 0xA0,
  0x9F, 0x9E, 0x9D, 0x9C, 0x9B, 0x9A, 0x99, 0x98, 0x97, 0x96, 0x95, 0x94, 0x93, 0x92, 0x91, 0x90,
  0x8F, 0x8E, 0x8D, 0x8C, 0x8B, 0x8A, 0x89, 0x88, 0x87, 0x86, 0x85, 0x84, 0x83, 0x82, 0x81, 0x80
  };

  uint8_t FORM_SQR[256] = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,

  0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
  0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
  0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
  0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
  0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
  0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
  0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
  0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
  };

  int8_t FORM_SIN[256] = {
  0x00, 0x03, 0x06, 0x09, 0x0C, 0x10, 0x13, 0x16, 0x19, 0x1C, 0x1F, 0x22, 0x25, 0x28, 0x2B, 0x2E,
  0x31, 0x34, 0x36, 0x39, 0x3C, 0x3F, 0x42, 0x44, 0x47, 0x49, 0x4C, 0x4E, 0x51, 0x53, 0x56, 0x58,
  0x5A, 0x5C, 0x5E, 0x60, 0x62, 0x64, 0x66, 0x68, 0x6A, 0x6C, 0x6D, 0x6F, 0x70, 0x72, 0x73, 0x74,
  0x76, 0x77, 0x78, 0x79, 0x7A, 0x7B, 0x7B, 0x7C, 0x7D, 0x7D, 0x7E, 0x7E, 0x7E, 0x7F, 0x7F, 0x7F,
  0x7F, 0x7F, 0x7F, 0x7F, 0x7E, 0x7E, 0x7D, 0x7D, 0x7C, 0x7C, 0x7B, 0x7A, 0x79, 0x78, 0x77, 0x76,
  0x75, 0x74, 0x72, 0x71, 0x70, 0x6E, 0x6C, 0x6B, 0x69, 0x67, 0x65, 0x63, 0x61, 0x5F, 0x5D, 0x5B,
  0x59, 0x57, 0x54, 0x52, 0x50, 0x4D, 0x4B, 0x48, 0x45, 0x43, 0x40, 0x3D, 0x3B, 0x38, 0x35, 0x32,
  0x2F, 0x2C, 0x29, 0x27, 0x24, 0x20, 0x1D, 0x1A, 0x17, 0x14, 0x11, 0x0E, 0x0B, 0x08, 0x05, 0x02,
  0xFE, 0xFB, 0xF8, 0xF5, 0xF2, 0xEF, 0xEC, 0xE9, 0xE6, 0xE3, 0xE0, 0xDC, 0xD9, 0xD7, 0xD4, 0xD1,
  0xCE, 0xCB, 0xC8, 0xC5, 0xC3, 0xC0, 0xBD, 0xBB, 0xB8, 0xB5, 0xB3, 0xB0, 0xAE, 0xAC, 0xA9, 0xA7,
  0xA5, 0xA3, 0xA1, 0x9F, 0x9D, 0x9B, 0x99, 0x97, 0x95, 0x94, 0x92, 0x90, 0x8F, 0x8E, 0x8C, 0x8B,
  0x8A, 0x89, 0x88, 0x87, 0x86, 0x85, 0x84, 0x84, 0x83, 0x83, 0x82, 0x82, 0x81, 0x81, 0x81, 0x81,
  0x81, 0x81, 0x81, 0x82, 0x82, 0x82, 0x83, 0x83, 0x84, 0x85, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A,
  0x8C, 0x8D, 0x8E, 0x90, 0x91, 0x93, 0x94, 0x96, 0x98, 0x9A, 0x9C, 0x9E, 0xA0, 0xA2, 0xA4, 0xA6,
  0xA8, 0xAA, 0xAD, 0xAF, 0xB2, 0xB4, 0xB7, 0xB9, 0xBC, 0xBE, 0xC1, 0xC4, 0xC7, 0xCA, 0xCC, 0xCF,
  0xD2, 0xD5, 0xD8, 0xDB, 0xDE, 0xE1, 0xE4, 0xE7, 0xEA, 0xED, 0xF0, 0xF4, 0xF7, 0xFA, 0xFD, 0x00
  };
*/
uint8_t  stroy8bit_32[36] = {237, 223, 211, 199, 188, 177, 167, 157, 148, 140, 132, 124,
                             118, 112, 105,  99,  94,  88,  83,  79,  74,  70,  66,  62,
                             59,  56,   52,  49,  47,  44,  41,  39,  37,  35,  33,  31
                            };

uint8_t  stroy8bit_64[12]  = {237, 223, 211, 199, 188, 177, 167, 157, 148, 140, 132, 124};

uint8_t  stroy8bit_128[12] = {237, 223, 211, 199, 188, 177, 167, 157, 148, 140, 132, 124};

uint8_t  stroy8bit_256[12] = {237, 223, 211, 199, 188, 177, 167, 157, 148, 140, 132, 124};

uint16_t stroy16bit[12] = {3822, 3608, 3405, 3214,
                           3034, 2863, 2703, 2551,
                           2408, 2273, 2145, 2025
                          };
                                //1    2      3     4     5     6     7     8     9    10    11    12    13    14    15    16
uint16_t stroy16bit_pre[192] = {3822, 3820, 3806, 3792, 3778, 3764, 3750, 3736, 3722, 3708, 3694, 3680, 3666, 3652, 3638, 3624,
                                3608, 3606, 3593, 3580, 3567, 3554, 3541, 3528, 3515, 3502, 3489, 3476, 3463, 3450, 3437, 3424,
                                3405, 3403, 3391, 3379, 3367, 3355, 3343, 3331, 3319, 3307, 3295, 3283, 3271, 3259, 3247, 3235,
                                3214, 3212, 3201, 3190, 3179, 3168, 3157, 3146, 3135, 3124, 3113, 3102, 3091, 3080, 3069, 3058,

                                3034, 3032, 3021, 3010, 2999, 2988, 2977, 2966, 2955, 2944, 2933, 2922, 2911, 2800, 2889, 2878,
                                2863, 2861, 2851, 2841, 2831, 2821, 2811, 2801, 2791, 2781, 2771, 2761, 2751, 2741, 2731, 2721,
                                2703, 2701, 2691, 2681, 2671, 2661, 2651, 2641, 2631, 2621, 2611, 2601, 2591, 2581, 2571, 2561,
                                2551, 2549, 2540, 2531, 2522, 2513, 2504, 2495, 2486, 2477, 2468, 2459, 2450, 2441, 2432, 2423,

                                2408, 2406, 2397, 2388, 2379, 2370, 2361, 2352, 2343, 2334, 2325, 2316, 2307, 2298, 2289, 2280,
                                2273, 2271, 2263, 2255, 2247, 2239, 2231, 2223, 2215, 2207, 2199, 2191, 2183, 2175, 2167, 2159,
                                2145, 2143, 2134, 2125, 2116, 2107, 2098, 2089, 2080, 2071, 2062, 2053, 2044, 2035, 2026, 2017,
                                2025, 2023, 2015, 2007, 1999, 1991, 1983, 1975, 1967, 1959, 1951, 1943, 1935, 1927, 1919, 1911
                               };


inline void KEY_ON_SECTOR_0();
inline void KEY_OFF_SECTOR_0();
inline void KEY_ON_SECTOR_1();
inline void KEY_OFF_SECTOR_1();
inline void KEY_ON_SECTOR_2();
inline void KEY_OFF_SECTOR_2();

inline void setPWM() __attribute__((always_inline));
//inline void setPWM();
inline void setPWM() {
  while ((TIFR1 & 0x01) == 0);
  TIFR1 |= 0x01;

  OSC_1 += FREQ_1;
  OSC_2 += FREQ_2;

//  OUT_DCO1_SQR = FORM_SQR[OSC_1 >> 8] ;
//  OUT_DCO1_TRI = FORM_SAW[OSC_1 >> 8] + 127;
//  OUT_DCO2_SQR = FORM_SQR[OSC_2 >> 8] ;
//  OUT_DCO2_TRI = FORM_SAW[OSC_2 >> 8] + 127;
  //system_clk ++;
}

int main() {
  cli();
  DDRB  = B00111110;

  PMW_init();
  KBD_init();
  ADC_init();

  DDRD &= ~(1 << 4);
  DDRD &= ~(1 << 7);
  DDRB &= ~(1 << 0);
  DDRB |=  (1 << 1);
  DDRB |=  (1 << 2);
  DDRD |=  (1 << 5);
  DDRD |=  (1 << 6);

  TIMSK1 |= _BV(OCIE1A);

  keypressed  = 0;
  keypressed_pred = 1;
  LFO_1 = 0;
  ADMUX = 0x61;

  uint32_t tim = 0;
  //sei();
  while (1) {

    KBD_SCAN();
   /* 
    while(tim != 0) tim--; 
    tim = 0x001FFFFF;
    GATE_PORT ^=  (GATE_ENABLE << GATE_PIN);
*/
    ADCSRA |= (1 << ADSC);
    while (((ADCSRA >> ADSC) & 0x01) != 0);
    ADMUX = 0x63;
    DETUNE = ADCH;

    ADCSRA |= (1 << ADSC);
    while (((ADCSRA >> ADSC) & 0x01) != 0);
    ADMUX = 0x64;
    LFO_1 = ADCH - 127;
    LFO_1 >>= 2;

/*
    ADCSRA |= (1 << ADSC);
    while (((ADCSRA >> ADSC) & 0x01) != 0);*/
    ADMUX = 0x61;/*
    LFO_2 = ADCH - 127;
    LFO_2 >>= 2;
*/
    //DETUNE = DETUNE + LFO_1;
       
    Set_Generator();
    //keypressed++; if (keypressed > 47) keypressed = 0;
  }
  return 0;
}

inline void KEY_ON_SECTOR_0() {
  keypressed  = KBD + shift_tone_sector_0;
  GATE_ON();
}

inline void KEY_ON_SECTOR_1() {
  keypressed  = KBD + shift_tone_sector_1;
  GATE_ON();
}

inline void KEY_ON_SECTOR_2() {
  keypressed  = KBD + shift_tone_sector_2;
  GATE_ON();
}

inline void KEY_OFF_SECTOR_0() {
  keyreleased = KBD + shift_tone_sector_0;
  GATE_OFF();
}

inline void KEY_OFF_SECTOR_1() {
  keyreleased = KBD + shift_tone_sector_1;
  GATE_OFF();
}

inline void KEY_OFF_SECTOR_2() {
  keyreleased = KBD + shift_tone_sector_2;
  GATE_OFF();
}

inline void ADC_init() {
  ADCSRA |=  (1 << ADEN);
  ADCSRA |=  (1 << ADPS0);
  ADCSRA |=  (1 << ADPS1);
  ADCSRA &= ~(1 << ADPS2);
}

uint8_t ADC_scan() {
  /*ADMUX = 0x61;
    ADCSRA |= (1 << ADSC);
    while ((ADCSRA >> ADSC) & 0x01 != 1);
    DETUNE = ADCH;*/
}

inline void Set_Generator() {
  //FREQ_1 = ((uint32_t)tone_inc[keypressed] * (BIAS_FREQ +          LFO_1)) >> 4;
  //FREQ_2 = ((uint32_t)tone_inc[keypressed] * (BIAS_FREQ + DETUNE + LFO_2)) >> 4;
  if ( keypressed < 36)                       TCCR0B = 0b00000011; //64
  if ((keypressed > 35) && (keypressed < 48)) TCCR0B = 0b00000010; //8

  if (keypressed > 35) OCR0A = stroy8bit_32[keypressed - 36] >> 1;
  else                 OCR0A = stroy8bit_32[keypressed] >> 1;

  OCR1A = stroy16bit_pre[( ( (keypressed % 12) << 4) + DETUNE ) % 192] >>
                           (((keypressed       << 4) + DETUNE ) / 192) + 2;
}

inline void GATE_ON()  {
  GATE_PORT |=  (GATE_ENABLE << GATE_PIN);
}
inline void GATE_OFF() {
  GATE_PORT &= ~(GATE_ENABLE << GATE_PIN);
}
inline void KBD_init() {
  KBD_DDR  = KBD_PIN_MASK;
}

inline void KBD_SCAN() {
  KBD_PORT = (KBD << KBD_SHIFT) & KBD_PIN_MASK;

  if (((KBD_PIN >> SECTOR_0_PIN) & 0x01) == 0) {
    if (((KBD_state_0 >> KBD) & 0x01) == 0) {
      KEY_ON_SECTOR_0();
      KBD_state_0 |= (1 << KBD);
    }
  } else {
    if (((KBD_state_0 >> KBD) & 0x01) == 1) {
      KEY_OFF_SECTOR_0();
      KBD_state_0 &= ~(1 << KBD);
    }
  }

  if (((KBD_PIN >> SECTOR_1_PIN) & 0x01) == 0) {
    if (((KBD_state_1 >> KBD) & 0x01) == 0) {
      KEY_ON_SECTOR_1();
      KBD_state_1 |= (1 << KBD);
    }
  } else {
    if (((KBD_state_1 >> KBD) & 0x01) == 1) {
      KEY_OFF_SECTOR_1();
      KBD_state_1 &= ~(1 << KBD);
    }
  }

  if (((KBD_PIN_B >> SECTOR_2_PIN) & 0x01) == 0) {
    if (((KBD_state_2 >> KBD) & 0x01) == 0) {
      KEY_ON_SECTOR_2();
      KBD_state_2 |= (1 << KBD);
    }
  } else {
    if (((KBD_state_2 >> KBD) & 0x01) == 1) {
      KEY_OFF_SECTOR_2();
      KBD_state_2 &= ~(1 << KBD);
    }
  }

  KBD++;
  if (KBD > 15) KBD = 0;
}


inline void PMW_init() {
  TCNT0  = 0;
  OCR0A  = 0;
  OCR0B  = 0;
  TIMSK0 = 0;
  TCCR0A = 0;
  TCCR0B = 0;
  ASSR   = 0;
  
  TCCR1A = 0b01000000;
  TCCR1B = 0b00001010;

  TCCR0A = 0b01010010;
  TCCR0B = 0b00000011;

  //TCCR1A = 0B10100001; //8-bit fast PWM
  //TCCR1B = 0B00001001;
  //TCCR0A = 0B10100011; //8-bit fast PWM
  //TCCR0B = 0B00000001;
}