
#include "effects.h"
#include "rgb.h"
#include "leds.h"
#include "trig.h"
#include <stdlib.h>

unsigned short last_millis;
unsigned char num_particles = 0;
#define max_particles 32
#define max_circles 3
#define max_spirals 3
#define max_lines 2

typedef struct { signed short pos, duration; signed char x1, y1, x2, y2, x3, y3; rgb colour; } particle;
particle particles[max_particles];

unsigned short random(unsigned short lessthan) {
    return (unsigned short)rand() % lessthan;
}

void effect1(rgb* buffer, unsigned char scheme, unsigned short millis) { // snaking RGB spectrum
  last_millis = millis;
  for( unsigned char i = 0; i < 80; ++i ) {
    colourScheme(&buffer[i], scheme, (i*3 + millis/8)&255);
  }
}

void effect2(rgb* buffer, unsigned char scheme, unsigned short millis) { // radiating RGB circles from centre
  unsigned short millis_passed = millis - last_millis;
  last_millis = millis;
  if( millis_passed > 0 ) {
    if( num_particles < max_circles && (num_particles == 0 || random(500) < millis_passed) ) {
      particles[num_particles].pos = 0;
      colourSchemeBright(&particles[num_particles].colour, scheme, (unsigned char)random(255), (unsigned char)random(255));
      ++num_particles;
    }
    for( unsigned char i = 0; i < num_particles; ++i ) {
      particles[i].pos += millis_passed;
      if( particles[i].pos >= 1600 ) {
        for( unsigned char j = i; j < num_particles-1; ++j ) {
          particles[j].pos = particles[j+1].pos;
          particles[j].colour.r = particles[j+1].colour.r;
          particles[j].colour.g = particles[j+1].colour.g;
          particles[j].colour.b = particles[j+1].colour.b;
        }
        --num_particles;
      }
    }
    for( unsigned char i = 0; i < 80; ++i ) {
      black(&buffer[i]);
      for( unsigned char j = 0; j < num_particles; ++j ) {
        signed short dist = particles[j].pos/2 - LEDs[i].dist*8;
        if( dist < 0 )
          dist *= -1; 
        if( dist < 128 )
          addRGB(&buffer[i], (unsigned char)(particles[j].colour.r * (128 - dist) / 128), (unsigned char)(particles[j].colour.g * (128 - dist) / 128), (unsigned char)(particles[j].colour.b * (128 - dist) / 128));
      }
    }
  }
}

void effect3(rgb* buffer, unsigned char scheme, unsigned short millis) { // radiating RGB spectrum
  last_millis = millis;
  for( unsigned char i = 0; i < 80; ++i ) {
    colourScheme(&buffer[i], scheme, (LEDs[i].dist*2 + millis/8)&255);
  }
}

void effect4(rgb* buffer, unsigned char scheme, unsigned short millis) { // rotating RGB spectrum
  last_millis = millis;
  for( unsigned char i = 0; i < 80; ++i ) {
    colourScheme(&buffer[i], scheme, (LEDs[i].angle - millis/8)&255);
  }
}

void effect5(rgb* buffer, unsigned char scheme, unsigned short millis) { // rotating RGB points
  last_millis = millis;
  for( unsigned char i = 0; i < 80; ++i ) {
    unsigned char dist = (LEDs[i].angle - millis/8)&255;
    if( dist < 64 ) {
      colourScheme(&buffer[i], scheme, (millis/8)&255);
      fade(&buffer[i], dist*4);
    }
  }
}

void effect6(rgb* buffer, unsigned char scheme, unsigned short millis) { // rotating RGB line
  last_millis = millis;
  for( unsigned char i = 0; i < 80; ++i ) {
    signed short dist = (LEDs[i].angle - millis/8)&255;
    if( dist >= 128 )
      dist -= 128;
    dist -= 32;
    if( dist < 0 )
      dist *= -1; 
    if( dist < 32 ) {
      colourScheme(&buffer[i], scheme, (millis/8)&255);
      fade(&buffer[i], (unsigned char)(255 - dist*8));
    } else {
      black(&buffer[i]);
    }
  }
}

