// Child's busy board. All pin numbers are Raspberry Pi Pico GPIO numbers.
#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_GC9A01A.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Servo.h>
#include <algorithm>
#include <math.h>
#include "audio_effects.h"
constexpr int POT=26, MASTER_PIN=27, CONTROL_LIGHT_POWER=28;
// Contact wiring: COM=GND, NC=input with 3.3V pull-up, NO=LED negative.
// NC grounded while OFF => LOW; NC disconnected while ON => HIGH.
constexpr int DC=16, TFT_SCK_PIN=18, TFT_MOSI_PIN=19;
constexpr int SDA_PIN=0, SCL_PIN=1, CS=17, METER=22;
constexpr int BUTTON_PINS[3]={2,3,4};
constexpr int ROTARY_PINS[3]={5,6,7};
constexpr int AUDIO_PIN=20; // GP21 is spare.
AudioEffects audio;
constexpr int BAR_PINS[8]={8,9,10,11,12,13,14,15};
Adafruit_GC9A01A tft(&SPI, DC, CS, -1);
Adafruit_MPU6050 imu;
Servo needle;
GFXcanvas16 frame(240,240); // 115200-byte frame: avoids flashing/flickering redraws.
struct Debounce { bool raw=false, stable=false; uint32_t changed=0; } controls[7];
bool latched[3]={false,false,false}, powered=false, imuOK=false;
float roll=0, pitch=0, shownRoll=0, shownPitch=0, speed=0;
uint32_t bootAt=0, feedbackAt=0, lastSample=0, lastDraw=0;
int feedback=-1;
const uint16_t BLACK=0, WHITE=0xffff, YELLOW=0xffe0;
uint8_t readControls() {
  uint8_t bits=digitalRead(MASTER_PIN)==LOW ? 1 : 0;
  for(int i=0;i<3;i++) if(digitalRead(BUTTON_PINS[i])==HIGH) bits|=1u<<(i+1);
  for(int i=0;i<3;i++) if(digitalRead(ROTARY_PINS[i])==HIGH) bits|=1u<<(i+4);
  return bits;
}
void writeLights(uint16_t bits) {
  for(int i=0;i<8;i++) digitalWrite(BAR_PINS[i],(bits>>i)&1);
  // GP28 powers only six low-current simulated LEDs. All six switches
  // light their own LEDs directly through NO; NC is the separate input.
  // Hardware lamps instead use the master-switched 5V supply, never GP28.
  digitalWrite(CONTROL_LIGHT_POWER,powered?HIGH:LOW);
}
void label(int x,int y,const char *s,int size=2) {
  frame.setTextSize(size); frame.setTextColor(WHITE); frame.setCursor(x,y); frame.print(s);
}
// Wokwi custom-chip SPI receives bytes. Avoid the RP2040 library's
// optimized 16-bit pixel transfers: send big-endian rows via its 8-bit path.
void presentFrame() {
  uint16_t row[240];
  const uint16_t *pixels=frame.getBuffer();
  tft.startWrite();
  tft.setAddrWindow(0,0,240,240);
  for(int y=0;y<240;y++) {
    for(int x=0;x<240;x++) {
      uint16_t c=pixels[y*240+x];
      row[x]=uint16_t((c>>8)|(c<<8));
    }
    // block=true, bigEndian=true: RP2040 uses 8-bit spi_write_blocking.
    tft.writePixels(row,240,true,true);
  }
  tft.endWrite();
}
void drawFrame(uint32_t now) {
  frame.fillScreen(BLACK);
  if(!powered) { presentFrame(); return; }
  float r=shownRoll*DEG_TO_RAD, sn=sinf(r), cs=cosf(r);
  float offset=constrain(shownPitch,-45.f,45.f)*2;
  // Positive pitch moves the horizon downward; positive roll banks it clockwise.
  for(int y=2;y<238;y++) {
    if(abs(y-120)>117) continue;
    int half=int(sqrtf(117.f*117.f-(y-120.f)*(y-120.f)));
    for(int x=120-half;x<=120+half;x++) {
      float v=-(x-120)*sn+(y-120)*cs-offset;
      frame.drawPixel(x,y, fabsf(v)<1.2f?WHITE:(v<0?0x349f:0x9204));
    }
  }
  frame.drawCircle(120,120,118,WHITE);
  // Fixed aircraft symbol, with pitch ticks moving with the horizon.
  for(int degrees=-20;degrees<=20;degrees+=10) {
    if(!degrees) continue;
    float yy=offset-degrees*2;
    frame.drawLine(120-15*cs-yy*sn,120-15*sn+yy*cs,
                   120+15*cs-yy*sn,120+15*sn+yy*cs,WHITE);
  }
  frame.drawFastHLine(60,120,45,YELLOW); frame.drawFastHLine(135,120,45,YELLOW);
  frame.drawLine(105,120,120,130,YELLOW); frame.drawLine(120,130,135,120,YELLOW);
  char text[32]; snprintf(text,sizeof(text),"SPEED %03d",int(speed*100)); label(60,190,text);
  if(now-bootAt<3000) {
    frame.fillRect(37,60,166,48,BLACK); label(54,68,"BUS 5.00 V"); label(48,90,"SIMULATED",1);
  }
  if(feedback>=0 && now-feedbackAt<2200) {
    frame.fillRect(40,45,160,48,BLACK);
    snprintf(text,sizeof(text),"%c : %s",'A'+feedback,latched[feedback]?"ON":"OFF"); label(54,54,text);
    if(feedback==0) frame.fillCircle(120,85,6,YELLOW);
    if(feedback==1) frame.fillTriangle(114,91,126,91,120,79,YELLOW);
    if(feedback==2) frame.fillRect(114,79,12,12,YELLOW);
  }
  if(!imuOK) { frame.fillRect(42,145,156,20,BLACK); label(46,150,"MPU NOT FOUND",1); }
  presentFrame();
}
void setup() {
  audio.begin(AUDIO_PIN);
  pinMode(MASTER_PIN,INPUT_PULLUP);
  for(int p:BUTTON_PINS) pinMode(p,INPUT_PULLUP);
  for(int p:ROTARY_PINS) pinMode(p,INPUT_PULLUP);
  for(int p:BAR_PINS) pinMode(p,OUTPUT);
  pinMode(CONTROL_LIGHT_POWER,OUTPUT); writeLights(0);
  analogReadResolution(12);
  // Arduino-Pico core: assign SPI/I2C pins before calling begin().
  SPI.setSCK(TFT_SCK_PIN); SPI.setTX(TFT_MOSI_PIN);
  // No TFT MISO connection is needed: the display is write-only.
  // Leave RX at its default; tft.begin() configures the DC GPIO as output.
  SPI.begin(); tft.begin(16000000);
  Wire.setSDA(SDA_PIN); Wire.setSCL(SCL_PIN); Wire.begin();
  imuOK=imu.begin(0x68,&Wire);
  if(imuOK) { imu.setAccelerometerRange(MPU6050_RANGE_2_G); imu.setGyroRange(MPU6050_RANGE_250_DEG); imu.setFilterBandwidth(MPU6050_BAND_21_HZ); }
  needle.attach(METER,500,2400); needle.write(0);
  // Baseline contact states: no fake press/jingle when power is first enabled.
  uint8_t initial=readControls();
  for(int i=0;i<7;i++) controls[i].raw=controls[i].stable=bool(initial&(1u<<i));
  powered=controls[0].stable; bootAt=millis();
  for(int i=0;i<3;i++)latched[i]=controls[i+1].stable;
  writeLights(0);
  lastSample=millis(); drawFrame(lastSample);
}
void loop() {
  uint32_t now=millis(); audio.tick(now); uint8_t bits=readControls();
  bool changed[7]={};
  for(int i=0;i<7;i++) {
    bool raw=bits&(1u<<i);
    if(raw!=controls[i].raw){controls[i].raw=raw;controls[i].changed=now;}
    if(raw!=controls[i].stable && now-controls[i].changed>=30){controls[i].stable=raw;changed[i]=true;}
  }
  if(changed[0]) {
    powered=controls[0].stable;feedback=-1;speed=0;roll=pitch=shownRoll=shownPitch=0;
    audio.stop();
    if(powered) {
      bootAt=now;
      // Rebaseline after master ON so pre-existing latch states cause no sounds.
      for(int i=1;i<7;i++) {
        controls[i].raw=controls[i].stable=bool(bits&(1u<<i));
        controls[i].changed=now;changed[i]=false;
      }
    }
  }
  for(int i=0;i<3;i++) {
    latched[i]=controls[i+1].stable; // Real physical latch state, no software toggle.
    if(powered && changed[i+1]) {
      feedback=i;feedbackAt=now;
      if(latched[i])audio.request(AudioEffects::CONFIRM);
    }
    if(powered && changed[i+4])
      audio.request(controls[i+4].stable?AudioEffects::ROTARY_ON:AudioEffects::ROTARY_OFF);
  }
  audio.tick(millis());
  if(now-lastSample>=20) {
    float dt=std::min((now-lastSample)/1000.f,0.1f); lastSample=now;
    uint32_t rawPot=analogRead(POT);
    float pot=constrain(rawPot/4095.f,0.f,1.f);
    speed+=( (powered?pot:0)-speed )*(1-expf(-dt/0.7f));
    needle.write(powered?int(speed*180):0); // Needle proxy, NOT a 0-5V meter drive.
    if(powered && imuOK) {
      sensors_event_t a,g,t; imu.getEvent(&a,&g,&t);
      float ar=atan2f(a.acceleration.y,a.acceleration.z)*RAD_TO_DEG;
      float ap=atan2f(-a.acceleration.x,hypotf(a.acceleration.y,a.acceleration.z))*RAD_TO_DEG;
      float blend=expf(-dt/0.5f);
      roll=blend*(roll+g.gyro.x*RAD_TO_DEG*dt)+(1-blend)*ar;
      pitch=blend*(pitch+g.gyro.y*RAD_TO_DEG*dt)+(1-blend)*ap;
      float slow=1-expf(-dt/1.2f); // Deliberately slow attitude response.
      shownRoll+=(roll-shownRoll)*slow; shownPitch+=(pitch-shownPitch)*slow;
    }
    int count=std::min(8,int((rawPot*8)/4095));
    uint16_t lights=powered?uint16_t((1u<<count)-1):0;
    writeLights(lights);
  }
  if(now-lastDraw>=100) { lastDraw=now; drawFrame(now); }
  audio.tick(millis());
  delay(1);
}