#ifndef DRAW_H
#define DRAW_H

#include "Palettes.h"
#define FASTLED_ESP32_I2S_NUM_DMA_BUFFERS 4
#define CUBE_SIZE 10 // 10x10x10 cube
#define AXIS_X 0x78
#define AXIS_Y 0x79
#define AXIS_Z 0x7a
#define NUM_LEDS (CUBE_SIZE*CUBE_SIZE*CUBE_SIZE)
#define UPDATES_PER_SECOND 30
#define BRIGHTNESS 60
#define NUM_PINS 5
#define LEDS_PER_PIN 200

CRGB leds[NUM_PINS][LEDS_PER_PIN];
volatile unsigned char cube[CUBE_SIZE][CUBE_SIZE];

// Lookup table: converts (x,y,z) to (pin_number, led_index)
struct LEDAddress {
  uint8_t pin;      // Which pin (0-4)
  uint16_t index;   // Index within that pin's array (0-199)
};

LEDAddress LEDLookUp(uint8_t x, uint8_t y, uint8_t z) {
  LEDAddress addr;
  
  // Determine which string/pin based on Z layer (each pin handles 2 Z layers)
  addr.pin = z / 2;  // Z=0,1→pin0; Z=2,3→pin1; Z=4,5→pin2; etc.
  
  if (z % 2 == 0) {
    if (x % 2 == 0) {
      // Even z and even x: Y goes bottom-to-top (0→9)
      addr.index = x * CUBE_SIZE + y;
        } else {
      // Even z and odd x rows: Y goes top-to-bottom (9→0) - serpentine
            addr.index = x * CUBE_SIZE +(CUBE_SIZE-1-y);
        }
    }
    else {
        if (x % 2 == 0) {
    // Odd z and even x: Y goes top-to-bottom (9→0)
            addr.index = CUBE_SIZE*CUBE_SIZE+ (CUBE_SIZE-1-x) * CUBE_SIZE + (CUBE_SIZE-1-y);
        } 
        else {
    // Odd z and odd x: Y goes bottom-to-top (0→9) - serpentine
            addr.index = CUBE_SIZE*CUBE_SIZE+ (CUBE_SIZE-1-x) * CUBE_SIZE + y;
        }
    }   
  // Add offset for second Z layer within this string
  //addr.index = localZ * 100 + layerIndex;
  
  return addr;
}

// basic drawing functions here:

// This function validates that we are drawing inside the cube.
unsigned char inrange(int x, int y, int z)
{
	if (x >= 0 && x < CUBE_SIZE && y >= 0 && y < CUBE_SIZE && z >= 0 && z < CUBE_SIZE)
	{
		return 1;
	} 
    else
	{
		// One of the coordinates was outside the cube.
		return 0;
	}
}

// Draw a pixel at x,y,z with CRGB color

void DrawPixel(uint8_t x, uint8_t y, uint8_t z, CRGB color) {
  if (x >= CUBE_SIZE || y >= CUBE_SIZE || z >= CUBE_SIZE) return; // Bounds check
  
  LEDAddress addr = LEDLookUp(x, y, z);
  leds[addr.pin][addr.index] = color;
}

// Clear a pixel at x,y,z
 void ClearPixel(int x, int y, int z)
{
    if (inrange(x, y, z))
    {
          LEDAddress addr = LEDLookUp(x, y, z);
          leds[addr.pin][addr.index] = CRGB::Black;
    }
}

// Get the current status of a pixel
CRGB getPixel(int x, int y, int z)
{
	if (inrange(x,y,z))
    {
        LEDAddress addr = LEDLookUp(x, y, z);
	    return leds[addr.pin][addr.index];
    }
    else

        return CRGB(0,0,0); // Out of range returns black
}

