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[6] = { 0, 0, 0, 0, 0, 0 };
unsigned long time_final_aleta[6] = { 0, 0, 0, 0, 0, 0 };
unsigned long time_giro = 0;
unsigned long time_final_giro = 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 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 0:
      digitalWrite(A, HIGH);
      digitalWrite(B, HIGH);
      digitalWrite(C, LOW);
      break;
    case 1:
      digitalWrite(A, LOW);
      digitalWrite(B, LOW);
      digitalWrite(C, LOW);
      break;
    case 2:
      digitalWrite(A, HIGH);
      digitalWrite(B, LOW);
      digitalWrite(C, LOW);
      break;
    case 3:
      digitalWrite(A, LOW);
      digitalWrite(B, HIGH);
      digitalWrite(C, LOW);
      break;
    case 4:
      digitalWrite(A, LOW);
      digitalWrite(B, LOW);
      digitalWrite(C, HIGH);
      break;
    case 5:
      digitalWrite(A, LOW);
      digitalWrite(B, HIGH);
      digitalWrite(C, HIGH);
      break;
    case 6:
      digitalWrite(A, HIGH);
      digitalWrite(B, HIGH);
      digitalWrite(C, HIGH);
      break;
    case 7:
      digitalWrite(A, HIGH);
      digitalWrite(B, LOW);
      digitalWrite(C, HIGH);
      break;
  }
}

boolean fcread(int pin)
{
  int count = 0;
  for (int j = 0; j < 5; j++)
  {
    for (int i = 0; i < 8; i++)
    {
      selgrup(i);
      delay(2);
      fc[count] = digitalRead(G[j]);
      count++;
    }
  }
  return fc[pin];
}

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)
{ 
  if (num < 10)
  {
    if (value > 0) flag_dir_c[num] = 1;
    else if (value < 0) flag_dir_c[num] = -1;
    else flag_dir_c[num] = 0;
    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_a[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 };
int fc_aleta_up[6]  = {2,    0,    4,    7,    8,   10};
int fc_aleta_down[6]= {3,    1,    5,    6,    9,   11};

void aleta(int num, int value)
{
  if (num < 6)
  {
    if (value > 0) flag_dir_a[num] = 1;
    else if (value < 0) flag_dir_a[num] = -1;
    else flag_dir_a[num] = 0;
    
    if (value != 0) 
      {
        shift1Write(en_a[num], true);
        analogWrite(PWM[num], abs(value));
      }
    else shift1Write(en_a[num], false);
    
    if ((value > 0)&&(digitalRead(fc_aleta_up[num]))) {
      shift1Write(dirA[num], true);
      shift1Write(dirB[num], false);
    }
    else if ((value < 0)&&(digitalRead(fc_aleta_down[num])))  {
      shift1Write(dirA[num], false);
      shift1Write(dirB[num], true);
    }
  }
}

int flag_dir_g = 0;

void giro(int mode)
  {
    if (mode > 0) 
      {
        flag_dir_g = 1;
        digitalWrite(UP, HIGH);
        digitalWrite(DOWN, LOW);
      }
    else if (mode < 0) 
      {
        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;


void control()
{
  for (int num = 0; num < 5; num++)
    {
      if ((flag_dir_a[num]==-1)&&(!digitalRead(fc_aleta_down[num]))) aleta(num,0);
      else if ((flag_dir_a[num]==1)&&(!digitalRead(fc_aleta_up[num]))) aleta(num,0);
    }
//  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)))
//          aleta(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_giro = 1;
//        if (flag_dir_g != 0)
//          {
//           giro(STOP,0);
//          }
//      }
// }
}
void reset_costillas()
{
  for (int i = 0; i < 10; i++) costilla(i, -255);
  delay(10000);
  for (int i = 0; i < 10; i++) costilla(i, 0);
}

void setup() {
  Serial.begin(9600);
  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();
  reset_costillas();
  reset_aletas();
}

void reset_aletas()
  {
    aleta(0, -255);
    aleta(1, -255);
    aleta(4, -255);
    control();
    aleta(3, -255);
    control();
    aleta(3, 255);
    delay(4000);
    aleta(3, 0);
    giro(255);
    delay(120000);
  }

byte command = 0x00;
int speed_motor = 0x00;
int address = 0x00;
void serialEvent() {
  while (Serial.available()) {
    command = Serial.read();
    address = command>>4;
    speed_motor = command&0x0F;
    if (speed_motor>=8) speed_motor = map(speed_motor, 8, 15, 0, 255);//speed_motor - 8;
    else speed_motor = map(speed_motor, 0, 7, 0, -255); //speed_motor * (-1);
    if (address == 0) giro(speed_motor);
    else  if (address >= 5) aleta(address - 1, speed_motor);
    else  costilla(address - 6, speed_motor);
  }
}

  
int M = 3;
int SPEED = 255;

void loop() {
  control();
}