#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "radDisplay.h"
#include "radDisplaySettings.h"

Adafruit_SSD1306 radDisplay(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire1, -1);

void initRadDisplay() {
    Wire1.begin(RAD_I2C_SDA, RAD_I2C_SCL);
    if(!radDisplay.begin(SSD1306_SWITCHCAPVCC, RAD_DISPLAY_I2C_ADDRESS)) {
        //Serial.println("SSD1306 #2 allocation failed");
        for(;;);
    }

    changeRad(0);
}

void changeRad(float value) {
    radDisplay.clearDisplay();
    radDisplay.fillScreen(RADBGRND);
    overlayRadDisplay();
    drawLineRadDisplay(convertValueToRadDisplay(value));
    radDisplay.display();
}

float convertValueToRadDisplay(float value) {
    return mapFloat(value, 0, 10, 90, -90);
}

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

    for (int x = radCenterX - 1; x < radCenterX + 1; x++) {
        for (int y = radCenterY - 1; y < radCenterY + 1; y++) {
            radDisplay.drawLine(x, y, x + (r * radSin), y - (r * radCos), RADCOLOR);
        }
    }
}

//todo optimaze
void calcRad(int16_t x, int16_t y, int radius, float radSin, float radCos) {
    int endX = x + (radius * radSin);
    int endY = y - (radius * radCos);
    radDisplay.drawPixel(endX, endY, RADCOLOR);
}

void overlayRadDisplay() {
    radDisplay.drawPixel(radCenterX, radCenterY, RADCOLOR);
    for (int r = 1; r < 5; r++) {
        radDisplay.drawCircle(radCenterX, radCenterY, r, RADCOLOR);
    }

    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);

        calcRad(radCenterX, radCenterY, radRadius[internal], radSin, radCos);
        calcRad(radCenterX, radCenterY, radRadius[external], radSin, radCos);

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

                int endLine4ChrX = radCenterX + ((radRadius[extended]+5) * radSin);
                int endLine4ChrY = radCenterY - ((radRadius[extended]+5) * radCos);

                radDisplay.drawLine(
                    radCenterX + (radRadius[internal] * radSin),
                    radCenterY - (radRadius[internal] * radCos),
                    endLineX,
                    endLineY,
                    RADCOLOR
                );

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

            } else {
                radDisplay.drawLine(
                    radCenterX + (radRadius[internal] * radSin),
                    radCenterY - (radRadius[internal] * radCos),
                    radCenterX + (radRadius[external] * radSin),
                    radCenterY - (radRadius[external] * radCos),
                    RADCOLOR
                );
            }
        }
    }
}