// Fill a plane along the X axis
void setplane_x (int x, uint8_t hue, uint8_t sat, uint8_t val)
{
	int z;
	int y;
	if (x>=0 && x<CUBE_SIZE)
	{
		for (z=0;z<CUBE_SIZE;z++)
		{
			for (y=0;y<CUBE_SIZE;y++)
			{
				DrawPixel(x,y,z, CHSV(hue,sat,val));
			}
		}
	}
}

// Fill a plane along the Y axis
void setplane_y (int y, uint8_t hue, uint8_t sat, uint8_t val)
{
    int z;
    int x;
    if (y>=0 && y<CUBE_SIZE)
    {
        for (z=0;z<CUBE_SIZE;z++)
        {
            for (x=0;x<CUBE_SIZE;x++)
            {
                DrawPixel(x,y,z,CHSV(hue,sat,val));
            }
        }
    }
}
// Fill a plane along the Z axis
void setplane_z (int z, uint8_t hue, uint8_t sat, uint8_t val)
{
    int y;
    int x;
    if (z>=0 && z<CUBE_SIZE)
    {
        for (y=0;y<CUBE_SIZE;y++)
        {
            for (x=0;x<CUBE_SIZE;x++)
            {
                DrawPixel(x,y,z,CHSV(hue,sat,val));
            }
        }
    }
}

// Clear a plane along the X axis
void clrplane_x (int x)
{
    int z;
    int y;
    if (x>=0 && x<CUBE_SIZE)
    {
        for (z=0;z<CUBE_SIZE;z++)
        {
            for (y=0;y<CUBE_SIZE;y++)
            {
                ClearPixel(x,y,z); 
            }
        }
    }
}
// Clear a plane along the Y axis
void clrplane_y (int y)
{
	int x;
    int z;
	if (y>=0 && y<CUBE_SIZE)
	{
		for (z=0;z<CUBE_SIZE;z++)
        {
            for (x=0;x<CUBE_SIZE;x++)
			ClearPixel(x,y,z); 
	    }
    }
}
// Clear a plane along the Z axis
void clrplane_z (int z)
{
    int y;
    int x;
    if (z>=0 && z<CUBE_SIZE)
    {
        for (y=0;y<CUBE_SIZE;y++)
        {
            for (x=0;x<CUBE_SIZE;x++)
                ClearPixel(x,y,z); 
            
        }
    }
}

void setplane (char axis, unsigned char i, uint8_t hue, uint8_t sat, uint8_t val)
{
    switch (axis)
    {
        case AXIS_X:
            setplane_x(i, hue, sat, val);
            break;
        
       case AXIS_Y:
            setplane_y(i, hue, sat, val);
            break;

       case AXIS_Z:
            setplane_z(i, hue, sat, val);
            break;
    }
}

void clrplane (char axis, unsigned char i)
{
    switch (axis)
    {
        case AXIS_X:
            clrplane_x(i);
            break;
        
       case AXIS_Y:
            clrplane_y(i);
            break;

       case AXIS_Z:
            clrplane_z(i);
            break;
    }
}

