#include <AccelStepper.h>
#include <Ps3Controller.h>
#include <Wire.h>
#include <LIDARLite.h>
#include <TFT_eSPI.h>

// MOTORS
/////////////////////////////////////////////////////////////////////
const int DIR_PAN = 27;
const int STEP_PAN = 33;
const int DIR_TILT = 13;
const int STEP_TILT = 12;
AccelStepper panStepper(AccelStepper::DRIVER, STEP_PAN, DIR_PAN);
AccelStepper tiltStepper(AccelStepper::DRIVER, STEP_TILT, DIR_TILT);
// Stepper configuration
const int MICRO_MODE = 32;
const int STEPS_PER_REV = 200 * MICRO_MODE;
const float STEPS_PER_DEGREE = STEPS_PER_REV / 360.0;
const float MAX_SPEED = 2000.0;
const float ACCELERATION = 1000.0;
const float CONTINUOUS_SPEED = 800.0;

// LED (LASER POINTER)
/////////////////////////////////////////////////////////////////////
const int LED_PIN = 4;

// Globals
/////////////////////////////////////////////////////////////////////
bool ledState = false;
bool panMoving = false;
bool tiltMoving = false;

// LIDAR
/////////////////////////////////////////////////////////////////////
LIDARLite myLidarLite;

// TFT
/////////////////////////////////////////////////////////////////////
TFT_eSPI tft = TFT_eSPI();
TFT_eSprite bspr = TFT_eSprite(&tft);
TFT_eSprite spr2 = TFT_eSprite(&tft);

#include "laser_warning.h"
#define GFXFF 1
#define CF_OL24 &Orbitron_Light_24
#define CF_OL32 &Orbitron_Light_32
// Display variables
int xhair_color = TFT_RED;
int xhair_x = 0;
int xhair_y = 0;
int xhair_dx = 2;
int xhair_dy = 2;
int ringR = 0;
int currentDistance = 0;
int biasCount = 0;
unsigned long lastLidarRead = 0;
const int lidarReadInterval = 100; // Read LIDAR every 50ms

// Non blocking delays
/////////////////////////////////////////////////////////////////////
unsigned long lastDisplayUpdate = 0;
const int displayUpdateInterval = 150;


// Helper functions
/////////////////////////////////////////////////////////////////////
void onConnect() {
    Serial.println("PS3 Controller Connected!");
}

void notify() {
    if (Ps3.event.button_down.cross) {
        ledState = !ledState;
        digitalWrite(LED_PIN, ledState);
        Serial.print("LED ");
        Serial.println(ledState ? "ON" : "OFF");
    }
    if (Ps3.event.button_down.triangle) {
        panStepper.setCurrentPosition(0);
        tiltStepper.setCurrentPosition(0);
        Serial.println("Pan/Tilt positions zeroed!");
    }
}

void readLidarDistance() {
    unsigned long currentMillis = millis();
    if (currentMillis - lastLidarRead >= lidarReadInterval) {
        lastLidarRead = currentMillis;
        if (biasCount == 0) {
            currentDistance = myLidarLite.distance();
            biasCount = 1;
        } else {
            currentDistance = myLidarLite.distance(false);
            biasCount++;
            if (biasCount >= 100) {
                biasCount = 0;
            }
        }
    }
}

void handleMotorControl() {
    if (!Ps3.isConnected()) {
        panStepper.stop();
        tiltStepper.stop();
        panMoving = false;
        tiltMoving = false;
        return;
    }
    int lx = Ps3.data.analog.stick.lx;
    if (abs(lx) >= 10) {
        float panSpeed = map(abs(lx), 10, 127, 100, CONTINUOUS_SPEED);
        panStepper.setMaxSpeed(panSpeed);
        if (lx > 0) {
            panStepper.moveTo(panStepper.currentPosition() + 1000000);
        } else {
            panStepper.moveTo(panStepper.currentPosition() - 1000000);
        }
        panMoving = true;
    } else {
        if (panMoving) {
            panStepper.stop();
            panMoving = false;
        }
    }

    int ry = Ps3.data.analog.stick.ry;
    
    if (abs(ry) >= 10) {
        float tiltSpeed = map(abs(ry), 10, 127, 100, CONTINUOUS_SPEED);
        tiltStepper.setMaxSpeed(tiltSpeed);
        
        // Set direction and move continuously
        if (ry > 0) {
            tiltStepper.moveTo(tiltStepper.currentPosition() + 1000000);
        } else {
            tiltStepper.moveTo(tiltStepper.currentPosition() - 1000000);
        }
        tiltMoving = true;
    } else {
        if (tiltMoving) {
            tiltStepper.stop();
            tiltMoving = false;
        }
    }
}

void drawGrid() {
    int w = bspr.width();
    int h = bspr.height();
    
    for (int x = 0; x < w; x += 20) {bspr.drawFastVLine(x, 0, h, 0x18E3);}
    for (int y = 0; y < h; y += 20) {bspr.drawFastHLine(0, y, w, 0x18E3);}
    int bracket_size = 20;
    bspr.drawFastHLine(0, 0, bracket_size, 0x07E0);
    bspr.drawFastVLine(0, 0, bracket_size, 0x07E0);
    bspr.drawFastHLine(w - bracket_size, 0, bracket_size, 0x07E0);
    bspr.drawFastVLine(w - 1, 0, bracket_size, 0x07E0);
    bspr.drawFastHLine(0, h - 1, bracket_size, 0x07E0);
    bspr.drawFastVLine(0, h - bracket_size, bracket_size, 0x07E0);
    bspr.drawFastHLine(w - bracket_size, h - 1, bracket_size, 0x07E0);
    bspr.drawFastVLine(w - 1, h - bracket_size, bracket_size, 0x07E0);
}