static inline unsigned char mySqrt(unsigned short n) {
  unsigned short res = 0;
  unsigned short one = 1 << 14;       // highest power of four <= 0xFFFF

  // Align 'one' to the highest power of four <= n
  while (one > n) one >>= 2;

  // 8 iterations max for 16-bit inputs
  while (one != 0) {
      if (n >= res + one) {
          n  -= res + one;
          res  = (res >> 1) + one;
      } else {
          res >>= 1;
      }
      one >>= 2;
  }
  return (unsigned char)res; // floor(sqrt(n))
}
static inline unsigned short mySqr(signed short x) {
  return (unsigned short)(x*x);
}
void effect7(rgb* buffer, unsigned char scheme, unsigned short millis) { // radiating RGB circles from different points
  unsigned short millis_passed = millis - last_millis;
  last_millis = millis;
  if( millis_passed > 0 ) {
    if( num_particles < max_circles && (num_particles == 0 || random(1000) < millis_passed) ) {
      particles[num_particles].pos = 0;
      particles[num_particles].x1 = (signed char)(random(121)-60);
      particles[num_particles].y1 = (signed char)(random(121)-60);
      colourScheme(&particles[num_particles].colour, scheme, (unsigned char)random(255));
      ++num_particles;
    }
    for( unsigned char i = 0; i < num_particles; ++i ) {
      particles[i].pos += millis_passed;
      if( particles[i].pos >= 2000 ) {
        for( unsigned char j = i; j < num_particles-1; ++j ) {
          particles[j].pos = particles[j+1].pos;
          particles[j].x1 = particles[j+1].x1;
          particles[j].y1 = particles[j+1].y1;
          particles[j].colour.r = particles[j+1].colour.r;
          particles[j].colour.g = particles[j+1].colour.g;
          particles[j].colour.b = particles[j+1].colour.b;
        }
        --num_particles;
      }
    }
    for( unsigned char i = 0; i < 80; ++i ) {
      black(&buffer[i]);
      for( unsigned char j = 0; j < num_particles; ++j ) {
        signed short dist = (signed short)particles[j].pos/2 - (signed short)mySqrt( mySqr((signed short)LEDs[i].x - (signed short)particles[j].x1) + mySqr((signed short)LEDs[i].y - (signed short)particles[j].y1) ) * 8;
        if( dist < 0 )
          dist *= -1; 
        if( dist < 128 )
          addRGB(&buffer[i], (unsigned char)(particles[j].colour.r * (128 - dist) / 128), (unsigned char)(particles[j].colour.g * (128 - dist) / 128), (unsigned char)(particles[j].colour.b * (128 - dist) / 128));
      }
    }
  }
}

void effect8(rgb* buffer, unsigned char scheme, unsigned short millis) { // radiating RGB circles from centre and back again
    unsigned short millis_passed = millis - last_millis;
    last_millis = millis;
    if( millis_passed > 0 ) {
      if( num_particles < max_circles && (num_particles == 0 || random(1500) < millis_passed) ) {
        particles[num_particles].pos = 0;
        colourSchemeBright(&particles[num_particles].colour, scheme, (unsigned char)random(255), (unsigned char)random(255));
        ++num_particles;
      }
      for( unsigned char i = 0; i < num_particles; ++i ) {
        particles[i].pos += millis_passed;
        if( particles[i].pos >= 3200 ) {
          for( unsigned char j = i; j < num_particles-1; ++j ) {
            particles[j].pos = particles[j+1].pos;
            particles[j].colour.r = particles[j+1].colour.r;
            particles[j].colour.g = particles[j+1].colour.g;
            particles[j].colour.b = particles[j+1].colour.b;
          }
          --num_particles;
        }
      }
      for( unsigned char i = 0; i < 80; ++i ) {
        black(&buffer[i]);
        for( unsigned char j = 0; j < num_particles; ++j ) {
          signed short dist = (signed short)(particles[j].pos >= 1600 ? 3200 - particles[j].pos : particles[j].pos)/2 - (signed short)LEDs[i].dist*8;
          if( dist < 0 )
            dist *= -1; 
          if( dist < 128 )
            addRGB(&buffer[i], (unsigned char)(particles[j].colour.r * (128 - dist) / 128), (unsigned char)(particles[j].colour.g * (128 - dist) / 128), (unsigned char)(particles[j].colour.b * (128 - dist) / 128));
        }
      }
    }
}

