#include <Ultrasonic.h>
#include <Adafruit_NeoPixel.h>
#ifdef __AVR__
#include <avr/power.h>
#endif

#define PIN        6 
#define POTI       3 
#define NUMPIXELS 256

Adafruit_NeoPixel pixels(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);

Ultrasonic ultrasonic(4, 3);
int distance;

uint16_t currentDisplay[8];

byte Numbers[10][5] = {
  {0x07, 0x05, 0x05, 0x05, 0x07}, 
  {0x02, 0x03, 0x02, 0x02, 0x07}, 
  {0x07, 0x04, 0x02, 0x01, 0x07}, 
  {0x07, 0x04, 0x06, 0x04, 0x07}, 
  {0x05, 0x05, 0x07, 0x04, 0x04},
  {0x07, 0x01, 0x07, 0x04, 0x07}, 
  {0x07, 0x01, 0x07, 0x05, 0x07}, 
  {0x07, 0x04, 0x02, 0x02, 0x02}, 
  {0x07, 0x05, 0x07, 0x05, 0x07}, 
  {0x07, 0x05, 0x07, 0x04, 0x07}
  };

void setup()
{
    Serial.begin(9600);
    pixels.begin();
}

void loop()
{
    pixels.clear();

    distance = ultrasonic.read(CM);

    Serial.print("Distance in CM: ");
    Serial.println(distance);

    printNumber(distance);

    pixels.show();
}

void printNumber(int number)
{
    int hundreds = number/100;
    int tens = (number % 100) / 10;
    int units = number % 10;

    ClearCurrentDisplay();

    for (int i = 0; i < 5; i++)
    {
        currentDisplay[i] = currentDisplay[i] | Numbers[units][i] << 8;
    }
    for (int i = 0; i < 5; i++)
    {
        currentDisplay[i] = currentDisplay[i] | Numbers[tens][i] << 4;
    }
    for (int i = 0; i < 5; i++)
    {
        currentDisplay[i] = currentDisplay[i] | Numbers[hundreds][i];
    }

    for (int i = 5; i < 8; i++)
    {
        currentDisplay[i] = 0x00;
    }

    UpdateDisplay();
}

void ClearCurrentDisplay()
{
    for (int i = 0; i < 8; i++)
    {
        currentDisplay[i] = 0x00;
    }
}

void UpdateDisplay()
{
    int cdSize = sizeof(currentDisplay) / sizeof(currentDisplay[0]);
    Serial.println(sizeof(currentDisplay[0]));
    for (int i = 0; i < cdSize; i++)
    {
        for (int j = 0; j < 16; j++)
        {
            if (j % 2 == 0)
            {
                if ((currentDisplay[i] & (1 << j)) != 0)
                {
                    pixels.setPixelColor(i + 16 * j, pixels.Color(255, 0, 0));
                }
            }
            else
            {
                if ((currentDisplay[i] & (1 << j)) != 0)
                {
                    pixels.setPixelColor(15 - i + 16 * j, pixels.Color(255, 0, 0));
                }
            }
        }
    }
}