// ===== Build Switches =====
#define USE_LCD 0          // 0 = ปิด LCD เพื่อลดขนาด, 1 = เปิด (อาจทำให้แฟลชเต็ม)
#define USE_SOFT_SERVO 1   // 1 = จำลองบน Wokwi ใช้ซอฟต์แวร์เซอร์โว, 0 = ใช้ Servo.h (ฮาร์ดแวร์จริง)

#include <Wire.h>
#if USE_LCD
  #include <LiquidCrystal_I2C.h>
#endif

// ===== Pins (Nucleo-C031C6) =====
const int PIN_BTN_COIN = D2;
const int PIN_BTN_DROP = D3;

const int PIN_JOY_VERT = A0;  // X
const int PIN_JOY_HORZ = A1;  // Y
const int PIN_JOY_SEL  = A2;  // SEL

// Servo pins
const int PIN_SERVO_L = D5;   // PA8
const int PIN_SERVO_R = D6;   // PA9

// A4988 STEP/DIR
// X
const int X_STEP = D8;
const int X_DIR  = D7;
// Y
const int Y_STEP = D10;
const int Y_DIR  = D9;
// Z
const int Z_STEP = D12;
const int Z_DIR  = D11;

#if USE_LCD
  #define LCD_ADDR 0x27
  LiquidCrystal_I2C lcd(LCD_ADDR, 20, 4);
#endif

// ===== Game timing =====
const unsigned long GAME_MS   = 25000;
const unsigned long Z_DOWN_MS = 2000;
const int DEAD = 50;

unsigned long attempt = 0;
bool forced = false;

// ===== Claw Angles =====
const int CLAW_OPEN_L  = 30;
const int CLAW_CLOSE_L = 85;
const int CLAW_OPEN_R  = 150;
const int CLAW_CLOSE_R = 95;

// ===== Software Servo (50Hz, เบา) =====
#if USE_SOFT_SERVO
static inline int angleToUs(int d){ if(d<0)d=0; if(d>180)d=180; return 500 + (2000L*d)/180; }
volatile int servoL_us = angleToUs(CLAW_OPEN_L);
volatile int servoR_us = angleToUs(CLAW_OPEN_R);

void servoInit(){
  pinMode(PIN_SERVO_L, OUTPUT);
  pinMode(PIN_SERVO_R, OUTPUT);
  digitalWrite(PIN_SERVO_L, LOW);
  digitalWrite(PIN_SERVO_R, LOW);
}
static inline void setClawL(int deg){ servoL_us = angleToUs(deg); }
static inline void setClawR(int deg){ servoR_us = angleToUs(deg); }

// ยิงเฟรม 20ms (เรียกบ่อย ๆ ใน loop)
void servoFrameOnce(){
  unsigned long t0 = micros();
  digitalWrite(PIN_SERVO_L, HIGH);
  digitalWrite(PIN_SERVO_R, HIGH);
  int tmin = (servoL_us < servoR_us)? servoL_us : servoR_us;
  int tmax = (servoL_us > servoR_us)? servoL_us : servoR_us;

  delayMicroseconds(tmin);
  if(servoL_us < servoR_us) digitalWrite(PIN_SERVO_L, LOW);
  else if(servoR_us < servoL_us) digitalWrite(PIN_SERVO_R, LOW);

  delayMicroseconds(tmax - tmin);
  if(servoL_us >= servoR_us) digitalWrite(PIN_SERVO_L, LOW);
  else                       digitalWrite(PIN_SERVO_R, LOW);

  while (micros() - t0 < 20000) { /* keep 50Hz */ }
}
void sweepServoLOnce(){
  for(int d=CLAW_OPEN_L; d<=CLAW_CLOSE_L; d+=4){ setClawL(d); servoFrameOnce(); }
  for(int d=CLAW_CLOSE_L; d>=CLAW_OPEN_L; d-=4){ setClawL(d); servoFrameOnce(); }
}
void sweepServoROnce(){
  for(int d=CLAW_OPEN_R; d<=CLAW_CLOSE_R; d+=4){ setClawR(d); servoFrameOnce(); }
  for(int d=CLAW_CLOSE_R; d>=CLAW_OPEN_R; d-=4){ setClawR(d); servoFrameOnce(); }
}
#else
  #include <Servo.h>
  Servo clawL, clawR;
  void servoInit(){ clawL.attach(PIN_SERVO_L); clawR.attach(PIN_SERVO_R); clawL.write(CLAW_OPEN_L); clawR.write(CLAW_OPEN_R); }
  static inline void setClawL(int d){ clawL.write(d); }
  static inline void setClawR(int d){ clawR.write(d); }
  static inline void servoFrameOnce(){ /* HW timer */ }
  void sweepServoLOnce(){ for(int i=0;i<=20;i++){ setClawL(CLAW_OPEN_L + (CLAW_CLOSE_L-CLAW_OPEN_L)*i/20); delay(8); } for(int i=0;i<=20;i++){ setClawL(CLAW_CLOSE_L - (CLAW_CLOSE_L-CLAW_OPEN_L)*i/20); delay(8); } }
  void sweepServoROnce(){ for(int i=0;i<=20;i++){ setClawR(CLAW_OPEN_R + (CLAW_CLOSE_R-CLAW_OPEN_R)*i/20); delay(8); } for(int i=0;i<=20;i++){ setClawR(CLAW_CLOSE_R - (CLAW_CLOSE_R-CLAW_OPEN_R)*i/20); delay(8); } }
