#include <Wire.h>

// Serial-to-TWI/I2C code, used to convert serial data from a PC/Mac to
// TWI/I2C data for a Peggy 2.0 with the VideoPeggyTwi firmware.
//

#define PEGGY_ADDRESS 34
#define TWI_FREQ 300000

void setup()
{
  Serial.begin(115200);

  UBRR0H = 0;
  UBRR0L = 16; // manually set 115200
  UCSR0A = (1<<U2X0);
  //UCSR0B = (1<<RXEN0) | (1<<TXEN0);

  //Serial.print("Sender Initialized...");
  Wire.begin();

  PORTC |=  (1<<PORTC5) | (1<<PORTC4); // enable pullups
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
  digitalWrite(A5, LOW);
  digitalWrite(A4, HIGH);


  // jack up the frequency for TWI, we need a pretty high
  // rate  from the TWI engine for this to handle 115k input
  // without getting buffer overruns
  TWSR &= ~(1<<TWPS0);
  TWSR &= ~(1<<TWPS1);
  TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;

}

void loop()
{
  uint8_t count = Serial.available();
 
  if (count > 0)
  {
    // dont allow send too many bytes at once, dont want to exceed the buffer
    // size of the Wire library
    if (count > 16) count = 16;
 
    Wire.beginTransmission(PEGGY_ADDRESS);
    while (count-- > 0 )
    {
      uint8_t c = Serial.read();
      Wire.send(c);
    }
    Wire.endTransmission();  
  }
}

#include <Wire.h>

// Serial-to-TWI/I2C code, used to convert serial data from a PC/Mac to
// TWI/I2C data for a Peggy 2.0 with the VideoPeggyTwi firmware.
//

#define PEGGY_ADDRESS 34
#define TWI_FREQ 300000

void setup()
{
  Serial.begin(115200);

  UBRR0H = 0;
  UBRR0L = 16; // manually set 115200
  UCSR0A = (1<<U2X0);
  //UCSR0B = (1<<RXEN0) | (1<<TXEN0);

  //Serial.print("Sender Initialized...");
  Wire.begin();

  PORTC |=  (1<<PORTC5) | (1<<PORTC4); // enable pullups
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
  digitalWrite(A5, HIGH);
  digitalWrite(A4, HIGH);


  // jack up the frequency for TWI, we need a pretty high
  // rate  from the TWI engine for this to handle 115k input
  // without getting buffer overruns
  TWSR &= ~(1<<TWPS0);
  TWSR &= ~(1<<TWPS1);
  TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;

}

void loop()
{
  uint8_t count = Serial.available();
 
  if (count > 0)
  {
    // dont allow send too many bytes at once, dont want to exceed the buffer
    // size of the Wire library
    if (count > 16) count = 16;
 
    Wire.beginTransmission(PEGGY_ADDRESS);
    while (count-- > 0 )
    {
      uint8_t c = Serial.read();
      Wire.send(c);
    }
    Wire.endTransmission();  
  }
}
 

//#define FPS 144
#define FPS 90

// 25 rows * 13 bytes per row == 325
#define DISP_BUFFER_SIZE 325
#define MAX_BRIGHTNESS 15

#define TWI_SLAVE_ID 34

////////////////////////////////////////////////////////////////////////////////////////////
uint8_t frameBuffer[DISP_BUFFER_SIZE];

uint8_t *currentRowPtr = frameBuffer;
uint8_t currentRow=0;
uint8_t currentBrightness=0;


