/***************************************************************************
 *   nvram parameter read/write                                            *
 *                                                                         *
 *   Copyright (C) 2011 - 2012 by Olaf Rempel                              *
 *   razzor AT kopf MINUS tisch DOT de                                     *
 *                                                                         *
 *   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; version 2 of the License,               *
 *                                                                         *
 *   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, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>

#include "rgb16mpm.h"

/* 16 values per color */
uint8_t chan_value[16][3];

/* (16 +1) * 4 values (portA, portC, OCR0, flags) per color */
static uint8_t chan_rawdata[3][17 * 4];

/* used only in softpwm ISRs */
register uint8_t * pCurrentStep asm("r2"); /* r3:r2 */

/* used to sync softpwm ISRs and color_update() */
register uint8_t nextColor asm("r4");      /* r4 */

/* worst case: 9+2+12+8+7+13 = 61 clks -> 7.625us @8MHz */
ISR(TIMER0_OVF_vect, ISR_NAKED)
{
    asm volatile(
        /* save registers 2+1+2+2+2 = 9 */
        "push r24          \n\t"
        "in r24, __SREG__  \n\t"
        "push r24          \n\t"
        "push r30          \n\t"
        "push r31          \n\t"
        ::
    );

    asm volatile(
        /* disable outputs 1+1 = 2 */
        "out %0, r1        \n\t" /* PORTA = 0x00; */
        "out %1, r1        \n\t" /* PORTC = 0x00; */
        :: "I" (_SFR_IO_ADDR(PORTA)),
           "I" (_SFR_IO_ADDR(PORTC))
    );

    asm volatile(
        /* switch color and assign pCurrentStep */                     //  R   G   B
        "mov r24, %0       \n\t"                                       //  1   1   1
        "inc %0            \n\t" /* nextColor++ */                     //  2   2   2
        "cpi r24, 1        \n\t"                                       //  3   3   3
        "brlo L_red%=      \n\t" /* if (nextColor < 1) -> red */       //  5   4   4
        "breq L_green%=    \n\t" /* if (nextColor == 1) -> green */    //  -   6   5
        "clr %0            \n\t" /* nextColor    = 0;           */     //  -   -   6
        "ldi r24, %7       \n\t" /* PORTB        = (1<<ROW4);   */     //  -   -   7
        "ldi r30, lo8(%3)  \n\t" /* pCurrentStep = &rawdata[2]; */     //  -   -   8
        "ldi r31, hi8(%3)  \n\t"                                       //  -   -   9
        "rjmp L_end%=      \n\t"                                       //  -   -  11
        "L_red%=:          \n\t" /* red:                        */
        "ldi r24, %5       \n\t" /* PORTB        = (1<<ROW1);   */     //  6   -   -
        "ldi r30, lo8(%1)  \n\t" /* pCurrentStep = &rawdata[0]; */     //  7   -   -
        "ldi r31, hi8(%1)  \n\t"                                       //  8   -   -
        "rjmp L_end%=      \n\t"                                       // 10   -   -
        "L_green%=:        \n\t" /* green:                      */
        "ldi r24, %6       \n\t" /* PORTB        = (1<<ROW2);   */     //  -   7   -
        "ldi r30, lo8(%2)  \n\t" /* pCurrentStep = &rawdata[1]; */     //  -   8   -
        "ldi r31, hi8(%2)  \n\t"                                       //  -   9   -
        "L_end%=:          \n\t"
        "out %4, r24       \n\t" /* set PORTB */                       // 11  10  12
        :: "r" (nextColor),
           "i" (&chan_rawdata[0]), /* RED    */
           "i" (&chan_rawdata[1]), /* GREEN  */
           "i" (&chan_rawdata[2]), /* BLUE   */
           "I" (_SFR_IO_ADDR(PORTB)),
           "i" ((1<<ROW1)), /* RED   */
           "i" ((1<<ROW2)), /* GREEN */
           "i" ((1<<ROW3))  /* BLUE  */
    );

    asm volatile(
        /* load table values 1+1+1+1+1+1+1+1+1 = 8 */
        "ld r24, z+        \n\t"
        "out %1, r24       \n\t" /* PORTA = *pCurrentStep++; */
        "ld r24, z+        \n\t"
        "out %2, r24       \n\t" /* PORTC = *pCurrentStep++; */
        "ld r24, z+        \n\t"
        "out %3, r24       \n\t" /* OCR0  = *pCurrentStep++; */
        "ld r24, z+        \n\t"
        "movw %0, r30      \n\t"
        :: "r" (pCurrentStep),
           "I" (_SFR_IO_ADDR(PORTA)),
           "I" (_SFR_IO_ADDR(PORTC)),
           "I" (_SFR_IO_ADDR(OCR0))
    );

    asm volatile(
        /* check if IRQ must be enabled 1+1+1+1+1+1+1 = 3/7 */
        "or r24, r24       \n\t" /* if (*pCurrentStep++) { */
        "breq L_skip%=     \n\t"
        "ldi r24, %1       \n\t" /*   TIFR   = (1<<OCF0);  */
        "out %0, r24       \n\t"
        "in r24, %2        \n\t" /*   TIMSK |= (1<<OCIE0); */
        "ori r24, %3       \n\t"
        "out %2, r24       \n\t"
        "L_skip%=:         \n\t" /* } */
        :: "I" (_SFR_IO_ADDR(TIFR)),
           "M" ((1<<OCF0)),
           "I" (_SFR_IO_ADDR(TIMSK)),
           "M" ((1<<OCIE0))
    );

    asm volatile(
        /* restore registers 2+2+2+1+2+4 = 13 */
        "pop r31           \n\t"
        "pop r30           \n\t"
        "pop r24           \n\t"
        "out __SREG__, r24 \n\t"
        "pop r24           \n\t"
        "reti              \n\t"
        ::
    );
}