// Shift the cube along one axis
void shift(int axis, int direction)
{
    static int x, y, z;
    CRGB layer_buffer[CUBE_SIZE][CUBE_SIZE];

    if (axis == AXIS_X)
    {
        if (direction > 0)
        {
            // first move layer 7 to buffer
            for (y = 0; y < CUBE_SIZE; y++)
            {
                for (z = 0; z < CUBE_SIZE; z++)
                    layer_buffer[y][z] = getPixel(CUBE_SIZE-1, y, z);  
            }
            // next shift all other layers up
            for (x = CUBE_SIZE - 1; x > 0; x--)
            {
                for (y = 0; y < CUBE_SIZE; y++)
                {
                    for (z = 0; z < CUBE_SIZE; z++)
                        DrawPixel(x,y,z, getPixel(x-1,y,z));
                }
            }
            // Transfer layer buffer to the first layer
            for (y = 0; y < CUBE_SIZE; y++)
            {
                for (z = 0; z < CUBE_SIZE; z++)
                    DrawPixel(0, y, z, layer_buffer[y][z]);
            }
            FastLED.show();
            FastLED.delay(100/UPDATES_PER_SECOND);
        }  
        else
        {
            // first move layer 0 to buffer
            for (y = 0; y < CUBE_SIZE; y++)
            {
                for (z = 0; z < CUBE_SIZE; z++)
                    layer_buffer[y][z] = getPixel(0,y,z);
            }
            // next shift all other layers down
            for (x = 0; x < CUBE_SIZE-1; x++)
            {
                for (y = 0; y < CUBE_SIZE; y++)
                {
                    for (z = 0; z < CUBE_SIZE; z++)
                        DrawPixel(x,y,z, getPixel(x+1,y,z));
                }
            }
            // Transfer layer buffer to the top layer
            for (y = 0; y < CUBE_SIZE; y++)
            {
                for (z = 0; z < CUBE_SIZE; z++)
                    DrawPixel(CUBE_SIZE-1, y, z, layer_buffer[y][z]);
            }
            FastLED.show();
            FastLED.delay(100/UPDATES_PER_SECOND);
        }
    }
    else
    if (axis == AXIS_Y)
    {
        if (direction > 0)
        {
            // first move layer 7 to buffer
            for (x = 0; x < CUBE_SIZE; x++)
            {
                for (z = 0; z < CUBE_SIZE; z++)
                    layer_buffer[x][z] = getPixel(x,CUBE_SIZE-1,z);
            }
            // next shift all other layers up
            for (y = CUBE_SIZE - 1; y > 0; y--)
            {
                for (x = 0; x < CUBE_SIZE; x++)
                {
                    for (z = 0; z < CUBE_SIZE; z++)
                        DrawPixel(x,y,z, getPixel(x,y-1,z));
                }
            }
            // Transfer layer buffer to the first layer
            for (x = 0; x < CUBE_SIZE; x++)
            {
                for (z = 0; z < CUBE_SIZE; z++)
                    DrawPixel(x, 0, z, layer_buffer[x][z]);
            }
            FastLED.show();
            FastLED.delay(100/UPDATES_PER_SECOND);
        }
        else
        {
            // first move layer 0 to buffer
            for (x = 0; x < CUBE_SIZE; x++)
            {
                for (z = 0; z < CUBE_SIZE; z++)
                    layer_buffer[x][z] = getPixel(x,0,z);   
            }
            // next shift all other layers down
            for (y = 0; y < CUBE_SIZE - 1; y++)
            {
                for (x = 0; x < CUBE_SIZE; x++)
                {
                    for (z = 0; z < CUBE_SIZE; z++)
                        DrawPixel(x,y,z,getPixel(x,y+1,z));
                }
            }
            // Transfer layer buffer to the top layer
            for (x = 0; x < CUBE_SIZE; x++)
            {
                for (z = 0; z < CUBE_SIZE; z++)
                    DrawPixel(x, CUBE_SIZE-1, z, layer_buffer[x][z]);
            }
            FastLED.show();
            FastLED.delay(100/UPDATES_PER_SECOND);
        }
    }
    else

    if (axis == AXIS_Z)
    {
        if (direction > 0)
        {
            // first move layer 7 to buffer
            for (x = 0; x < CUBE_SIZE; x++)
            {
                for (y = 0; y < CUBE_SIZE; y++)
                    layer_buffer[x][y] = getPixel(x,y,CUBE_SIZE-1);
            }
            // next shift all other layers up
            for (z = CUBE_SIZE - 1; z > 0; z--)
            {
                for (y = 0; y < CUBE_SIZE; y++)
                {
                    for (x = 0; x < CUBE_SIZE; x++)
                        DrawPixel(x,y,z, getPixel(x,y,z-1));
                }
            }
            // Transfer layer buffer to the first layer
            for (x = 0; x < CUBE_SIZE; x++)
            {
                for (y = 0; y < CUBE_SIZE; y++)
                    DrawPixel(x, y, 0, layer_buffer[x][y]);
            }   
            FastLED.show();
            FastLED.delay(100/UPDATES_PER_SECOND);
        }
        else
        {
            // first move layer 0 to buffer
            for (x = 0; x < CUBE_SIZE; x++)
            {
                for (y = 0; y < CUBE_SIZE; y++)
                    layer_buffer[x][y] = getPixel(x,y,CUBE_SIZE-1);
            }
            // next shift all other layers down
            for (z = 0; z < CUBE_SIZE - 1; z++)
            {
                for (y = 0; y < CUBE_SIZE; y++)
                {
                    for (x = 0; x < CUBE_SIZE; x++)
                        DrawPixel(x,y,z, getPixel(x,y,z+1));
                }
            }
            // Transfer layer buffer to the top layer
            for (x = 0; x < CUBE_SIZE; x++)
            {
                for (y = 0; y < CUBE_SIZE; y++)
                    DrawPixel(x, y, CUBE_SIZE-1, layer_buffer[x][y]);
            }   
            FastLED.show();
            FastLED.delay(100/UPDATES_PER_SECOND);
        }
    }
}