#endif

// ===== Debounce =====
struct Debounce {
  int pin; int last; unsigned long t;
  void begin(int p){ pin=p; pinMode(pin, INPUT_PULLUP); last=digitalRead(pin); t=millis(); }
  bool fell(){ int v=digitalRead(pin); unsigned long now=millis();
    if(v!=last){ last=v; t=now; }
    return (v==LOW) && (now - t > 20);
  }
} btnCoin, btnDrop;

// ===== Minimal Stepper Driver (เล็กกว่า AccelStepper มาก) =====
// ตำแหน่งจำลอง (step count)
volatile long posX=0, posY=0, posZ=0;

static inline void stepOnce(int pinSTEP, int pinDIR, bool dirPositive, volatile long &pos){
  digitalWrite(pinDIR, dirPositive ? HIGH : LOW);
  digitalWrite(pinSTEP, HIGH);
  delayMicroseconds(2);
  digitalWrite(pinSTEP, LOW);
  delayMicroseconds(2);
  pos += dirPositive ? 1 : -1;
}

// วิ่งด้วย "ความเร็ว" (สtep/วินาที) จากจอย: ใช้ตัวสะสมเวลาอย่างง่าย
struct AxisSpeed {
  int pinSTEP, pinDIR;
  volatile long* ppos;
  float v_sps;          // ความเร็วเป้าหมาย steps/s
  unsigned long last;   // ไมโครวินาทีที่ยิงสเต็ปล่าสุด
  AxisSpeed(int s,int d, volatile long* p):pinSTEP(s),pinDIR(d),ppos(p),v_sps(0),last(0){}
  void begin(){ pinMode(pinSTEP, OUTPUT); pinMode(pinDIR, OUTPUT); digitalWrite(pinSTEP, LOW); }
  void setSpeed(float v){ v_sps = v; if(v_sps<0) digitalWrite(pinDIR, LOW); else digitalWrite(pinDIR, HIGH); }
  void run(){
    float spd = v_sps; if(spd<0) spd = -spd;
    if (spd < 1.0f) return; // dead low speed
    unsigned long now = micros();
    unsigned long period = (unsigned long)(1000000.0f / spd); // us/step
    if (now - last >= period){
      // pulse
      digitalWrite(pinSTEP, HIGH);
      // สั้น ๆ
      __asm__ __volatile__("nop\n\t""nop\n\t""nop\n\t");
      digitalWrite(pinSTEP, LOW);
      last = now;
      // update pos
      if (v_sps >= 0) (*ppos)++; else (*ppos)--;
    }
  }
};

// ขยับแกนแบบ blocking ไปยัง target (ความเร็วคงที่)
void runAxisToBlocking(int pinSTEP,int pinDIR, volatile long &pos, long target, unsigned long period_us){
  bool dir = (target > pos);
  digitalWrite(pinDIR, dir ? HIGH : LOW);
  while (pos != target){
    digitalWrite(pinSTEP, HIGH);
    delayMicroseconds(2);
    digitalWrite(pinSTEP, LOW);
    delayMicroseconds( (int)period_us );
    pos += dir ? 1 : -1;
    servoFrameOnce(); // รักษา 50Hz ระหว่างบล็อค
  }
}

// สร้างแกน
AxisSpeed axX(X_STEP, X_DIR, &posX);
AxisSpeed axY(Y_STEP, Y_DIR, &posY);
AxisSpeed axZ(Z_STEP, Z_DIR, &posZ);