/* worst case: 9+9+5+13 = 36 clks -> 4.5us @ 8MHz */
ISR(TIMER0_COMP_vect, ISR_NAKED)
{
    asm volatile(
        /* save registers 2+1+2+2+2 = 9 */
        "push r24          \n\t"
        "in r24, __SREG__  \n\t"
        "push r24          \n\t"
        "push r30          \n\t"
        "push r31          \n\t"
        ::
    );

    asm volatile(
        /* load table values 1+1+1+1+1+1+1+1+1 = 9 */
        "movw r30, %0      \n\t"
        "ld r24, z+        \n\t"
        "out %1, r24       \n\t" /* PORTA = *pCurrentStep++; */
        "ld r24, z+        \n\t"
        "out %2, r24       \n\t" /* PORTC = *pCurrentStep++; */
        "ld r24, z+        \n\t"
        "out %3, r24       \n\t" /* OCR0  = *pCurrentStep++; */
        "ld r24, z+        \n\t"
        "movw %0, r30      \n\t"
        :: "r" (pCurrentStep),
           "I" (_SFR_IO_ADDR(PORTA)),
           "I" (_SFR_IO_ADDR(PORTC)),
           "I" (_SFR_IO_ADDR(OCR0))
    );

    asm volatile(
        /* check if IRQ must be disabled 1+1+1+1+1 = 3/5 */
        "or r24, r24       \n\t" /* if (!(*pCurrentStep++)) { */
        "brne L_skip%=     \n\t"
        "in r24, %0        \n\t" /*   TIMSK &= ~(1<<OCIE0); */
        "andi r24, %1      \n\t"
        "out %0, r24       \n\t"
        "L_skip%=:         \n\t" /* } */
        :: "I" (_SFR_IO_ADDR(TIMSK)),
           "M" (0xFD) /* ~(1<<OCIE0) */
    );

    asm volatile(
        /* restore registers 2+2+2+1+2+4 = 13 */
        "pop r31           \n\t"
        "pop r30           \n\t"
        "pop r24           \n\t"
        "out __SREG__, r24 \n\t"
        "pop r24           \n\t"
        "reti              \n\t"
        ::
    );
}

