#include <Adafruit_NeoPixel.h>

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

//define pins---------------------------------------------------------
//servos
//define servos---------------------------------------------------------------------------------
#define SERVOMIN 500
#define SERVOMAX 2400

#define servo_antenna_short 1
#define servo_antenna_long 5

//angles
int servo_angle_start = 0;
int servo_angle_short = 7;
int servo_angle_long = 10;

//status of defrom---------------------------------------
int deform_status;  //0=until, 1=done

//I2C---------------------------------------
//21 SDA
//22 SCL

//SWs-----------------------------------------
#define sw 23

//NeoPixels------------------------------------
#define pin_engine 13

int pixels_engine = 14;  //50mA x 16 = 800mA

int led_engine_brt_max = 255;
int led_engine_brt_0 = 20;
int led_engine_brt_1 = 20;

Adafruit_NeoPixel led_engine = Adafruit_NeoPixel(pixels_engine, pin_engine, NEO_GRB + NEO_KHZ800);

//color
int rgb_red[] = { 255, 0, 0 };
int rgb_green[] = { 0, 255, 0 };
int rgb_blue[] = { 0, 0, 255 };
int rgb_yel[] = { 255, 255, 0 };

//main program-------------------------------------------
void loop() {
  if (!digitalRead(sw) && deform_status == 0) deform_foward();
  if (!digitalRead(sw) && deform_status == 1) deform_reverse();
}

void setup() {
  //def sw
  pinMode(sw, INPUT_PULLUP);

  //multi task
  TaskHandle_t th;
  xTaskCreatePinnedToCore(engine, "engine", 4096, NULL, 5, &th, 0);

  //servo setup
  pwm.begin();
  pwm.setPWMFreq(50);
  delay(1000);

  //initialize servos
  deform_reverse();
}



void engine_rotate(int color[], int hz, int pixles, int led_brt) {
  for (int i = 0; i < led_engine.numPixels(); i++) {
    led_engine.clear();
    led_engine.show();

    //led group 1
    int center = i;
    int side_plus_0 = i + 1;
    int side_minus_0 = i - 1;
    if (side_plus_0 > led_engine.numPixels() - 1) side_plus_0 -= led_engine.numPixels();  //center=11の時とか
    if (side_minus_0 < 0) side_minus_0 = led_engine.numPixels() - 1;                      //center=0の時とか

    //center led
    led_engine.setPixelColor(center, led_engine.Color(color[0], color[1], color[2]));
    led_engine.setBrightness(led_brt);
    led_engine.show();

    //side led
    if (pixles == 3) {
      led_engine.setPixelColor(side_plus_0, led_engine.Color(color[0], color[1], color[2]));
      led_engine.setPixelColor(side_minus_0, led_engine.Color(color[0], color[1], color[2]));
      led_engine.setBrightness(led_brt);
      led_engine.show();
    }

    //led group 2
    int con = led_engine.numPixels() / 2;
    int con_center = i + con;
    int con_side_plus_0 = side_plus_0 + con;
    int con_side_minus_0 = side_minus_0 + con;
    if (con_center > led_engine.numPixels() - 1) con_center -= led_engine.numPixels();              //center=8の時とか
    if (con_side_plus_0 > led_engine.numPixels() - 1) con_side_plus_0 -= led_engine.numPixels();    //center=11の時とか
    if (con_side_minus_0 < 0) con_side_minus_0 = led_engine.numPixels() - 1;                        //center=0の時とか
    if (con_side_minus_0 > led_engine.numPixels() - 1) con_side_minus_0 -= led_engine.numPixels();  //center=0の時とか

    //center led
    led_engine.setPixelColor(con_center, led_engine.Color(color[0], color[1], color[2]));
    led_engine.setBrightness(led_brt);
    led_engine.show();

    //side led
    if (pixles == 3) {
      led_engine.setPixelColor(con_side_plus_0, led_engine.Color(color[0], color[1], color[2]));
      led_engine.setPixelColor(con_side_minus_0, led_engine.Color(color[0], color[1], color[2]));
      led_engine.setBrightness(led_brt);
      led_engine.show();
    }

    delay(hz);
  }
}

void engine_low(int color[]) {
  engine_rotate(color, 70, 3, 10);
}

void engine_high(int color[]) {
  engine_rotate(color, 20, 1, 255);
}

//loop-------------------------------------------------------------------------------------

void engine(void* pvParameters) {
  while (1) {
    if (deform_status == 0) engine_low(rgb_green);
    if (deform_status == 1) engine_high(rgb_red);
  }
}

//void deforms-------------------------------------
void deform_foward() {
  servo_write(servo_antenna_short, servo_angle_short);
  delay(300);
  deform_status = 1;
  servo_write(servo_antenna_long, servo_angle_long);
}

void deform_reverse() {
  servo_write(servo_antenna_long, servo_angle_start);
  delay(500);
  servo_write(servo_antenna_short, servo_angle_start);
  delay(500);

  deform_status = 0;
}

//void moves-------------------------------------------------------
//servo----------------------------------------------
void servo_write(int servo_pin, int ang) {
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX);
  pwm.writeMicroseconds(servo_pin, ang);
}