// Note: the refresh code has been optimized heavily from the previous version.
SIGNAL(TIMER0_COMPA_vect)
{      
        // there are 15 passes through this interrupt for each row per frame.
        // ( 15 * 25) = 375 times per frame.
        // during those 15 passes, a led can be on or off.
        // if it is off the entire time, the perceived brightness is 0/15
        // if it is on the entire time, the perceived brightness is 15/15
        // giving a total of 16 average brightness levels from fully on to fully off.
        // currentBrightness is a comparison variable, used to determine if a certain
        // pixel is on or off during one of those 15 cycles.   currentBrightnessShifted
        // is the same value left shifted 4 bits:  This is just an optimization for
        // comparing the high-order bytes.
        if (++currentBrightness >= MAX_BRIGHTNESS)  
        {
                currentBrightness=0;
                if (++currentRow > 24)
                {
                        currentRow =0;
                        currentRowPtr = frameBuffer;
                }
                else
                {
                        currentRowPtr += 13;
                }
        }

               
        ////////////////////  Parse a row of data and write out the bits via spi
        uint8_t currentBrightnessShifted = currentBrightness <<4;
       
        uint8_t *ptr = currentRowPtr + 12;  // its more convenient to work from right to left
        uint8_t p, bits=0;
         
        // optimization: by using variables for these two masking constants, we can trick gcc into not
        // promoting to 16-bit int (constants are 16 bit by default, causing the
        // comparisons to get promoted to 16bit otherwise)].  This turns out to be a pretty
        // substantial optimization for this handler
        uint8_t himask = 0xf0;  
        uint8_t lomask = 0x0f;
       
        // Opimization: interleave waiting for SPI with other code, so the CPU can do something useful
        // when waiting for each SPI transmission to complete
       
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=1;
        SPDR = bits;

        bits=0;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=64;
        if ((p & himask) > currentBrightnessShifted)    bits|=128;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=16;
        if ((p & himask) > currentBrightnessShifted)    bits|=32;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=4;
        if ((p & himask) > currentBrightnessShifted)    bits|=8;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=1;
        if ((p & himask) > currentBrightnessShifted)    bits|=2;

        while (!(SPSR & (1<<SPIF)))  { } // wait for prior bitshift to complete
        SPDR = bits;
       
       
        bits=0;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=64;
        if ((p & himask) > currentBrightnessShifted)    bits|=128;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=16;
        if ((p & himask) > currentBrightnessShifted)    bits|=32;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=4;
        if ((p & himask) > currentBrightnessShifted)    bits|=8;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=1;
        if ((p & himask) > currentBrightnessShifted)    bits|=2;

        while (!(SPSR & (1<<SPIF)))  { } // wait for prior bitshift to complete
        SPDR = bits;
       
       
        bits=0;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=64;
        if ((p & himask) > currentBrightnessShifted)    bits|=128;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=16;
        if ((p & himask) > currentBrightnessShifted)    bits|=32;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=4;
        if ((p & himask) > currentBrightnessShifted)    bits|=8;
        p = *ptr--;
        if ((p & lomask) > currentBrightness)                   bits|=1;
        if ((p & himask) > currentBrightnessShifted)    bits|=2;

        while (!(SPSR & (1<<SPIF)))  { }// wait for prior bitshift to complete
        SPDR = bits;
       
        ////////////////////  Now set the row and latch the bits
       
        uint8_t portD;
       
       
        if (currentRow < 15)
                portD = currentRow+1;
        else
                portD = (currentRow -14)<<4;


        while (!(SPSR & (1<<SPIF)))  { } // wait for last bitshift to complete
       
        //if (currentBrightness == 0)
        PORTD = 0;                              // set all rows to off
        PORTB |= _BV(1);//(1<<PB1); //  latch it, values now set
        //if (currentBrightness == 0)
        PORTD = portD;     // set row
        PORTB &= ~( _BV(1));//~((1<<PB1)); // reset latch for next time
       
        // notes to self, calculations from the oscope:
        // need about minimum of 6us total to clock out all 4 bytes
        // roughly 1.5ms per byte, although some of that is
        // idle time taken between bytes.  6=7us therefore is our
        // absolute minimum time needed to refresh a row, not counting calculation time.
        // Thats just if we do nothing else when writing out SPI and toggle to another row.
        //Measured values from this routine    
        // @ 144 fps the latch is toggled every 19us with an actual 4byte clock out time of 12-13us
        // @ 70 fps the latch is toggle every 39us, with a clock out time of 13-14us
        // times do not count setup/teardown of stack frame
       
        // one byte @ 115k takes 86us (max) 78us (min) , measured time
        // one byte @ 230k takes 43us (max) 39us (min) , measured time
        // so 230k serial might barely be possible, but not with a 16mhz crystal (error rate to high)
        // 250k might just barely be possible
}

