// ===== Pins (Nucleo-C031C6) =====
const int PIN_BTN_COIN = D2;
const int PIN_BTN_DROP = D3;
const int PIN_JOY_VERT = A0;
const int PIN_JOY_HORZ = A1;
const int PIN_JOY_SEL  = A2;

const int PIN_SERVO_L = D5;
const int PIN_SERVO_R = D6;

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

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

// ===== 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 =====
volatile int servoL_us = 500 + (CLAW_OPEN_L*2000)/180;
volatile int servoR_us = 500 + (CLAW_OPEN_R*2000)/180;

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 = 500 + (deg*2000)/180; }
static inline void setClawR(int deg){ servoR_us = 500 + (deg*2000)/180; }

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);
  digitalWrite(PIN_SERVO_L,servoL_us>servoR_us?HIGH:LOW);
  digitalWrite(PIN_SERVO_R,servoR_us>servoL_us?HIGH:LOW);
  delayMicroseconds(tmax-tmin);
  digitalWrite(PIN_SERVO_L,LOW); digitalWrite(PIN_SERVO_R,LOW);
  while(micros()-t0<20000){}
}

// ===== 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)&&(millis()-t>20);}
} btnCoin, btnDrop;

// ===== Minimal Stepper =====
volatile long posX=0,posY=0,posZ=0;

struct AxisSpeed{
  int pinSTEP,pinDIR; volatile long* ppos; int v_sps; 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(int v){ v_sps=v; digitalWrite(pinDIR,v>=0?HIGH:LOW); }
  void run(){
    int spd=v_sps; if(spd<0) spd=-spd; if(spd<1) return;
    unsigned long now=micros(); unsigned long period=1000000/spd;
    if(now-last>=period){
      digitalWrite(pinSTEP,HIGH); digitalWrite(pinSTEP,LOW); last=now;
      *ppos += v_sps>=0?1:-1;
    }
  }
};

AxisSpeed axX(X_STEP,X_DIR,&posX);
AxisSpeed axY(Y_STEP,Y_DIR,&posY);
AxisSpeed axZ(Z_STEP,Z_DIR,&posZ);

// ===== State =====
enum State{IDLE,PLAY,ZDOWN,ZUP,RELEASE};
State st=IDLE;
unsigned long tStart=0;
int servoL_cur=CLAW_OPEN_L, servoR_cur=CLAW_OPEN_R;

// ===== Setup =====
void setup(){
  pinMode(LED_BUILTIN,OUTPUT);
  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();
}

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

  switch(st){
    case IDLE:
      if(coinPressed){ tStart=millis(); st=PLAY; }
      break;

    case PLAY:{
      int vert = analogRead(PIN_JOY_VERT)-512;
      int horz = analogRead(PIN_JOY_HORZ)-512;
      axX.setSpeed(abs(vert)>DEAD?vert/2:0);
      axY.setSpeed(abs(horz)>DEAD?horz/2:0);
      axX.run(); axY.run();

      if(btnDrop.fell() || millis()-tStart>GAME_MS){ axZ.setSpeed(-900); st=ZDOWN; }
      servoFrameOnce();
    } break;

    case ZDOWN:
      axZ.run();
      servoR_cur += 8; if(servoR_cur>CLAW_CLOSE_R) servoR_cur=CLAW_OPEN_R;
      setClawR(servoR_cur);
      if(posZ<-1800){ axZ.setSpeed(0); st=ZUP; }
      break;

    case ZUP:
      axZ.setSpeed(900); axZ.run();
      if(posZ>=0){ posZ=0; axZ.setSpeed(0); st=RELEASE; }
      break;

    case RELEASE:
      servoL_cur += 8; if(servoL_cur>CLAW_CLOSE_L) servoL_cur=CLAW_OPEN_L;
      setClawL(servoL_cur);
      st=IDLE;
      break;
  }

  digitalWrite(LED_BUILTIN,(millis()/500)%2);
}