void effect9(rgb* buffer, unsigned char scheme, unsigned short millis) { // moving RGB angled lines
  unsigned short millis_passed = millis - last_millis;
  last_millis = millis;

  if( millis_passed > 0 ) {
    if( num_particles < 2 && (num_particles == 0 || random(1000) < millis_passed) ) {
      unsigned short angle = random(1024);
      particles[num_particles].duration = 0;
      if( (scheme&1) == 0 )
        colourScheme(&particles[num_particles].colour, scheme, (unsigned char)random(255));
      else
        colourSchemeBright(&particles[num_particles].colour, scheme, (unsigned char)random(255), (unsigned char)random(255));
      particles[num_particles].x1 = sin(angle) * 120 / 256;
      particles[num_particles].y1 = cos(angle) * 120 / 256;
      particles[num_particles].x2 = sin(angle + 256) * 120 / 256;
      particles[num_particles].y2 = cos(angle + 256) * 120 / 256;
      particles[num_particles].x3 = sin(angle + 512) * 120 / 256;
      particles[num_particles].y3 = cos(angle + 512) * 120 / 256;
      ++num_particles;
    }
    for( unsigned char i = 0; i < num_particles; ++i ) {
      particles[i].duration += millis_passed;
      if( particles[i].duration > 256*8 ) {
        for( unsigned char j = i; j < num_particles-1; ++j ) {
          particles[j].duration = particles[j+1].duration;
          particles[j].colour.r = particles[j+1].colour.r;
          particles[j].colour.g = particles[j+1].colour.g;
          particles[j].colour.b = particles[j+1].colour.b;
          particles[j].x1 = particles[j+1].x1;
          particles[j].y1 = particles[j+1].y1;
          particles[j].x2 = particles[j+1].x2;
          particles[j].y2 = particles[j+1].y2;
          particles[j].x3 = particles[j+1].x3;
          particles[j].y3 = particles[j+1].y3;
        }
        --num_particles;
      }
    }
    for( unsigned char i = 0; i < 80; ++i ) {
//      buffer[i].r = buffer[i].g = buffer[i].b = 0;
      black(&buffer[i]);

      // The unrolled loop is quite a bit faster (num_particles is always 0, 1 o r2)
#if 0
      for( unsigned char j = 0; j < num_particles; ++j ) {
        signed short dur = (signed short)(particles[j].duration >> 3);
        signed short a = particles[j].x3 * dur >> 7;
        signed short b = particles[j].y3 * dur >> 7;
        signed short x1 = particles[j].x1 + a;
        signed short y1 = particles[j].y1 + b;
        signed short x2 = particles[j].x2 + a;
        signed short y2 = particles[j].y2 + b;
        signed short dist = ((x2 - x1) * (LEDs[i].y - y1) - (y2 - y1) * (LEDs[i].x - x1)) >> 5;
        if( dist < 0 )
          dist *= -1;
        if( dist < 64 ) {
          addRGB(&buffer[i], (unsigned char)(particles[j].colour.r * (64 - dist) >> 6), (unsigned char)(particles[j].colour.g * (64 - dist) >> 6), (unsigned char)(particles[j].colour.b * (64 - dist) >> 6));
        }
      }
#else
      if( num_particles ) {
        signed short dur = (signed short)(particles[0].duration >> 3);
        signed short a = particles[0].x3 * dur >> 7;
        signed short b = particles[0].y3 * dur >> 7;
        signed short x1 = particles[0].x1 + a;
        signed short y1 = particles[0].y1 + b;
        signed short x2 = particles[0].x2 + a;
        signed short y2 = particles[0].y2 + b;
        signed short dist = ((x2 - x1) * (LEDs[i].y - y1) - (y2 - y1) * (LEDs[i].x - x1)) >> 5;
        if( dist < 0 )
          dist *= -1;
        if( dist < 64 ) {
          addRGB(&buffer[i], (unsigned char)(particles[0].colour.r * (64 - dist) >> 6), (unsigned char)(particles[0].colour.g * (64 - dist) >> 6), (unsigned char)(particles[0].colour.b * (64 - dist) >> 6));
        }
        if( num_particles > 1 ) {
            signed short dur = (signed short)(particles[1].duration >> 3);
            signed short a = particles[1].x3 * dur >> 7;
            signed short b = particles[1].y3 * dur >> 7;
            signed short x1 = particles[1].x1 + a;
            signed short y1 = particles[1].y1 + b;
            signed short x2 = particles[1].x2 + a;
            signed short y2 = particles[1].y2 + b;
            signed short dist = ((x2 - x1) * (LEDs[i].y - y1) - (y2 - y1) * (LEDs[i].x - x1)) >> 5;
            if( dist < 0 )
              dist *= -1;
            if( dist < 64 ) {
              addRGB(&buffer[i], (unsigned char)(particles[1].colour.r * (64 - dist) >> 6), (unsigned char)(particles[1].colour.g * (64 - dist) >> 6), (unsigned char)(particles[1].colour.b * (64 - dist) >> 6));
            }
        }
      }
#endif
    }
  }
}