void drawLidarDistance(int distance) {
    bspr.setFreeFont(CF_OL32);  
    bspr.setTextColor(TFT_WHITE, TFT_BLACK);
    bspr.drawString("LazR:", 60, 30, GFXFF);
    
    if (distance < 50) {
        bspr.setTextColor(TFT_RED, TFT_BLACK);
    } else if (distance < 200) {
        bspr.setTextColor(TFT_WHITE, TFT_BLACK);
    } else {
        bspr.setTextColor(TFT_LIGHTGREY, TFT_BLACK);
    }

    char buffer[16];
    sprintf(buffer, "%d", distance);
    bspr.drawString(buffer, 172, 30, GFXFF);
}

void drawPan(long pan) {
    bspr.setTextColor(TFT_CYAN, TFT_BLACK);
    char buffer[32];
    sprintf(buffer, "pan: %ld", pan);
    bspr.drawString(buffer, 70, 85, GFXFF);
}

void drawTilt(long tilt) {
    bspr.setTextColor(TFT_YELLOW, TFT_BLACK);
    char buffer[32];
    sprintf(buffer, "tilt: %ld", tilt);
    bspr.drawString(buffer, 70, 115, GFXFF);
}

void drawCrosshair(int x, int y, int color) {
    bspr.fillCircle(x, y, 3, color);
    bspr.drawCircle(x, y, 5, color);
    bspr.drawLine(x, y - 10, x, y + 10, color);
    bspr.drawLine(x - 10, y, x + 10, y, color);
    ringR = (ringR + 1) % 10;
    for (int i = 0; i < 4; i++) {bspr.drawCircle(x, y, ringR + i * 10, color);}
}

float stepsToDegrees(long steps) {
    return steps / STEPS_PER_DEGREE;
}

void setup() {
    Serial.begin(115200);

    pinMode(LED_PIN, OUTPUT);
    digitalWrite(LED_PIN, LOW);

    panStepper.setMaxSpeed(MAX_SPEED);
    panStepper.setAcceleration(ACCELERATION);
    panStepper.setCurrentPosition(0);

    tiltStepper.setMaxSpeed(MAX_SPEED);
    tiltStepper.setAcceleration(ACCELERATION);
    tiltStepper.setCurrentPosition(0);

    myLidarLite.begin(0, true);
    myLidarLite.configure(0);

    tft.begin();
    tft.setRotation(3);
    tft.fillScreen(TFT_BLACK);  
    tft.setTextDatum(MC_DATUM);

    bspr.createSprite(tft.width(), tft.height()); 
    bspr.fillSprite(TFT_BLACK);
    bspr.setTextDatum(MC_DATUM);

    spr2.createSprite(60, 60);
    spr2.fillSprite(TFT_TRANSPARENT);
    int16_t x = 0, y = 0;
    for (int w = 0; w < LASER_WARNING_WIDTH * LASER_WARNING_HEIGHT; w++) {
        uint16_t color = laser_warning[w];
        uint8_t r = (color >> 11) & 0x1F;
        uint8_t g = (color >> 5) & 0x3F;
        uint8_t b = color & 0x1F;
        uint16_t brightness = (r * 255 / 31 + g * 255 / 63 + b * 255 / 31) / 3;
        if (brightness > 200) { color = 0x0000; }
        spr2.drawPixel(x, y, color);
        x++;
        if (x >= LASER_WARNING_WIDTH) { x = 0; y++; }
    }

    xhair_x = bspr.width() / 2;
    xhair_y = bspr.height() / 2;

    // PS3 controller
    Ps3.attach(notify);
    Ps3.attachOnConnect(onConnect);
    Ps3.begin("0C:DC:7E:CC:64:0E");  // <-- Replace here!!!

    Serial.println("Ready. Pan/Tilt Robot with LIDAR initialized (AccelStepper).");
    Serial.println("Controls: Left stick X = Pan, Right stick Y = Tilt");
    Serial.println("Cross = LED toggle, Triangle = Zero positions");
}

void loop() {

    handleMotorControl();
    
    panStepper.run();
    tiltStepper.run();

    readLidarDistance();

    unsigned long currentMillis = millis();
    if (currentMillis - lastDisplayUpdate >= displayUpdateInterval) {
        lastDisplayUpdate = currentMillis;

        xhair_x += xhair_dx;
        xhair_y += xhair_dy;
        if (xhair_x <= 10 || xhair_x >= bspr.width() - 10) xhair_dx *= -1;
        if (xhair_y <= 10 || xhair_y >= bspr.height() - 10) xhair_dy *= -1;

        bspr.fillSprite(TFT_BLACK);

        drawGrid();
        spr2.pushToSprite(&bspr, bspr.width() - 60, bspr.height() - 60, TFT_TRANSPARENT);

        drawLidarDistance(currentDistance);

        bspr.setFreeFont(CF_OL24);  
        
        drawPan(stepsToDegrees(panStepper.currentPosition()));
        drawTilt(-stepsToDegrees(tiltStepper.currentPosition()));
        drawCrosshair(xhair_x, xhair_y, xhair_color);

        bspr.pushSprite(0, 0);
    }

}