#include <AccelStepper.h>

#define red 3
#define green 6
#define blue 4

AccelStepper stepper(1,2,5); // Defaults to AccelStepper::FULL4WIRE (4 pins) on 2, 3, 4, 5

const uint8_t PROGMEM gamma8[] = {
    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,
    1,  1,  1,  1,  1,  1,  1,  1,  1,  2,  2,  2,  2,  2,  2,  2,
    2,  3,  3,  3,  3,  3,  3,  3,  4,  4,  4,  4,  4,  5,  5,  5,
    5,  6,  6,  6,  6,  7,  7,  7,  7,  8,  8,  8,  9,  9,  9, 10,
   10, 10, 11, 11, 11, 12, 12, 13, 13, 13, 14, 14, 15, 15, 16, 16,
   17, 17, 18, 18, 19, 19, 20, 20, 21, 21, 22, 22, 23, 24, 24, 25,
   25, 26, 27, 27, 28, 29, 29, 30, 31, 32, 32, 33, 34, 35, 35, 36,
   37, 38, 39, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 50,
   51, 52, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 66, 67, 68,
   69, 70, 72, 73, 74, 75, 77, 78, 79, 81, 82, 83, 85, 86, 87, 89,
   90, 92, 93, 95, 96, 98, 99,101,102,104,105,107,109,110,112,114,
  115,117,119,120,122,124,126,127,129,131,133,135,137,138,140,142,
  144,146,148,150,152,154,156,158,160,162,164,167,169,171,173,175,
  177,180,182,184,186,189,191,193,196,198,200,203,205,208,210,213,
  215,218,220,223,225,228,231,233,236,239,241,244,247,249,252,255 };
      
unsigned long time_motor = 0;
unsigned long time_filter = 0;
unsigned long time_open = 0;
unsigned long pas_count = 0;
unsigned long pas_count_ant = 0;

#define CONST_TIME 5000
#define CONST_TIME_PAS CONST_TIME/(2*255)
int time_pas = CONST_TIME_PAS;

uint8_t i=0;
bool flag_up=true;
bool flag_down=false;
bool flag_time_open=true;

void setup()
{
 Serial.begin(115200);  
 pinMode(53, INPUT);
 pinMode(8, OUTPUT);
 pinMode(red, OUTPUT);
 pinMode(green, OUTPUT);
 pinMode(blue, OUTPUT);
 digitalWrite(red, LOW); 
 digitalWrite(green, LOW); 
 digitalWrite(blue, LOW); 
 digitalWrite(53, HIGH); 
 digitalWrite(8, LOW); 
 stepper.setEnablePin(8);
 stepper.setPinsInverted(false, false, true); // invert direction & enable pin
 stepper.enableOutputs();
 
// stepper.setMaxSpeed(10000000);
// stepper.setSpeed(1050);

 stepper.setMaxSpeed(10000);
 stepper.setSpeed(4000);
 
// analogWrite(red,255);
// analogWrite(green,0);
// analogWrite(blue,0);
// delay(2000);
// analogWrite(red,0);
// analogWrite(green,255);
// analogWrite(blue,0);
// delay(2000);
// analogWrite(red,0);
// analogWrite(green,0);
// analogWrite(blue,255);
// delay(2000);
}



void loop()
{  
  stepper.runSpeed();
  
  if((digitalRead(53))||flag_down==true)
    {
      if(digitalRead(53))
        {
          flag_time_open=true;
          time_filter = millis();
        }
      if (((millis()-time_motor)>=time_pas)&&(flag_up))
        {
          i++;
          analogWrite(blue,0);
          analogWrite(green,0);
          //analogWrite(red,i);
          analogWrite(red, pgm_read_byte(&gamma8[i]));
          time_motor = millis();
          if (i==255) 
            {
              flag_up=false;
              flag_down=true;
            }
//          Serial.print("Subo: ");
//          Serial.println(i);
        }
      else if(((millis()-time_motor)>=(2*time_pas))&&(flag_down))
        {
          i=i-1;
          analogWrite(blue,0);
          analogWrite(green,0);
          //analogWrite(red,i);
          analogWrite(red, pgm_read_byte(&gamma8[i]));
          time_motor = millis();
          if (i==0) 
            {
              flag_down=false;
            }
//           Serial.print("Bajo: ");
//           Serial.println(i);
        }
      
    }
    
  if(!digitalRead(53))
    {
      if(((millis()-time_filter)>1000))
        {
          if (flag_time_open)
            {
//                analogWrite(blue,0);
//                analogWrite(green,0);
//                analogWrite(red,0);
                time_filter = millis();
                pas_count = stepper.currentPosition();
                Serial.print("Tiempo transcurrido = ");
                Serial.print(millis()-time_open);
                time_pas = ((millis()-time_open)/2.)/255;
                Serial.print(", pasos = ");
                Serial.print(pas_count-pas_count_ant);
                Serial.print(", paso = ");
                Serial.print(time_pas);
                if (((time_pas)<(CONST_TIME_PAS-1))||((time_pas)>(CONST_TIME_PAS+4))) 
                  {
                    Serial.print(", ERROR DE FC pasos corregidos = ");
                    time_pas=CONST_TIME_PAS;
                    Serial.print(time_pas);
                  }
                Serial.println();

                pas_count_ant = pas_count;
                time_open = millis();
                flag_time_open=false;
            }
          time_open = millis();
          if ((flag_up==false)&&(flag_down==false))
                  {
                     flag_up=true;
                     flag_down=false;
                  }
//          i=0;
          //flag_up=true;
        }    
    }
   
}