void effect10(rgb* buffer, unsigned char scheme, unsigned short millis) { // snaking RGB points
  unsigned short millis_passed = millis - last_millis;
  last_millis = millis;
  if( millis_passed > 0 ) {
    if( num_particles < max_particles && (num_particles == 0 || random(200) < millis_passed) ) {
      particles[num_particles].pos = 0;
      colourSchemeBright(&particles[num_particles].colour, scheme, (unsigned char)random(255), (unsigned char)random(255));
      ++num_particles;
    }
    for( unsigned char i = 0; i < 80; ++i )
      black(&buffer[i]);
    for( unsigned char i = 0; i < num_particles; ++i ) {
      particles[i].pos += millis_passed*4;
      if( particles[i].pos >= 80*128 ) {
        for( unsigned char j = i; j < num_particles-1; ++j ) {
          particles[j].pos = particles[j+1].pos;
          particles[j].colour.r = particles[j+1].colour.r;
          particles[j].colour.g = particles[j+1].colour.g;
          particles[j].colour.b = particles[j+1].colour.b;
        }
        --num_particles;
      } else {
        unsigned char pixel = (unsigned char)(particles[i].pos/128);
        unsigned char sub_pixel = particles[i].pos%128;
        unsigned char sub_pixel2 = (unsigned char)((unsigned short)sub_pixel*sub_pixel/128);
        addRGB(&buffer[pixel], (unsigned char)(particles[i].colour.r * (128 - sub_pixel2) / 128), (unsigned char)(particles[i].colour.g * (128 - sub_pixel2) / 128), (unsigned char)(particles[i].colour.b * (128 - sub_pixel2) / 128));
        if( sub_pixel > 0 && pixel < 79 ) {
          sub_pixel2 = (unsigned char)((unsigned short)(128-sub_pixel)*(128-sub_pixel)/128);
          addRGB(&buffer[pixel+1], (unsigned char)(particles[i].colour.r * (128 - sub_pixel2) / 128), (unsigned char)(particles[i].colour.g * (128 - sub_pixel2) / 128), (unsigned char)(particles[i].colour.b * (128 - sub_pixel2) / 128));
        }
      }
    }
  }
}

