/*

    Cute Useless Robot (´・ω・`)

    Developed by tobychui
    Idea come from Kairoshi

*/

/* Libraries */
#include <MD_MAX72xx.h>
#include <SPI.h>
#include <SD.h>
#include <Servo.h> //Require ESP32Servo, not the Arduino build in Servo.h

/* Pins Definations */
#define STP_DATA_PIN 4    // Stepper Shift Register DS
#define STP_CLOCK_PIN 16  // Stepper Shift Register SH_CP
#define STP_LATCH_PIN 17  // Stepper Shift Register ST_CP

#define SERVO_SWITCH 27 //Servo to push the switch
#define SERVO_COVER 14  //Servo to push the cover

#define DP_CLK_PIN   32 //Display CLK
#define DP_DATA_PIN  33 //Display DIN
#define DP_CS_PIN    25 //Display CS

#define SD_CS_PIN 5 //SD Card CS pin

/* Display settings generated by trial and error. Don't touch these */
#define HARDWARE_TYPE MD_MAX72XX::DR1CR0RR0_HW
#define MAX_DEVICES 8

/* Hardware Type Definations */
Servo servoSwitchPusher;
Servo servoCoverPusher;
MD_MAX72XX mx = MD_MAX72XX(HARDWARE_TYPE, DP_DATA_PIN, DP_CLK_PIN, DP_CS_PIN, MAX_DEVICES);

void setup() {
  Serial.begin(115200);
  
  /* Stepper IO */
  pinMode(STP_DATA_PIN, OUTPUT);
  pinMode(STP_CLOCK_PIN, OUTPUT);
  pinMode(STP_LATCH_PIN, OUTPUT);
  standbySteppers();
  
  /* Servo IO */
  servoSwitchPusher.attach(SERVO_SWITCH);
  servoCoverPusher.attach(SERVO_COVER);
  /* SD Card */
  // Initialize SD card
  if (!SD.begin(SD_CS_PIN)) {
    Serial.println("[Error] Unable to mount SD card");
  }
  
  /* Display Module */
  mx.begin();
  setDisplayBrightness(0x4);
  renderFrame();
  delay(5000);
}

void loop() {
  //Stepper test
  forward(50);
  delay(3000);
  backward(50);
  delay(3000);
  rotateAntiClockwise(100);
  delay(1000);
  //blink
  loadAndRender("/b.bin");
  delay(500);
  loadAndRender("/a.bin");
  delay(2000);
  //rotate backward
  rotateClockwise(100);
  delay(3000);
  //Servo test
  servoCoverPusher.write(90);
  delay(1000);
  servoSwitchPusher.write(180);
  delay(1000);
  servoCoverPusher.write(0);
  servoSwitchPusher.write(0);
  delay(3000);
  
}