#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <display.h>

//Adafruit_SSD1306 display1(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire1, -1);

void initDisplay() {
    /*Wire.begin(D1_I2C_SDA, D1_I2C_SCL);
    if(!display1.begin(SSD1306_EXTERNALVCC, DISPLAY_I2C_ADDRESS)) {
        Serial.println("SSD1306 #1 allocation failed");
        for(;;);
    }*/

    Wire1.begin(D2_I2C_SDA, D2_I2C_SCL);
    if(!display2.begin(SSD1306_SWITCHCAPVCC, DISPLAY_I2C_ADDRESS)) {
        Serial.println("SSD1306 #2 allocation failed");
        for(;;);
    }

    //display1.clearDisplay();
    //Serial.println("start display 2");
    //заливаем весь дисплей
    /*display2.clearDisplay();
    for (int x = 0; x < 128; x ++) {
        for (int y = 0; y < 64; y++) {
            display2.drawPixel(x, y, SSD1306_WHITE);
        }
    }*/

    //default huynnya
    display2.clearDisplay();
    //display2BorderDebug();
    display2Border();
    display2DrawSenseLine(3.5);
    display2.display();
    //Serial.println("start display 2 test success");
}

void drawPoint(int displayId, int x, int y) {
    //float rad = y * PI / 180.0;
    switch (displayId)
    {
        case 1:
        break;
        case 2:
            display2.clearDisplay();
            display2Border();
            display2DrawSenseLine(map(x, 0, 10, 90, -90));
            display2.display();
            break;
        default:
        break;
    }
}

const int16_t borders[][2] = {
        {20, 59},
                {20, 30},
                        {29, 15},
                            {40, 3},
                                {45, 1},
                                    {65, 0},
                                {80, 1},
                            {91, 3},
                        {103, 15},
                {110, 30},
        {110, 59}
    };
const int borderSize = sizeof(borders) / sizeof(borders[0]);

void display2BorderDebug() {
    int x = 0;
    int y = 1;
    for (int i = 1; i <borderSize; i++) {
        display2.drawLine(borders[i-1][x], borders[i-1][y], borders[i][x], borders[i][y], WHITE);
    } 
    display2.drawLine(borders[borderSize-1][x], borders[borderSize-1][y], borders[0][x], borders[0][y], WHITE);
}

const int TwoLetterPoint[][2] = {
    {0,3},{0,2},{0,1},{0,0},
                      {1,0},
                {1,1},
          {2,2},
    {3,3},
    {4,3},{4,2},{4,1},{4,0}
};

const int FourLetterPoint[][2] = {
                    {0, 1},
            {1, 2}, {1, 1},
        {2, 3},     {2, 1},
    {3, 3},{3,2},   {3, 1}, {3, 0},
                    {4, 1}            
};

const int SixLetterPoint[][2] = {
    {0, 3}, {0, 2}, {0, 1}, {0, 0},
    {1, 3},
    {2, 3}, {2, 2}, {2, 1}, {2, 0},
    {3, 3},                 {3, 0},
    {4, 3}, {4, 2}, {4, 1}, {4, 0}
};

const int EightLetterPoint[][2] = {
    {0, 3}, {0, 2}, {0, 1}, {0, 0},
    {1, 3},                 {1, 0},
    {2, 3}, {2, 2}, {2, 1}, {2, 0},
    {3, 3},                 {3, 0},
    {4, 3}, {4, 2}, {4, 1}, {4, 0}
};

const int centerX = 65;
const int centerY = 55;

void display2Border() {
    int radius[3] = {30, 40, 45};
    const int internal = 0;
    const int external = 1;
    const int extended = 2;

    display2.drawPixel(centerX, centerY, WHITE);
    for (int r = 1; r < 5; r++) {
        display2.drawCircle(centerX, centerY, r, WHITE);
    }

    for (int angle = -90; angle <= 90; angle++) {
        float rad = angle * PI / 180.0;
        int absAngle = abs(angle);
        float radSin = sin(rad);
        float radCos = cos(rad);

        radsCalc(centerX, centerY, radius[internal], radSin, radCos);
        radsCalc(centerX, centerY, radius[external], radSin, radCos);

        //need chunk
        if (absAngle % 18 == 0) {
            if ((absAngle + 90) % 36 == 0) {
                int endLineX = centerX + (radius[extended] * radSin);
                int endLineY = centerY - (radius[extended] * radCos);

                int endLine4ChrX = centerX + ((radius[extended]+5) * radSin);
                int endLine4ChrY = centerY - ((radius[extended]+5) * radCos);

                display2.drawLine(
                    centerX + (radius[internal] * radSin),
                    centerY - (radius[internal] * radCos),
                    endLineX,
                    endLineY,
                    WHITE
                );

                int letChr = (5 - ((angle + 90) / 36)) * 2;
                switch (letChr)
                {
                    case 2:
                        for (int i = 0; i < sizeof(TwoLetterPoint) / sizeof(TwoLetterPoint[0]); i++) {
                            display2.drawPixel(endLine4ChrX + TwoLetterPoint[i][0], endLine4ChrY + TwoLetterPoint[i][1], WHITE);
                        }
                        break;
                    case 4:
                        for (int i = 0; i < sizeof(FourLetterPoint) / sizeof(FourLetterPoint[0]); i++) {
                            display2.drawPixel(endLine4ChrX + FourLetterPoint[i][0], endLine4ChrY + FourLetterPoint[i][1], WHITE);
                        }
                        break;
                    case 6:
                        for (int i = 0; i < sizeof(SixLetterPoint) / sizeof(SixLetterPoint[0]); i++) {
                            display2.drawPixel(endLine4ChrX + SixLetterPoint[i][0], endLine4ChrY + SixLetterPoint[i][1], WHITE);
                        }
                        break;
                    case 8:
                        for (int i = 0; i < sizeof(EightLetterPoint) / sizeof(EightLetterPoint[0]); i++) {
                            display2.drawPixel(endLine4ChrX + EightLetterPoint[i][0], endLine4ChrY + EightLetterPoint[i][1], WHITE);
                        }
                        break;
                    default:
                        break;
                }
                

            } else {
                display2.drawLine(
                    centerX + (radius[internal] * radSin),
                    centerY - (radius[internal] * radCos),
                    centerX + (radius[external] * radSin),
                    centerY - (radius[external] * radCos),
                    WHITE
                );
            }
        }
    }
}

void radsCalc(int16_t x, int16_t y, int radius, float radSin, float radCos) {
    int endX = x + (radius * radSin);
    int endY = y - (radius * radCos);
    display2.drawPixel(endX, endY, WHITE);
}

void display2DrawSenseLine(float value) {
    int r = 45;
    float rad = value * PI / 180.0;
    float radSin = sin(rad);
    float radCos = cos(rad);

    for (int x = centerX - 1; x < centerX + 1; x++) {
        for (int y = centerY - 1; y < centerY + 1; y++) {
            display2.drawLine(x, y, x + (r * radSin), y - (r * radCos), WHITE);
        }
    }
}

//65, 55, 50, ?
void rads(int16_t x, int16_t y, int radius, float rad) {
    //float rad = angle * PI / 180.0;
    //display2.drawPixel(x, y, WHITE);
    int endX = x + (radius * sin(rad));
    int endY = y - (radius * cos(rad));
    display2.drawPixel(endX, endY, WHITE);
}