void displayInit(void)
{
        // need to set output for SPI clock, MOSI, SS and latch.  Eventhough SS is not connected,
        // it must apparently be set as output for hardware SPI to work.
        DDRB =  (1<<DDB5) | (1<<DDB3) | (1<<DDB2) | (1<<DDB1);
        // set all portd pins as output
        DDRD = 0xff;

        PORTD=0; // select no row
       
        // enable hardware SPI, set as master and clock rate of fck/2
        SPCR = (1<<SPE) | (1<<MSTR);
        SPSR = (1<<SPI2X);

        // setup the interrupt.
        TCCR0A = (1<<WGM01); // clear timer on compare match
        TCCR0B = (1<<CS01); // timer uses main system clock with 1/8 prescale
        OCR0A  = (F_CPU >> 3) / 25 / 15 / FPS; // Frames per second * 15 passes for brightness * 25 rows
        TIMSK0 = (1<<OCIE0A);   // call interrupt on output compare match


        for (uint8_t i=0; i < 4; i++)
        {
                SPDR = 0;
                while (!bit_is_set(SPSR, SPIF)) {}
        }
}




////////////////////////////////////////////////////////////////////////////////////////////
// I2C  routines
////////////////////////////////////////////////////////////////////////////////////////////

// TWI Slave Receiver staus codes, from Atmel notes
#define TWI_SRX_ADR_ACK            0x60
#define TWI_SRX_ADR_ACK_M_ARB_LOST 0x68
#define TWI_SRX_GEN_ACK            0x70
#define TWI_SRX_GEN_ACK_M_ARB_LOST 0x78
#define TWI_SRX_ADR_DATA_ACK       0x80
#define TWI_SRX_ADR_DATA_NACK      0x88
#define TWI_SRX_GEN_DATA_ACK       0x90
#define TWI_SRX_GEN_DATA_NACK      0x98
#define TWI_SRX_STOP_RESTART       0xA0
#define TWI_NO_STATE               0xF8
#define TWI_BUS_ERROR              0x00

void initTwiSlave(uint8_t addr)
{
 
    //PORTC |=  _BV(5) | _BV(4);      //(1<<PC5) | (1<<PC4); // enable pullups
    //PORTC |= (1<<PORTC5) | (1<<PORTC4);

    TWAR = (0<<TWGCE) |((uint8_t) (0xff & (addr<<1)));    // set slave address, no general call address
    TWDR = 0xff; // Default content = SDA released

    TWCR = (1<<TWINT) |   // "clear the flag"  (hate this backward terminology)
          (1<<TWEA) |     // send acks to master when getting address or data
          (0<<TWSTA) |    // not a master, cant do start
          (0<<TWSTO) |    // doc says set these to 0
          (0<<TWWC) |
          (1<<TWEN) |   // hardware TWI  enabled
          (0<<TWIE);   // do NOT generate interrupts.

    while (TWCR & (1<<TWIE)) { }

}


