#define USE_MOTOR 1
#define USE_LED 1
#define USE_DISPLAY 1

#define pinButtonMode 9
#define pinDiag 6

// Main LED strip setup
#define pinLED 3
#define NUM_LEDS 20
#define LED_PART 10
#define BRIGHTNESS 250
#define LED_TYPE WS2811

// Relay controlled motor
#define pinMotor 7

#if USE_LED
#include <FastLED.h>
int cycles = 100;
int cycle_duration = 100;
CRGB leds[NUM_LEDS];

CRGB color_red;
CRGB color_blue;
CRGB color_green;
#endif

#if USE_DISPLAY
#include <Wire.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32

#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#endif

// Program state

bool stateButtonMode = false;
int programId = 0;
bool programChanged = true;
#define MAX_PROGRAMS 3
bool flag_motor = false;
bool flag_lighting = false;


void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(pinButtonMode, INPUT);
#if USE_LED
  // Calculate colours
  hsv2rgb_spectrum(CHSV(4, 255, 100), color_red);
  hsv2rgb_spectrum(CHSV(170, 255, 100), color_blue);
  hsv2rgb_spectrum(CHSV(90, 255, 100), color_green);
  pinMode(pinLED, OUTPUT);
#endif
  pinMode(pinDiag, OUTPUT);
#if USE_MOTOR
  pinMode(pinMotor, OUTPUT);
#endif
  
#if USE_LED
  // Main LED strip
  FastLED.addLeds<LED_TYPE, pinLED, RGB>(leds, NUM_LEDS);
#endif
#if USE_DISPLAY
  Serial.begin(9600);
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
    pinMode(pinLED, OUTPUT);
    digitalWrite(pinLED, HIGH);
  }
#endif
}

void loop() {
  int buttonState = digitalRead(pinButtonMode);
  if (buttonState && !stateButtonMode) {
    programId = (programId + 1) % MAX_PROGRAMS;
    programChanged = true;
    stateButtonMode = true;
  }
  if (!buttonState) {
    stateButtonMode = false;
  }
  switch (programId)
  {
  case 0:
    program_off();
    break;
  case 1:
    program_still();
    break;
  case 2:
    program_rotate();
    break;
  default:
    break;
  }

  if (programChanged) {
    update_screen();
    programChanged = false;
  }
}

// Utility for updating LEDs
void fill_segmented(CRGB c1, CRGB c2)
{
#if USE_LED
  //fill_solid(leds, LED_PART, c1);
  fill_gradient_RGB(leds, LED_PART, CRGB::Black ,c1);
  fill_gradient_RGB(leds + LED_PART, NUM_LEDS - LED_PART, CRGB::Black, c2);
  FastLED.show(); 
#endif
}
void set_motor(bool flag)
{
#if USE_MOTOR
  if (flag) {
    digitalWrite(pinMotor, HIGH);
    flag_motor = true;
  }
  else {
    digitalWrite(pinMotor, LOW);
    flag_motor = false;
  }
#endif
}

// Update current display status
void update_screen()
{
#if USE_DISPLAY
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("Yasaka K.");
  display.print("P");
  display.print(programId);
  display.print(" ");
  if (flag_motor) {
    display.print("M");
  }
  if (flag_lighting) {
    display.print("L");
  }
  display.display();
#endif
}

void program_off()
{
  if (programChanged)
  {
    set_motor(false);
#if USE_LED
    flag_lighting = false;
    fill_solid(leds, NUM_LEDS, CRGB::Black);
    FastLED.show();
#endif
  }
}
void program_still()
{
  if (programChanged)
  {
    set_motor(false);
#if USE_LED
    flag_lighting = true;
    fill_segmented(CRGB::Green, CRGB::Orange);
    FastLED.show();
#endif
  }
}
void program_rotate()
{
  if (programChanged)
  {
    set_motor(true);
  }
#if USE_LED
  flag_lighting = true;
  fill_segmented(CRGB::Green, CRGB::Orange);
  delay(cycle_duration/2);
  fill_solid(leds, NUM_LEDS, CRGB::Black);
  FastLED.show();
  delay(cycle_duration/2);
#endif
}