/*
 * Copyright (C) 2022, Andrei Egorov
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
 
#define	NumColors 3
#define DO_PIN 13

#if DO_PIN > 19
  #error !!! DO_PIN must be less than 20 !!!
#endif
#define DOPORT (DO_PIN>=0 && DO_PIN<=7) ? 0x0B : (DO_PIN>=8 && DO_PIN<=13) ? 0x05 : (DO_PIN>=14 && DO_PIN<=19) ? 0x08 : 0
#define DOPIN (DO_PIN>=0 && DO_PIN<=7) ? DO_PIN : (DO_PIN>=8 && DO_PIN<=13) ? DO_PIN - 8 : (DO_PIN>=14 && DO_PIN<=19) ? DO_PIN - 14 : 0

volatile boolean interrupt_called (false);
void check_out () {
  detachInterrupt(0);
  interrupt_called=true;
};

void __attribute__ ((noinline))send_pixels(uint16_t num, uint8_t *ppixels) {
  asm volatile(
    "IN   __tmp_reg__,%0 \n\t"
    "PUSH __tmp_reg__ \n\t"
    "CLI \n\t"
		"LDI  r19,1<<%3 \n\t"
    "LDI  r18,2 \n\t"
    "LD   __tmp_reg__,%a1+ \n\t"
    "label_1: \n\t"
    "BST  __tmp_reg__,7 \n\t"
    "OUT  %2-2,r19 \n\t"
    "BRTC label_2 \n\t"
    "ROL  __tmp_reg__ \n\t"
    "LSL  r18 \n\t"
    "RJMP .+0\n\t"
    "RJMP .+0\n\t"  
    "BRCS label_3 \n\t"
    "NOP \n\t"
    "OUT  %2-2,r19 \n\t"
    "RJMP label_1 \n\t"
    "label_2: \n\t"
    "BST  __tmp_reg__,7 \n\t"
    "OUT  %2-2,r19 \n\t"
    "ROL  __tmp_reg__ \n\t"
    "LSL  r18 \n\t"
    "RJMP .+0\n\t"
    "RJMP .+0\n\t"
    "BRCC label_1 \n\t"
    "RJMP label_4 \n\t"
    "label_3: \n\t"
    "OUT  %2-2,r19 \n\t"
    "RJMP .+0\n\t"
    "BST  __tmp_reg__,7 \n\t"
    "label_4: \n\t"
    "OUT  %2-2,r19 \n\t"
    "BRTC label_5 \n\t"
    "RJMP .+0\n\t"
    "LDI  r18,2 \n\t"
    "SBIW %4,1 \n\t"
    "BREQ label_6 \n\t"
    "LD   __tmp_reg__,%a1+ \n\t"
    "OUT  %2-2,r19 \n\t"
    "RJMP label_1 \n\t"
    "label_5: \n\t"
    "LDI  r18,2 \n\t"
    "OUT  %2-2,r19 \n\t"
    "SBIW %4,1 \n\t"
    "LD   __tmp_reg__,%a1+ \n\t"
    "RJMP .+0\n\t"
    "BRNE label_1 \n\t"
    "RJMP label_7 \n\t"
    "label_6: \n\t"
    "NOP \n\t"
    "OUT  %2-2,r19 \n\t"
    "label_7: \n\t"
    "POP  __tmp_reg__ \n\t"
    "OUT  %0,__tmp_reg__\n\t"
    ::"I" (_SFR_IO_ADDR(SREG)), "e" (ppixels), "I" (_SFR_IO_ADDR(_SFR_IO8(DOPORT))), "M" (DOPIN), "w" (num):"r18","r19"
  );
}

void setup () {
  asm volatile(
    "SBI %0-1,%1 \n\t"
    "CBI %0,%1 \n\t"
    ::"I" (_SFR_IO_ADDR(_SFR_IO8(DOPORT))), "I" (DOPIN)//, "r" (DO)
  );
  attachInterrupt(0, check_out, CHANGE);
  uint8_t pixel[3]={0,0,0};
  uint16_t p(0);
  do {
    send_pixels(NumColors, pixel);
    p++;
  } while (!interrupt_called);
  Serial.begin(9600);
  Serial.println(p-1);
  Serial.end();
}

void loop() {
}