// ===== Utilities =====
#if USE_LCD
  static inline void lcdMsg(const char* a,const char* b,const char* c=nullptr,const char* d=nullptr){
    lcd.clear();
    if(a){ lcd.setCursor(0,0); lcd.print(a); }
    if(b){ lcd.setCursor(0,1); lcd.print(b); }
    if(c){ lcd.setCursor(0,2); lcd.print(c); }
    if(d){ lcd.setCursor(0,3); lcd.print(d); }
  }
#else
  static inline void lcdMsg(const char* a,const char* b,const char* c=nullptr,const char* d=nullptr){
    Serial.println(F("-----"));
    if(a) Serial.println(a);
    if(b) Serial.println(b);
    if(c) Serial.println(c);
    if(d) Serial.println(d);
  }
#endif

enum State { IDLE, PLAY, ZDOWN, ZUP, RETURNING, RELEASE, SHOW };
State st = IDLE;
unsigned long tStart = 0;

// ===== Setup =====
void setup(){
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(115200);

#if USE_LCD
  Wire.begin();
  lcd.init(); lcd.backlight();
#endif
  lcdMsg("Claw Ready (Lite)","Nucleo C031C6","COIN D2 DROP D3","JOY A0/A1 SEL:A2");

  btnCoin.begin(PIN_BTN_COIN);
  btnDrop.begin(PIN_BTN_DROP);
  pinMode(PIN_JOY_SEL, INPUT_PULLUP);

  servoInit();
  setClawL(CLAW_OPEN_L);
  setClawR(CLAW_OPEN_R);

  axX.begin(); axY.begin(); axZ.begin();
  digitalWrite(LED_BUILTIN, HIGH);
}

// ===== Loop =====
void loop(){
  bool coinPressed = btnCoin.fell() || (digitalRead(PIN_JOY_SEL)==LOW);

  switch(st){
    case IDLE:
      if (coinPressed){
        sweepServoLOnce();
        attempt++; forced = (attempt % 25 == 0);
        tStart = millis();
        lcdMsg("Time: 25s   Try#", "Move Joystick X/Y","DROP when ready","");
        st = PLAY;
      }
      break;

    case PLAY: {
      unsigned long elapsed = millis() - tStart;
      unsigned long left = (elapsed >= GAME_MS) ? 0 : (GAME_MS - elapsed);

      // จอย → ความเร็ว (steps/s) (ช่วง -1200..1200)
      int vert = analogRead(PIN_JOY_VERT) - 512;
      int horz = analogRead(PIN_JOY_HORZ) - 512;
      float vx=0, vy=0;
      if (vert >  DEAD) vx =  (float)vert / 511.0f * 1200.0f;
      if (vert < -DEAD) vx =  (float)vert / 511.0f * 1200.0f;
      if (horz >  DEAD) vy =  (float)horz / 511.0f * 1200.0f;
      if (horz < -DEAD) vy =  (float)horz / 511.0f * 1200.0f;

      axX.setSpeed(vx);
      axY.setSpeed(vy);
      axX.run();
      axY.run();

      if (btnDrop.fell() || left == 0){
        // ลง ~2 วินาที ที่ 900 steps/s → ระยะประมาณ 1800 steps
        long stroke = (long)(900.0f * (Z_DOWN_MS / 1000.0f));
        long target = posZ - stroke; // ถ้าทิศกลับ ให้เปลี่ยนเป็น +stroke
        lcdMsg("Dropping...","Z Down...");
        runAxisToBlocking(Z_STEP, Z_DIR, posZ, target, 1000000UL/900UL);
        st = ZDOWN;
      }
      servoFrameOnce(); // รักษา 50Hz
    } break;

    case ZDOWN:
      sweepServoROnce();
      lcdMsg("Lifting...","Z Up...");
      runAxisToBlocking(Z_STEP, Z_DIR, posZ, 0, 1000000UL/900UL);
      st = ZUP;
      break;

    case ZUP:
      lcdMsg("Returning...","Z->0 then X,Y->0");
      // Z -> 0 (แล้ว), X -> 0, Y -> 0
      runAxisToBlocking(X_STEP, X_DIR, posX, 0, 1000000UL/800UL);
      runAxisToBlocking(Y_STEP, Y_DIR, posY, 0, 1000000UL/800UL);

      sweepServoROnce();
      st = RELEASE;
      break;

    case RELEASE:
      lcdMsg(forced ? "Guaranteed WIN!" : "Release...", " ");
      delay(400);
      st = SHOW;
      break;

    case SHOW:
      lcdMsg(forced ? "You WIN!" : "Try again", "Insert coin (D2)","or press JOY", "");
      posX = posY = posZ = 0;
      st = IDLE;
      break;
  }

  // สัญญาณชีพ
  digitalWrite(LED_BUILTIN, (millis()/500)%2);
}