#include <nLights.h>
#include <math.h>
#byte OSCTUNE = 0xF9B


int sinLookupTable[1024];
float var = 0;
int32 frame = 0;
long a = 100;
long b = 98;
long c = 50;
const int leds[32] = {PIN_C0,PIN_C1,PIN_C2,PIN_C3,PIN_D0,PIN_D1,PIN_D2,PIN_D3,
                      PIN_E2,PIN_E1,PIN_E0,PIN_A5,PIN_A4,PIN_A3,PIN_A2,PIN_A1,
                      PIN_C4,PIN_C5,PIN_C6,PIN_C7,PIN_D4,PIN_D5,PIN_D6,PIN_D7,
                      PIN_A0,PIN_B7,PIN_B6,PIN_B5,PIN_B4,PIN_B3,PIN_B2,PIN_B1};
const int16 tableLength = 1024;
long brightness[32] = {0,0,0,0,0,0,0,0,
                      0,0,0,0,0,0,0,0,
                      0,0,0,0,0,0,0,0,
                      0,0,0,0,0,0,0,0};
long lastBrightness[32] = {0,0,0,0,0,0,0,0,
                      0,0,0,0,0,0,0,0,
                      0,0,0,0,0,0,0,0,
                      0,0,0,0,0,0,0,0};                      

const int numLeds = 32;
const int pwmPeak = 255;
int pwmCount = 0;
int32 test = 0;



void main() {

  // setup_oscillator(OSC_8MHZ | OSC_INTRC);
  //    OSCTUNE = 0xF0;

   //output_high(PIN_C7);
   setup();
   //output_low(PIN_C7);

   setup_adc_ports(NO_ANALOGS|VSS_VDD);
   setup_adc(ADC_OFF);
   setup_psp(PSP_DISABLED);
   setup_spi(FALSE);
   setup_wdt(WDT_OFF);
   setup_timer_0(RTCC_INTERNAL);
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DIV_BY_1,0xbf,3);
   enable_interrupts(INT_TIMER2);
   enable_interrupts(global);

   setup_timer_3(T3_DISABLED|T3_DIV_BY_1);
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);

   while(1) {
         draw();
   }
}

void setup() {
   int16 i16 = 0;
   for(i16 = 0; i16 < tableLength; i16++) {
		var = (float)(i16) * ((PI / 2.0) / 1024.0);
		sinLookupTable[i16] = (int)(255 * abs(sin(var)));
	}

}

void draw() {
   int32 tmpBrightness;
   long i = 0;
   const int32 rate = 1000;
   const int step = 10;

   c = 281 - sinLookup(frame/2);
   b = 133 - sinLookup(frame/3)/2;
   a = 135 - sinLookup(frame/5)/2;

   for(i = 0; i < numLeds; i++) {
      lastBrightness[i] = brightness[i];
      tmpBrightness = sinLookup(rate*(frame+i*i)/a) * sinLookup(rate*(frame+i*i)/b);
      tmpBrightness *= cosLookup(rate*(frame+i*i)/c);
      tmpBrightness /= (65025);
      
      brightness[i] = tmpBrightness;
      brightness[i] += 3*lastBrightness[i];
      brightness[i] /= 4;
   }

   frame += 1;

}

#inline
int32 sinLookup(int32 x) {
   if((x / tableLength) % 2 != 1) {
      // ascending
      return sinLookupTable[x % tableLength];
   }
   else {
      return sinLookupTable[tableLength - (x % tableLength) - 1];
   }
}

#inline
int32 cosLookup(int32 x) {
   if((x / tableLength) % 2 == 1) {
      return sinLookupTable[x % tableLength];
   }
   else {
      return sinLookupTable[tableLength - (x % tableLength) - 1];
   }
}