// Makes sure x1 is alwas smaller than x2
// This is usefull for functions that uses for loops,
// to avoid infinite loops
void argorder(int ix1, int ix2, int *ox1, int *ox2)
{
	if (ix1>ix2)
	{
		int tmp;
		tmp = ix1;
		ix1= ix2;
		ix2 = tmp;
	}
	*ox1 = ix1;
	*ox2 = ix2;
}

void box_wireframe(int x1, int y1, int z1, int x2, int y2, int z2, uint8_t hue, uint8_t sat, uint8_t val)
{
    int ix;
    int iy;
    int iz;

	argorder(x1, x2, &x1, &x2);
	argorder(y1, y2, &y1, &y2);
	argorder(z1, z2, &z1, &z2);

    // Lines along X axis
    for (ix=x1;ix<=x2;ix++)
    {
        DrawPixel(ix,y1,z1, CHSV(hue,sat,val));
        DrawPixel(ix,y2,z1, CHSV(hue,sat,val));
        DrawPixel(ix,y1,z2, CHSV(hue,sat,val));
        DrawPixel(ix,y2,z2, CHSV(hue,sat,val));
    }
    // Lines along Y axis
	for (iy=y1+1;iy<=y2-1;iy++)
	{
		DrawPixel(x1,iy,z1, CHSV(hue,sat,val));
		DrawPixel(x1,iy,z2, CHSV(hue,sat,val));
		DrawPixel(x2,iy,z1, CHSV(hue,sat,val));
		DrawPixel(x2,iy,z2, CHSV(hue,sat,val));
	}
	// Lines along Z axis
	for (iz=z1+1;iz<=z2-1;iz++)
	{
		DrawPixel(x1,y1,iz, CHSV(hue,sat,val));
		DrawPixel(x1,y2,iz, CHSV(hue,sat,val));
		DrawPixel(x2,y1,iz, CHSV(hue,sat,val));
		DrawPixel(x2,y2,iz, CHSV(hue,sat,val));
	}
        
    FastLED.show();
    FastLED.delay(100/UPDATES_PER_SECOND);
}

void fill(uint8_t hue, uint8_t sat, uint8_t val)
{
    for (int x = 0; x < CUBE_SIZE; x++)
    {
        for (int y = 0; y < CUBE_SIZE; y++)
        {
            for (int z = 0; z < CUBE_SIZE; z++)
            {
                DrawPixel(x, y, z, CHSV(hue, sat, val));
            }
        }
    }
    FastLED.show();
    FastLED.delay(100/UPDATES_PER_SECOND);
}

