extern int square1_period = 0;
int square2_period = 0;
int square3_period = 0;
int square4_period = 0;


extern unsigned char square1_amplitude = 0;
unsigned char square2_amplitude = 0;
unsigned char square3_amplitude = 0;
unsigned char square4_amplitude = 0;

unsigned char noise_amplitude = 0;

extern int square1_id = 0;
int square2_id = 0;
int square3_id = 0;
int square4_id = 0;

#define wave(amplitude, period, id)                   \
      asm volatile (                                         \
    	  "lds r24, " period "\n" /* load period low */        \
	      "lds r25, (" period ")+1\n" /* load period high */   \
        "lds r30, " id "\n" /* load id low */                \
        "lds r31, (" id ")+1\n" /* load id high */           \
        "lds r28, " amplitude "\n" /* load amplitude */      \
                                                             \
        /* if (id < period) goto .do_not_reset_<something> */\
        "cp r30, r24\n"                                      \
        "cpc r31, r25\n"                                     \
        "brlt .do_not_reset_%=\n"                            \
        /* Reset id and go to the end of the function */     \
        "ldi r30,0\n" /* id = 0 */                           \
        "ldi r31,0\n"                                        \
        "sts " id ",r30\n"                                   \
        "sts (" id ")+1,r31\n"                               \
        "rjmp .end_%=\n"                                     \
        ".do_not_reset_%=:\n"                                \
                                                             \
        /* if (id >= period/2) goto .Lb_<something> */       \
        "lsr r25\n"                                          \
        "ror r24\n"                                          \
        "cp r30, r24\n"                                      \
        "cpc r31, r25\n"                                     \
        "brge .Lb_%=\n"                                      \
                                                             \
        /* Add the amplitude */                              \
        "add r23, r28\n"                                     \
                                                             \
        ".Lb_%=:\n"                                          \
                                                             \
        /* out -= square_amplitude/2 */                      \
        "lsr r28\n" /* logical shift right = >> 1 */         \
        "sub r23, r28\n"                                     \
                                                             \
        /* square_id += 1 */                                 /*
        // You can also use : 
        "ldi r24, 1\n"                                       \
        "ldi r25, 0\n"                                       \
        "add r30,r24\n"                                      \
        "adc r31,r25\n"                                      \*/\
        "adiw r30, 1\n"                                      \
        "sts " id ",r30\n"                                   \
        "sts (" id ")+1,r31\n"                               \
                                                             \
        ".end_%=:\n"                                         \
        ::)

ISR(TIMER2_OVF_vect, ISR_NAKED)
{	
  asm volatile (
    "push r30\n" //square_id low
    "push r31\n" //square_id high
    "push r28\n" //square_amplitude
    "push r23\n" //out
    "push r24\n" //square_period low
    "push r25\n" //square_period high
    "in r23, __SREG__\n"
    "push r23\n"
    
    "ldi r23, lo8(128)\n" //out=128
    );
    asm volatile (
    //Update OCR2A with the out(r23) value calculated
    "ldi r30,lo8(179)\n"
    "ldi r31,hi8(179)\n"
    "st Z, r23\n"
    
    //pop saved registers
    "pop r23\n"
    "out __SREG__, r23\n"
    "pop r25\n"
    "pop r24\n"
    "pop r23\n"
    "pop r28\n"
    "pop r31\n"
    "pop r30\n");
  reti();
                
}
  
void setup() {
  // put your setup code here, to run once:

}

void loop() {
  // put your main code here, to run repeatedly:
  wave("square1_amplitude","square1_period", "square1_id");
//---------------

}