#int_timer2
void clock_isr() {
   if(pwmCount == 0) {
    {output_high(PIN_C0);} //C0
    {output_high(PIN_C1);}
    {output_high(PIN_C2);}
    {output_high(PIN_C3);}
    {output_high(PIN_D0);}
    {output_high(PIN_D1);}
    {output_high(PIN_D2);}
    {output_high(PIN_D3);}
    {output_high(PIN_E2);}
    {output_high(PIN_E1);}
    {output_high(PIN_E0);}
    {output_high(PIN_A5);}
    {output_high(PIN_A4);}
    {output_high(PIN_A3);}
    {output_high(PIN_A2);}
    {output_high(PIN_A1);}
    {output_high(PIN_C4);}
    {output_high(PIN_C5);}
    {output_high(PIN_C6);}
    {output_high(PIN_C7);}
    {output_high(PIN_D4);}
    {output_high(PIN_D5);}
    {output_high(PIN_D6);}
    {output_high(PIN_D7);}
    {output_high(PIN_A0);}
    {output_high(PIN_B7);}
    {output_high(PIN_B6);}
    {output_high(PIN_B5);}
    {output_high(PIN_B4);}
    {output_high(PIN_B3);}
    {output_high(PIN_B2);}
    {output_high(PIN_B1);}
   }

#IFDEF FLICKER
   if(pwmCount == brightness[0]) {output_low(PIN_C0);}
   if(pwmCount == brightness[1]) {output_low(PIN_C1);}
   if(pwmCount == brightness[2]) {output_low(PIN_C2);}
   if(pwmCount == brightness[3]) {output_low(PIN_C3);}
   if(pwmCount == brightness[4]) {output_low(PIN_D0);}
   if(pwmCount == brightness[5]) {output_low(PIN_D1);}
   if(pwmCount == brightness[6]) {output_low(PIN_D2);}
   if(pwmCount == brightness[7]) {output_low(PIN_D3);}
   if(pwmCount == brightness[8]) {output_low(PIN_E2);}
   if(pwmCount == brightness[9]) {output_low(PIN_E1);}
   if(pwmCount == brightness[10]) {output_low(PIN_E0);}
   if(pwmCount == brightness[11]) {output_low(PIN_A5);}
   if(pwmCount == brightness[12]) {output_low(PIN_A4);}
   if(pwmCount == brightness[13]) {output_low(PIN_A3);}
   if(pwmCount == brightness[14]) {output_low(PIN_A2);}
   if(pwmCount == brightness[15]) {output_low(PIN_A1);}
   if(pwmCount == brightness[16]) {output_low(PIN_C4);}
   if(pwmCount == brightness[17]) {output_low(PIN_C5);}
   if(pwmCount == brightness[18]) {output_low(PIN_C6);}
   if(pwmCount == brightness[19]) {output_low(PIN_C7);}
   if(pwmCount == brightness[20]) {output_low(PIN_D4);}
   if(pwmCount == brightness[21]) {output_low(PIN_D5);}
   if(pwmCount == brightness[22]) {output_low(PIN_D6);}
   if(pwmCount == brightness[23]) {output_low(PIN_D7);}
   if(pwmCount == brightness[24]) {output_low(PIN_A0);}
   if(pwmCount == brightness[25]) {output_low(PIN_B7);}
   if(pwmCount == brightness[26]) {output_low(PIN_B6);}
   if(pwmCount == brightness[27]) {output_low(PIN_B5);}
   if(pwmCount == brightness[28]) {output_low(PIN_B4);}
   if(pwmCount == brightness[29]) {output_low(PIN_B3);}
   if(pwmCount == brightness[30]) {output_low(PIN_B2);}
   if(pwmCount == brightness[31]) {output_low(PIN_B1);}
#ELSE
   if(pwmCount >= brightness[0]) {output_low(PIN_C0);}
   if(pwmCount >= brightness[1]) {output_low(PIN_C1);}
   if(pwmCount >= brightness[2]) {output_low(PIN_C2);}
   if(pwmCount >= brightness[3]) {output_low(PIN_C3);}
   if(pwmCount >= brightness[4]) {output_low(PIN_D0);}
   if(pwmCount >= brightness[5]) {output_low(PIN_D1);}
   if(pwmCount >= brightness[6]) {output_low(PIN_D2);}
   if(pwmCount >= brightness[7]) {output_low(PIN_D3);}
   if(pwmCount >= brightness[8]) {output_low(PIN_E2);}
   if(pwmCount >= brightness[9]) {output_low(PIN_E1);}
   if(pwmCount >= brightness[10]) {output_low(PIN_E0);}
   if(pwmCount >= brightness[11]) {output_low(PIN_A5);}
   if(pwmCount >= brightness[12]) {output_low(PIN_A4);}
   if(pwmCount >= brightness[13]) {output_low(PIN_A3);}
   if(pwmCount >= brightness[14]) {output_low(PIN_A2);}
   if(pwmCount >= brightness[15]) {output_low(PIN_A1);}
   if(pwmCount >= brightness[16]) {output_low(PIN_C4);}
   if(pwmCount >= brightness[17]) {output_low(PIN_C5);}
   if(pwmCount >= brightness[18]) {output_low(PIN_C6);}
   if(pwmCount >= brightness[19]) {output_low(PIN_C7);}
   if(pwmCount >= brightness[20]) {output_low(PIN_D4);}
   if(pwmCount >= brightness[21]) {output_low(PIN_D5);}
   if(pwmCount >= brightness[22]) {output_low(PIN_D6);}
   if(pwmCount >= brightness[23]) {output_low(PIN_D7);}
   if(pwmCount >= brightness[24]) {output_low(PIN_A0);}
   if(pwmCount >= brightness[25]) {output_low(PIN_B7);}
   if(pwmCount >= brightness[26]) {output_low(PIN_B6);}
   if(pwmCount >= brightness[27]) {output_low(PIN_B5);}
   if(pwmCount >= brightness[28]) {output_low(PIN_B4);}
   if(pwmCount >= brightness[29]) {output_low(PIN_B3);}
   if(pwmCount >= brightness[30]) {output_low(PIN_B2);}
   if(pwmCount >= brightness[31]) {output_low(PIN_B1);}  
#ENDIF

   pwmCount++;
   if(pwmCount > pwmPeak) {pwmCount = 0;}
}