void draw_positions_axis (char axis, unsigned char positions[CUBE_SIZE*CUBE_SIZE], int invert, int hue, int sat, int val)
{
	int x, y, p;
	
	FastLED.clear();
	
	for (x=0; x<CUBE_SIZE; x++)
	{
		for (y=0; y<CUBE_SIZE; y++)
		{
			if (invert)
			{
				p = (CUBE_SIZE-1-positions[(x*CUBE_SIZE)+y]);
			} else
			{
				p = positions[(x*CUBE_SIZE)+y];
			}
		
			if (axis == AXIS_Z)
				DrawPixel(x,y,p, CHSV(hue, sat, val));
				
			if (axis == AXIS_Y)
				DrawPixel(x,p,y, CHSV(hue, sat, val));
				
			if (axis == AXIS_X)
                DrawPixel(p,y,x, CHSV(hue, sat, val));
		}
	}
}

// Make a square on a given axis
void make_a_square(uint8_t axis, uint8_t a, uint8_t b, CRGB color)
{
switch(axis)
	{
		case 0:
            DrawPixel(a, b, a, color);
            DrawPixel(CUBE_SIZE-1-a, b, a, color);
            DrawPixel(b, a, a, color);
            DrawPixel(b, CUBE_SIZE-1-a, a, color);
			break;

		case 1:
            DrawPixel(a, a, b, color);
            DrawPixel(a, CUBE_SIZE-1-a, b, color);
            DrawPixel(a, b, a,color);
            DrawPixel(a, b, CUBE_SIZE-1-a,color);
			break;

		case 2:
			DrawPixel(b, a, a ,color);
            DrawPixel(b, a, CUBE_SIZE-1-a, color);
            DrawPixel(a, a, b, color);
            DrawPixel(CUBE_SIZE-1-a, a, b, color);
			break;
	}
}

// take a snapshot of the current cube state,
// show it for delay ms, then clear the cube
void snap(int delay)
{
    FastLED.show();
    FastLED.delay(delay);
    FastLED.clear();
}

CRGB colorPicker(int PaletteNumber, uint8_t colorID)
{
    CRGBPalette16 Pal = gGradientPalettes[PaletteNumber];
    return ColorFromPalette(Pal, colorID, 200, LINEARBLEND);
}


// Vorbox functions

void shrink(uint8_t axis, uint8_t delay, int invert)
{
// shrink to middle
    if(invert == 0)
    {
        for(int n = 0; n < CUBE_SIZE / 2; n++)
        {
            for(int i = n; i < CUBE_SIZE - n; i++)
            {
                make_a_square(axis, n, i, colorPicker(5, n * 32));
                snap(delay);
            }
        }
    }
    else
    {
        for(int n = CUBE_SIZE; n >3; n--)
        {
            for(int i=CUBE_SIZE-1-n; i < n+1; i++)
            {
                make_a_square(axis, n, i, colorPicker(5, n * 32));
                snap(delay);
            }
        }
    }
}

void expand(uint8_t axis, uint8_t delay, int invert)
{
    if(invert == 0)
    {
        for(int n = CUBE_SIZE / 2; n <CUBE_SIZE; n++)
        {
            for(int i=CUBE_SIZE-n; i < n+1; i++)
            {
                make_a_square(axis, n, i, colorPicker(5, n * 32));
                snap(delay);
            }
        }
    }
    else
    {
        for(int n = 3; n > -1; n--)
        {
            for(int i=n; i < CUBE_SIZE - n; i++)
            {
                make_a_square(axis, n, i, colorPicker(5, n * 32));
                snap(delay);
            }   
        }
    }
}

void shrink_expand(uint8_t axis, uint8_t delay, int invert)
{
    if(invert==0) // straight thru start plane = 0
    {
        // shrink to middle
        shrink(axis, delay, invert);
        // expand to outside
        expand(axis, delay, invert);
    }
    else
    {  // straight thru start plane = CUBE_SIZE - 1
        // shrink to middle
        shrink(axis, delay, invert);
        // expand to outside
        expand(axis, delay, invert);
    }
}

#endif // DRAW_H