/*
 * 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	NumPixels 24
#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

uint8_t pixels[NumPixels][NumColors];

void __attribute__ ((noinline))send_pixels(uint16_t num, uint8_t *ppixels) {
  static uint32_t endsend=0;
  while ((micros()-endsend)<300);
  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"
  );
  endsend=micros();
}

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)
  );
}

uint8_t print(int8_t  x) {
  x%=NumPixels;
  return (x<0?x+NumPixels:x);
};

void loop() {
  uint8_t p;
  for (uint8_t x=0;x<NumPixels;x++) {
    p=print(x-3);
    pixels[p][0]=0;
    pixels[p][1]=0;
    pixels[p][2]=0;
    p=print(x-2);
    pixels[p][0]=255;
    pixels[p][1]=0;
    pixels[p][2]=0;
    p=print(x-1);
    pixels[p][0]=0;
    pixels[p][1]=0;
    pixels[p][2]=255;
    p=print(x);
    pixels[p][0]=0;
    pixels[p][1]=255;
    pixels[p][2]=0;
    p=print(x+1);
    pixels[p][0]=0;
    pixels[p][1]=0;
    pixels[p][2]=255;
    p=print(x+2);
    pixels[p][0]=255;
    pixels[p][1]=0;
    pixels[p][2]=0;
    send_pixels(NumPixels*NumColors, *pixels);
    delay(100);
  };
}