uint8_t getTwiByte(void)
{
        uint8_t result=0;
 
        keepListening:
 
        // wait for an state change
        while (!(TWCR & (1<<TWINT))) { } // wait for TWINT to be set
 
       
        //uint8_t sr = TWSR;
        switch (TWSR)
        {
            case TWI_SRX_ADR_DATA_ACK:  // received a byte of data data
            case TWI_SRX_GEN_DATA_ACK:      
                        result = TWDR;
                TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
                        break;

               
//        case TWI_SRX_GEN_ACK_M_ARB_LOST:
//        case TWI_SRX_ADR_ACK_M_ARB_LOST:
        case TWI_SRX_GEN_ACK:            
        case TWI_SRX_ADR_ACK:      // receive our address byte      
                      TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
                      goto keepListening;
                      break;
                       
            case TWI_SRX_STOP_RESTART:       // A STOP or repeated START condition was received
                TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA);
                    goto keepListening;      
                break;          

            case TWI_SRX_ADR_DATA_NACK:   // data received, returned nack
            case TWI_SRX_GEN_DATA_NACK:
                //result = TWDR;
                TWCR = (1<<TWEN)|(1<<TWINT); //|(1<<TWEA);
                goto keepListening;
                        break;
           
        case TWI_NO_STATE:
                goto keepListening;
                break;  
       
//          case TWI_BUS_ERROR:      
            default:                             // something bad happened. assuming a bus error, we try to recover from this
              //state = TWSR;
              //TWCR = (1<<TWEN)|(0<<TWINT)|(0<<TWEA);    // Don't ack any further requests, will stop receiving
              // alternate handling: reset state and continue
              TWCR = (1<<TWEN)|(1<<TWINT)|(1<<TWEA)|(1<<TWSTO);    // ignore
              // wait for stop condition to be exectued; TWINT will not be set after a stop
                  while(TWCR & (1<<TWSTO)){ }
                  result = 0xff;
              break;
    }
        return result;
}



////////////////////////////////////////////////////////////////////////////////////////////
// MAIN LOOP: handle the input data stream  and stuff bytes into the framebuffer
////////////////////////////////////////////////////////////////////////////////////////////

void serviceInputData(void)
{
        uint8_t *ptr = frameBuffer;
        uint8_t state = 0;
        int counter = 0;
        while (1)
        {  
                uint8_t c = getTwiByte();
               
                // very simple state machine to look for 6 byte start of frame
                // marker and copy bytes that follow into buffer
                if (state  <6)
                {

                        // must get a 0xdeadbeef to start frame.
                        // I look for two more bytes after that, but
                        // they are reserved for future use.
                        // so send a 1 followed by a 0 for now.

                        if (state == 0 && c == 0xde) state++;
                        else if (state ==1 && c == 0xad) state++;
                        else if (state ==2 && c == 0xbe) state++;
                        else if (state ==3 && c == 0xef) state++;
                        else if (state ==4 && c == 0x01) state++;
                        else if (state ==5)  // dont care what 6th byte is
                        {
                                state++;
                                counter = 0;
                                ptr = frameBuffer;
                        }
                        else state = 0; // error: reset to look for start of frame
                }
                else
                {
                        // inside of a frame, so save each byte to buffer
                        *ptr++ = c;
                        counter++;
                        if (counter >= DISP_BUFFER_SIZE)
                        {
                                // buffer filled, so reset everything to wait for next frame
                                //counter = 0;
                                //ptr = frameBuffer;
                                state = 0;
                        }
                }
        }
}

 
void setup()                    // run once, when the sketch starts
{
        // Enable pullups for buttons/i2c
        //PORTB |= _BV(0);//(1<<PB0);
        //PORTC = _BV(5) |  _BV(4) |  _BV(3) | _BV(2) |  _BV(1) |  _BV(0);
                 //(1<<PC5) | (1<<PC4) | (1<<PC3) | (1<<PC2) | (1<<PC1) | (1<<PC0);

        UCSR0B =0; // turn OFF serial RX/TX, necessary if using arduino bootloader
       
        displayInit();

        initTwiSlave(TWI_SLAVE_ID);
       
        sei( );

        // clear display and set to test pattern
        // pattern should look just like the "gray test pattern" from EMS

        uint8_t v = 0;
        for (int i =0; i < DISP_BUFFER_SIZE; i++)
        {
                v = (v+2) % 16;
            // set to 0 for blank startup display
                // low order bits on the left, high order bits on the right
                        frameBuffer[i]= v + ((v+1)<<4);  
                //      frameBuffer[i]=0;
        }
       
        serviceInputData();  // never returns
}

void loop()                     // run over and over again
{
 
}