/* calc chan_valueX => chan_rawdataX */
static void calculate_timer_values(uint8_t color)
{
    uint8_t  *pData    = &chan_rawdata[color][4]; /* skip first entry (init) */
    uint8_t  index     = 0;
    uint16_t chan_used = 0xFFFF;
    uint16_t chan_init = 0xFFFF;

    /* loop until all channels are calculated */
    while (chan_used) {

        uint8_t  i;
        uint8_t  min_value = 0xFF;
        uint16_t chan_tmp  = chan_used;
        uint16_t chan_mask = 0x0001;

        for (i = 0; i < 16; i++) {
            uint8_t value = chan_value[i][color];

            /* skip if channel already used */
            if (chan_used & chan_mask) {
                /* channel is not used (value 0x00) */
                if (value == 0x00) {
                    chan_init &= (~chan_mask);
                    chan_used &= (~chan_mask);

                /* found a new lower value */
                } else if (value < min_value) {
                    min_value = value;
                    chan_tmp  = chan_used & (~chan_mask);

                /* found another value with the same value */
                } else if (value == min_value) {
                    chan_tmp &= (~chan_mask);
                }
            }

            chan_mask <<= 1;
        }

        chan_used &= chan_tmp;

        if (min_value < 0xFF) {
            /* set new outputs */
            *pData++        = (chan_used & 0xFF);           /* PORTA */
            *pData++        = ((chan_used >> 8) & 0xFF);    /* PORTC */

            /* previous step needs timervalue and enable IRQ */
            *(pData++ -4)   = min_value;                    /* OCR0  */
            *(pData++ -4)   = 0x01;                         /* flags */
        }

        index++;
    }

    /* fill all remaining slots */
    while (index < 16) {
        /* repeat enabled outputs */
        *pData++        = (chan_used & 0xFF);          /* PORTA */
        *pData++        = ((chan_used >> 8) & 0xFF);   /* PORTC */

        /* previous step was last one (no timevalue / disable IRQ) */
        *(pData++ -4)   = 0x00;                        /* OCR0  */
        *(pData++ -4)   = 0x00;                        /* flags */

        index++;
    }

    /* first slot/init: enable only channels that are > 0 */
    pData = &chan_rawdata[color][0];
    *pData++ = (chan_init & 0xFF);         /* PORTA */
    *pData++ = ((chan_init >> 8) & 0xFF);  /* PORTC */
}

uint8_t rgb_update(uint8_t dirty_mask, uint8_t update_mode)
{
    static uint8_t chan_dirty;

    chan_dirty |= (dirty_mask & COLOR_MASK);

    while ((chan_dirty != 0) && (update_mode != UPDATE_NONE)) {
        if ((chan_dirty & (1<<COLOR_RED)) && (nextColor == COLOR_BLUE)) {
            calculate_timer_values(COLOR_RED);
            chan_dirty &= ~(1<<COLOR_RED);

        } else if ((chan_dirty & (1<<COLOR_GREEN)) && (nextColor == COLOR_RED))  {
            calculate_timer_values(COLOR_GREEN);
            chan_dirty &= ~(1<<COLOR_GREEN);

        } else if ((chan_dirty & (1<<COLOR_BLUE)) && (nextColor == COLOR_GREEN))  {
            calculate_timer_values(COLOR_BLUE);
            chan_dirty &= ~(1<<COLOR_BLUE);

        } else if (update_mode != UPDATE_BLOCKING) {
            break;
        }
    };

    return chan_dirty;
}

void rgb_init(void)
{
    /* 16 PWM Outputs */
    PORTA = 0x00;
    DDRA  = 0xFF;

    PORTC = 0x00;
    DDRC  = 0xFF;

    /* color ROWs */
    PORTB &= ~((1<<ROW1) | (1<<ROW2) | (1<<ROW3) | (1<<ROW4));
    DDRB  |= (1<<ROW1) | (1<<ROW2) | (1<<ROW3) | (1<<ROW4);

    /* timer0, FCPU/64, overflow interrupt */
    TCCR0 = (1<<CS01) | (1<<CS00); /* FCPU/64 */
    TIMSK = (1<<TOIE0);
}