void effect11(rgb* buffer, unsigned char scheme, unsigned short millis) { // radiating RGB spirals
  unsigned short millis_passed = millis - last_millis;
  last_millis = millis;
  if( millis_passed > 0 ) {
    if( num_particles < max_spirals && (num_particles == 0 || random(250) < millis_passed) ) {
      particles[num_particles].pos = 0;
      particles[num_particles].duration = (signed short)random(255);
      colourSchemeBright(&particles[num_particles].colour, scheme, (unsigned char)random(255), (unsigned char)random(255));
      ++num_particles;
    }
    for( unsigned char i = 0; i < num_particles; ++i ) {
      particles[i].pos += millis_passed*4;
      particles[i].duration += millis_passed*2;
      if( particles[i].pos >= 1600*4 ) {
        for( unsigned char j = i; j < num_particles-1; ++j ) {
          particles[j].pos = particles[j+1].pos;
          particles[j].duration = particles[j+1].duration;
          particles[j].colour.r = particles[j+1].colour.r;
          particles[j].colour.g = particles[j+1].colour.g;
          particles[j].colour.b = particles[j+1].colour.b;
        }
        --num_particles;
      }
    }
    for( unsigned char i = 0; i < 80; ++i ) {
      black(&buffer[i]);
      for( unsigned char j = 0; j < num_particles; ++j ) {
        signed short dist1 = (signed short)particles[j].pos/8 - (signed short)LEDs[i].dist*8;
        signed short dist2 = (signed short)((LEDs[i].angle + particles[j].duration/8)&255) - 128;
        if( dist1 < 0 )
          dist1 *= -1; 
        if( dist2 < 0 )
          dist2 *= -1;
        if( dist1 < 128 && dist2 < 48/*32*/ )
          addRGB(&buffer[i], (unsigned char)(particles[j].colour.r * (128 - dist1) / 128), (unsigned char)(particles[j].colour.g * (128 - dist1) / 128), (unsigned char)(particles[j].colour.b * (128 - dist1) / 128));
      }
    }
  }
}

static unsigned char particles_in_position(unsigned char pos) {
  unsigned char ret = 0;
  for( unsigned char i = 0; i < num_particles; ++i )
    if( particles[i].pos == pos )
      ++ret;
  return ret;
}
void effect12(rgb* buffer, unsigned char scheme, unsigned short millis) { // twinkling RGB points
  unsigned short millis_passed = millis - last_millis;
  last_millis = millis;

  if( millis_passed > 0 ) {
    if( num_particles < max_particles && (num_particles == 0 || random(50) < millis_passed) ) {
      do {
        particles[num_particles].pos = (unsigned char)random(79);
      } while( particles_in_position((unsigned char)particles[num_particles].pos) > 0 );
      particles[num_particles].duration = 128*32;
      colourSchemeBright(&particles[num_particles].colour, scheme, (unsigned char)random(255), (unsigned char)random(255));
      ++num_particles;
    }
    for( unsigned char i = 0; i < 80; ++i )
      black(&buffer[i]);
    for( unsigned char i = 0; i < num_particles; ++i ) {
      particles[i].duration -= millis_passed*8;
      if( particles[i].duration <= 0 ) {
        for( unsigned char j = i; j < num_particles-1; ++j ) {
          particles[j].pos = particles[j+1].pos;
          particles[j].duration = particles[j+1].duration;
          particles[j].colour.r = particles[j+1].colour.r;
          particles[j].colour.g = particles[j+1].colour.g;
          particles[j].colour.b = particles[j+1].colour.b;
        }
        --num_particles;
      } else {
        unsigned char pixel = (unsigned char)particles[i].pos;
        signed char sub_pixel = (signed char)((signed short)(64*32 - particles[i].duration)/32);
        if( sub_pixel < 0 )
          sub_pixel *= -1;
        sub_pixel = 64 - sub_pixel;
        addRGB(&buffer[pixel], (unsigned char)(particles[i].colour.r * sub_pixel / 64), (unsigned char)(particles[i].colour.g * sub_pixel / 64), (unsigned char)(particles[i].colour.b * sub_pixel / 64));
      }
    }
  }
}

void effect(rgb* buffer, unsigned char which, unsigned char scheme, unsigned short millis) {
  switch(which) {
    case 1:
      effect1(buffer, scheme, millis);
      break;
    case 2:
      effect2(buffer, scheme, millis);
      break;
    case 3:
      effect3(buffer, scheme, millis);
      break;
    case 4:
      effect4(buffer, scheme, millis);
      break;
    case 5:
      effect5(buffer, scheme, millis);
      break;
    case 6:
      effect6(buffer, scheme, millis);
      break;
    case 7:
      effect7(buffer, scheme, millis);
      break;
    case 8:
      effect8(buffer, scheme, millis);
      break;
    case 9:
      effect9(buffer, scheme, millis);
      break;
    case 10:
      effect10(buffer, scheme, millis);
      break;
    case 11:
      effect11(buffer, scheme, millis);
      break;
    case 12:
      effect12(buffer, scheme, millis);
      break;
  }
}

void ResetEffect(unsigned short millis) {
    num_particles = 0;
    last_millis = millis;
}
