int G[5] = { 33, 31, 29, 27, 25};
boolean fc[40] = { 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
unsigned long time_costilla[10] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
unsigned long time_final_costilla[10] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
unsigned long time_aleta[8] = { 0, 0, 0, 0, 0, 0 };
unsigned long time_final_aleta[8] = { 0, 0, 0, 0, 0, 0 };
unsigned long time_giro = 0;
unsigned long time_final_giro = 0;
unsigned long time_sensor = 0;


#define A 39
#define B 37
#define C 35

#define PWM0 2
#define PWM1 6
#define PWM2 5
#define PWM3 4
#define PWM4 3
#define TIME_STOP 1200000

#define UP 15
#define DOWN 14
#define STOP 0

int latchPin0 = 7;
int clockPin0 = 8;
int dataPin0 = 9;

int latchPin1 = 12;
int clockPin1 = 11;
int dataPin1 = 10;

byte val_shift0[3] = { B10101010, B01010100, B00101010};

void shift0Write(int pin, boolean state)
{
  if (pin < 24)
  {
    int var = pin / 8;
    pin = pin - 8 * var;
    bitWrite(val_shift0[var], pin, state);
    digitalWrite(latchPin0, LOW);
    for (int i = 3; i >= 0; i--) shiftOut(dataPin0, clockPin0, MSBFIRST, val_shift0[i]);
    digitalWrite(latchPin0, HIGH);
  }
}

void resetshift0()
{
  digitalWrite(latchPin0, LOW);
  for (int i = 3; i >= 0; i--) shiftOut(dataPin0, clockPin0, MSBFIRST, val_shift0[i]);
  digitalWrite(latchPin0, HIGH);
  analogWrite(PWM0, 0);
}

byte val_shift1[5] = { 0x00, 0x00, 0x00};
void shift1Write(int pin, boolean state)
{
  if (pin < 24)
  {
    int var = pin / 8;
    pin = pin - 8 * var;
    bitWrite(val_shift1[var], pin, state);
    digitalWrite(latchPin1, LOW);
    for (int i = 3; i >= 0; i--) shiftOut(dataPin1, clockPin1, MSBFIRST, val_shift1[i]);
    digitalWrite(latchPin1, HIGH);
  }
}

void resetshift1()
{
  digitalWrite(latchPin1, LOW);
  for (int i = 3; i >= 0; i--) shiftOut(dataPin1, clockPin1, MSBFIRST, 0x00);
  digitalWrite(latchPin1, HIGH);
}

void selgrup(int var)
{
  switch (var)
  {
    case 3:
      digitalWrite(A, HIGH);
      digitalWrite(B, HIGH);
      digitalWrite(C, LOW);
      break;
    case 0:
      digitalWrite(A, LOW);
      digitalWrite(B, LOW);
      digitalWrite(C, LOW);
      break;
    case 1:
      digitalWrite(A, HIGH);
      digitalWrite(B, LOW);
      digitalWrite(C, LOW);
      break;
    case 2:
      digitalWrite(A, LOW);
      digitalWrite(B, HIGH);
      digitalWrite(C, LOW);
      break;
    case 4:
      digitalWrite(A, LOW);
      digitalWrite(B, LOW);
      digitalWrite(C, HIGH);
      break;
    case 6:
      digitalWrite(A, LOW);
      digitalWrite(B, HIGH);
      digitalWrite(C, HIGH);
      break;
    case 7:
      digitalWrite(A, HIGH);
      digitalWrite(B, HIGH);
      digitalWrite(C, HIGH);
      break;
    case 5:
      digitalWrite(A, HIGH);
      digitalWrite(B, LOW);
      digitalWrite(C, HIGH);
      break;
  }
}

boolean fcread(int pin)
{
  switch (pin)
  {
    case 0:
      selgrup(3);
      delay(1);
      
      if (!digitalRead(33))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(33))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 1:
      selgrup(0);
      delay(1);
      
      if (!digitalRead(33))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(33))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 2:
      selgrup(1);
      delay(1);
      
      if (!digitalRead(33))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(33))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 3:
      selgrup(2);
      delay(1);
      
      if (!digitalRead(33))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(33))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 4:
      selgrup(4);
      delay(1);
      
      if (!digitalRead(33))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(33))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 5:
      selgrup(6);
      delay(1);
      
      if (!digitalRead(33))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(33))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 6:
      selgrup(7);
      delay(1);
      
      if (!digitalRead(33))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(33))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 7:
      selgrup(5);
      delay(1);
      
      if (!digitalRead(33))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(33))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 8:
      selgrup(3);
      delay(1);
      
      if (!digitalRead(31))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(31))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 9:
      selgrup(0);
      delay(1);
      
      if (!digitalRead(31))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(31))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 10:
      selgrup(1);
      delay(1);
      
      if (!digitalRead(31))
        {
          unsigned long time_count=millis();
          while ((!digitalRead(31))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 11:
      selgrup(2);
      delay(1);
       if (!digitalRead(31))
        {
          unsigned long time_count = millis();
          while ((!digitalRead(31))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
    case 12:
      selgrup(4);
      delay(1);
       if (!digitalRead(31))
        {
          unsigned long time_count = millis();
          while ((!digitalRead(31))&&((millis()-time_count)<100));
          if ((millis()-time_count)>=100) return false;
          return true;
        }
      break;
  }
  return true;
}

int flag_dir_c[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
int dir[10] = { 2, 4, 6, 9, 11, 13, 15, 18, 20, 22};
int en_c[10] = { 1, 3, 5, 7, 10, 12, 14, 17, 19, 21};

void costilla(int num, int value, unsigned long timemax)
{
  time_final_costilla[num] = timemax;
  time_costilla[num] = millis();
  if (value > 0) flag_dir_c[num] = 1;
  else if (value < 0) flag_dir_c[num] = -1;
  else flag_dir_c[num] = 0;
  
  if (num < 10)
  {
    if (value != 0) 
      {
        shift0Write(en_c[num], false);
        analogWrite(PWM0, abs(value));
      }
    else shift0Write(en_c[num], true);
    if (value > 0) shift0Write(dir[num], true);
    else shift0Write(dir[num], false);
  }
}

int flag_dir[6] = { 0, 0, 0, 0, 0, 0 };
int dirA[6]     = { 21, 18, 10, 13, 2, 5 };
int dirB[6]     = { 20, 19, 11, 12, 3, 4 };
int en_a[6]     = { 22, 17, 9, 14, 1, 6 };
int PWM[6]     = { PWM3, PWM4, PWM2, PWM1, PWM2, PWM1 };


void motor(int num, int value, unsigned long timemax)
{
  time_final_aleta[num] = timemax;
  time_aleta[num] = millis();
  if (value > 0) flag_dir[num] = 1;
  else if (value < 0) flag_dir[num] = -1;
  else flag_dir[num] = 0;

  if (num < 6)
  {
    if (value != 0) 
      {
        shift1Write(en_a[num], true);
        analogWrite(PWM[num], abs(value));
      }
    else shift1Write(en_a[num], false);
    if (value > 0) {
      shift1Write(dirA[num], true);
      shift1Write(dirB[num], false);
    }
    else {
      shift1Write(dirA[num], false);
      shift1Write(dirB[num], true);
    }
  }
}

void aleta_izquierda(int Speed, unsigned long timemax)
  {
    motor(0, Speed, timemax);
  }

void aleta_derecha(int Speed, unsigned long timemax)
  {
    motor(1, Speed, timemax);
  }

void aleta_izquierda_giro(int Speed, unsigned long timemax)
  {
    motor(2, Speed, timemax);
  }

void aleta_derecha_giro(int Speed, unsigned long timemax)
  {
    motor(3, Speed, timemax);
  }
  
void cola(int Speed, unsigned long timemax)
  {
    motor(4, Speed, timemax);
  }
    
int flag_dir_g = 0;

void giro_cuerpo(int mode, unsigned long timemax)
  {
    time_final_giro = timemax;
    time_giro = millis();
    if (mode == UP) 
      {
        flag_dir_g = 1;
        digitalWrite(UP, HIGH);
        digitalWrite(DOWN, LOW);
      }
    else if (mode == DOWN) 
      {
        flag_dir_g = -1;
        digitalWrite(UP, LOW);
        digitalWrite(DOWN, HIGH);
      }
    else
      {
        flag_dir_g = 0;
        digitalWrite(UP, LOW);
        digitalWrite(DOWN, LOW);
      }
    
  }
int flag_costilla[10];
int flag_aleta[6];
int flag_giro;
int fc_aleta_up[6]   = {2, 0, 4, 7, 8, 10};
int fc_aleta_down[6] = {3, 1, 5, 6, 9, 11};

void control()
{
  for (int i = 0; i < 6; i++) flag_aleta[i] = 0;
  for (int i = 0; i < 10; i++) flag_costilla[i] = 0;
  flag_giro = 0;
  while (((!flag_aleta[0]) || (!flag_aleta[1]) || (!flag_aleta[2]) || (!flag_aleta[3]) || (!flag_aleta[4]) || (!flag_aleta[5]))
          ||((!flag_costilla[0]) || (!flag_costilla[1]) || (!flag_costilla[2]) || (!flag_costilla[3]) || (!flag_costilla[4]) || (!flag_costilla[5]) || (!flag_costilla[6]) || (!flag_costilla[7]) || (!flag_costilla[8]) || (!flag_costilla[9]))||(!flag_giro))
  {
    for (int num = 0; num < 6; num++)
    {
      if (((millis() - time_aleta[num]) >= time_final_aleta[num]) || (!fcread(fc_aleta_up[num]) && (flag_dir[num] == 1)) || (!fcread(fc_aleta_down[num]) && (flag_dir[num] == -1)))
      {
        flag_aleta[num] = 1;
        if (flag_dir[num] != 0)//&&((flag_dir[2] == 0)))
          motor(num, 0, 0);
      }
    }
    for (int num = 0; num < 10; num++)
    {
      if ((millis() - time_costilla[num]) >= time_final_costilla[num])
      {
        flag_costilla[num] = 1;
        if (flag_dir_c[num] != 0)
         costilla(num, 0, 0);
      }
    }
    if (((millis() - time_giro) >= time_final_giro)||((flag_dir_g==1)&&(!fcread(11)))||((flag_dir_g==-1)&&(!fcread(10))))
      {
        flag_giro = 1;
        if (flag_dir_g != 0)
          {
           giro_cuerpo(STOP,0);
          }
      }
 }


}
void reset_costillas()
{
  for (int i = 0; i < 10; i++) costilla(i, -255, 15000);
  control();
}

void setup() {
  Serial.begin(57600);
  for (int i = 0; i < 5; i++)
  {
    pinMode(G[i], INPUT);
    digitalWrite(G[i], HIGH);
  }
  pinMode(PWM0, OUTPUT);
  pinMode(PWM1, OUTPUT);
  pinMode(PWM2, OUTPUT);
  pinMode(PWM3, OUTPUT);
  pinMode(PWM4, OUTPUT);
  pinMode(UP, OUTPUT);
  pinMode(DOWN, OUTPUT);
  pinMode(A, OUTPUT);
  pinMode(B, OUTPUT);
  pinMode(C, OUTPUT);
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(UP, LOW);
  digitalWrite(DOWN, LOW);
  analogWrite(PWM0, 0);
  analogWrite(PWM1, 0);
  analogWrite(PWM2, 0);
  analogWrite(PWM3, 0);
  analogWrite(PWM4, 0);
  pinMode(latchPin0, OUTPUT);
  pinMode(clockPin0, OUTPUT);
  pinMode(dataPin0, OUTPUT);
  resetshift0();
  pinMode(latchPin1, OUTPUT);
  pinMode(clockPin1, OUTPUT);
  pinMode(dataPin1, OUTPUT);
  resetshift1();
  ini();

}

//0, 1 aletas delanteras arriba y abajo
//2, 3 aletas delanteras GIRO
boolean reposo = true;

void reset_aletas()
  {
    aleta_izquierda(-255, 35000);
    aleta_derecha(-255, 35000);
    cola(-255, 35000);
    control();
    //aleta_derecha_giro(-255, 35000);
    //aleta_izquierda_giro(-255, 35000);
    control();
    //aleta_derecha_giro(255, 4500);
    //aleta_izquierda_giro(255, 4200);
    control();
  }
  
void ini()
{
      reset_costillas();
      //giro_cuerpo(UP, 120000);
      control();
      reset_aletas();
}

int SPEED = 255;
int RAMP = 25;

void ballena()
  {
   for (int i=0; i<1; i++)
    {
      for (int j=0; j<RAMP; j++)
        {
          aleta_izquierda(j*SPEED/RAMP, 100);
          aleta_derecha(j*SPEED/RAMP, 100);
          //aleta_derecha_giro(j*SPEED/RAMP, 22);
          //aleta_izquierda_giro(j*SPEED/RAMP, 22);
          cola(-j*SPEED/RAMP, 100);
          for (int h = 0; h < 10; h=h+2) 
          {
            costilla(h, SPEED/3, 10);
            costilla(h+1, SPEED/3, 10);
          }
          control();
        }
      aleta_izquierda(255, 4000);
      aleta_derecha(255, 4000);
      cola(-255, 6000);
      control();
      for (int j=0; j<RAMP; j++)
        {
          aleta_izquierda(-j*SPEED/RAMP, 100);
          aleta_derecha(-j*SPEED/RAMP, 100);
          //aleta_derecha_giro(-j*SPEED/RAMP, 20);
          //aleta_izquierda_giro(-j*SPEED/RAMP, 20);
          cola(j*SPEED/RAMP, 100);
          for (int h = 0; h < 10; h=h+2) 
          {
            costilla(h, -SPEED/3, 20);
            costilla(h+1, -SPEED/3, 20);
          }
          control();
        }
      aleta_izquierda(-255, 4000);
      aleta_derecha(-255, 4000);
      cola(255, 6000);
      control();
    } 
//    for (int i=0; i<2; i++)
//    {
//      for (int j=0; j<RAMP; j++)
//        {
//          aleta_izquierda(j*SPEED/RAMP, 100);
//          aleta_derecha(j*SPEED/RAMP, 100);
//          aleta_derecha_giro(j*SPEED/RAMP, 60);
//          aleta_izquierda_giro(j*SPEED/RAMP, 60);
//          cola(-j*SPEED/RAMP, 100);
//          for (int h = 0; h < 10; h=h+2) 
//          {
//            costilla(h, SPEED/3, 10);
//            costilla(h+1, SPEED/3, 10);
//          }
//          control();
//        }
//      aleta_izquierda(255, 8000);
//      aleta_derecha(255, 8000);
//      cola(-255, 6000);
//      control();
//      for (int j=0; j<RAMP; j++)
//        {
//          aleta_izquierda(-j*SPEED/RAMP, 100);
//          aleta_derecha(-j*SPEED/RAMP, 100);
//          aleta_derecha_giro(-j*SPEED/RAMP, 20);
//          aleta_izquierda_giro(-j*SPEED/RAMP, 20);
//          cola(j*SPEED/RAMP, 100);
//          for (int h = 0; h < 10; h=h+2) 
//          {
//            costilla(h, -SPEED/3, 30);
//            costilla(h+1, -SPEED/3, 30);
//          }
//          control();
//        }
//      aleta_izquierda(-255, 8000);
//      aleta_derecha(-255, 8000);
//      cola(255, 6000);
//      control();
//    } 
//   for (int i=0; i<4; i++)
//    {
//      for (int j=0; j<RAMP; j++)
//        {
//          aleta_izquierda(j*SPEED/RAMP, 100);
//          aleta_derecha(j*SPEED/RAMP, 100);
//          aleta_derecha_giro(j*SPEED/RAMP, 80);
//          aleta_izquierda_giro(j*SPEED/RAMP, 80);
//          cola(-j*SPEED/RAMP, 100);
//          for (int h = 0; h < 10; h=h+2) 
//          {
//            costilla(h, SPEED/3, 10);
//            costilla(h+1, SPEED/3, 10);
//          }
//          control();
//        }
//      aleta_izquierda(255, 4000);
//      aleta_derecha(255, 4000);
//      cola(-255, 6000);
//      control();
//      aleta_izquierda(0, 1000);
//      aleta_derecha(0, 1000);
//      control();
//      for (int j=0; j<RAMP; j++)
//        {
//          aleta_izquierda(-j*SPEED/RAMP, 100);
//          aleta_derecha(-j*SPEED/RAMP, 100);
//          aleta_derecha_giro(-j*SPEED/RAMP, 20);
//          aleta_izquierda_giro(-j*SPEED/RAMP, 20);
//          cola(j*SPEED/RAMP, 100);
//          for (int h = 0; h < 10; h=h+2) 
//          {
//            costilla(h, -SPEED/3, 40);
//            costilla(h+1, -SPEED/3, 40);
//          }
//          control();
//        }
//      aleta_izquierda(-255, 4000);
//      aleta_derecha(-255, 4000);
//      cola(255, 6000);
//      control();
//    } 
//    for (int i=0; i<1; i++)
//    {
//      for (int j=0; j<RAMP; j++)
//        {
//          aleta_izquierda(j*SPEED/RAMP, 100);
//          aleta_derecha(j*SPEED/RAMP, 100);
//          aleta_derecha_giro(j*SPEED/RAMP, 100);
//          aleta_izquierda_giro(j*SPEED/RAMP, 100);
//          cola(-j*SPEED/RAMP, 100);
//          for (int h = 0; h < 10; h=h+2) 
//          {
//            costilla(h, SPEED/3, 10);
//            costilla(h+1, SPEED/3, 10);
//          }
//          control();
//        }
//      aleta_izquierda(255, 8000);
//      aleta_derecha(255, 8000);
//      cola(-255, 6000);
//      control();
//      for (int j=0; j<RAMP; j++)
//        {
//          aleta_izquierda(-j*SPEED/RAMP, 100);
//          aleta_derecha(-j*SPEED/RAMP, 100);
//          aleta_derecha_giro(-j*SPEED/RAMP, 20);
//          aleta_izquierda_giro(-j*SPEED/RAMP, 20);
//          cola(j*SPEED/RAMP, 100);
//          for (int h = 0; h < 10; h=h+2) 
//          {
//            costilla(h, -SPEED/3, 50);
//            costilla(h+1, -SPEED/3, 50);
//          }
//          control();
//        }
//      aleta_izquierda(-255, 8000);
//      aleta_derecha(-255, 8000);
//      cola(255, 6000);
//      control();
//    } 
// 
  }

  void barco()
  {
      aleta_izquierda(255, 35000);
      aleta_derecha(255, 35000);
      cola(255, 35000);
      control();
      aleta_derecha_giro(255, 15000);
      aleta_izquierda_giro(255, 15000);
      control();
      giro_cuerpo(DOWN, 120000);
      control();
      for (int i = 0; i < 10; i++) costilla(i, 255, 10000);
      control();
      if (reposo)
        {
          time_sensor=millis();
          while((millis()-time_sensor)<=TIME_STOP)
            {
              if (!fcread(12))
                {
                   for (int i = 0; i < 10; i=i+2) 
                    {
                      costilla(i, -SPEED, 2000);
                      costilla(i+1, -SPEED, 2000);
                      if (i==0) cola(-255,500);
                      if ((i==0)||(i==4)||(i==8))
                      {
                        aleta_derecha_giro(-255, 500);
                        aleta_izquierda_giro(-255, 500);
                      }
                      else
                      {
                        aleta_derecha_giro(255, 500);
                        aleta_izquierda_giro(255, 500);
                      }
                      control();
                    }
                  for (int i = 0; i < 10; i=i+2) 
                    {
                      costilla(i, SPEED, 2000);
                      costilla(i+1, SPEED, 2000);
                      if (i==0) cola(255,500);
                      if ((i==2)||(i==6)||(i==10))
                      {
                        aleta_derecha_giro(-255, 500);
                        aleta_izquierda_giro(-255, 500);
                      }
                      else
                      {
                        aleta_derecha_giro(255, 500);
                        aleta_izquierda_giro(255, 500);
                      }
                      control();
                    }
                }
            }
        }
      for (int i = 0; i < 4; i++) 
            {
              aleta_derecha_giro(255, 500);
              aleta_izquierda_giro(255, 500);
              control();
              aleta_derecha_giro(0, 500);
              aleta_izquierda_giro(0, 500);
              control();
              aleta_derecha_giro(-255, 500);
              aleta_izquierda_giro(-255, 500);
              control();
              aleta_derecha_giro(0, 500);
              aleta_izquierda_giro(0, 500);
              control();
            }
      aleta_derecha_giro(255, 500);
      aleta_izquierda_giro(255, 500);
      control();
      
//      for (int j=0; j<10; j++)
//        {
//          for (int i = 0; i < 10; i=i+2) 
//            {
//              costilla(i, -SPEED, 2000);
//              costilla(i+1, -SPEED, 2000);
//              if (i==0) cola(-255,500);
//              if ((i==0)||(i==4)||(i==8))
//              {
//                aleta_derecha_giro(-255, 500);
//                aleta_izquierda_giro(-255, 500);
//              }
//              else
//              {
//                aleta_derecha_giro(255, 500);
//                aleta_izquierda_giro(255, 500);
//              }
//              control();
//            }
//          for (int i = 0; i < 10; i=i+2) 
//            {
//              costilla(i, SPEED, 2000);
//              costilla(i+1, SPEED, 2000);
//              if (i==0) cola(255,500);
//              if ((i==2)||(i==6)||(i==10))
//              {
//                aleta_derecha_giro(-255, 500);
//                aleta_izquierda_giro(-255, 500);
//              }
//              else
//              {
//                aleta_derecha_giro(255, 500);
//                aleta_izquierda_giro(255, 500);
//              }
//              control();
//            }
//        }
      ini();
  }


void loop() { 
  //for(int i=0; i<40; i++) Serial.print(fcread(i));
 // Serial.println(fcread(12));
  //ballena();
  if (reposo)
        {
          time_sensor=millis();
          while((millis()-time_sensor)<=TIME_STOP)
            {
              if (!fcread(12))
                {
                  ballena();
                }
            }
        }
 // barco(); 
  ini();
}