#include <stdio.h>
#include "pico/stdlib.h"

#define LIGHTS_PIN 5
#define ARRAY_LENGTH(arr) (sizeof(arr) / sizeof((arr)[0]))
#define LOOP_CYCLES 5

typedef enum {
    MOTOR_STOP,
    MOTOR_UP,
    MOTOR_DOWN
} MotorState;

typedef struct {
  int pin_a;
  int pin_b;
} Motor;

typedef enum {
    POSITION_TOP,
    POSITION_MIDDLE,
    POSITION_BOTTOM
} GhostPosition;

typedef struct {
    MotorState head_motion;
    MotorState hand_motion;
    int duration;
} AnimationFrame;

typedef struct {
    AnimationFrame* frames;
    int length;
} Animation;

typedef enum {
    CYCLE_INIT,
    CYCLE_LOOPED,
    CYCLE_END
} CycleState;

Motor head_motor = {
  .pin_a = 2,
  .pin_b = 3
};
Motor hand_l_motor = {
  .pin_a = 4,
  .pin_b = 5
};
Motor hand_r_motor = {
  .pin_a = 6,
  .pin_b = 7
};

AnimationFrame init_ghost_cycle[] = {
    {
        MOTOR_DOWN,
        MOTOR_STOP,
        800
    },

    {
        MOTOR_STOP,
        MOTOR_DOWN,
        1200
    },
};

AnimationFrame looped_ghost_cycle[] = {
    {
        MOTOR_DOWN,
        MOTOR_UP,
        1500
    },

    {
        MOTOR_UP,
        MOTOR_DOWN,
        1500
    }
};

AnimationFrame end_ghost_cycle[] = {
    {
        MOTOR_DOWN,
        MOTOR_UP,
        1500
    },
    {
        MOTOR_UP,
        MOTOR_STOP,
        1500
    },
    {
        MOTOR_STOP,
        MOTOR_UP,
        1500
    }, 
    {
        MOTOR_UP,
        MOTOR_UP,
        1500
    }
};

Animation init_animation = {
    init_ghost_cycle,
    ARRAY_LENGTH(init_ghost_cycle)
};
Animation loop_animation = {
    looped_ghost_cycle,
    ARRAY_LENGTH(looped_ghost_cycle)
};
Animation end_animation = {
    end_ghost_cycle,
    ARRAY_LENGTH(end_ghost_cycle)
};

void initPins(Motor* motor);
void init();
void setMotor(Motor* motor, MotorState state);

int updateFrame(Animation* cycle, int* frame);
void setFrame(AnimationFrame frame);

int defaultCycle(CycleState* state, int* frame, int* loops);

int main() {
  init();

  CycleState currentState = CYCLE_INIT;
  int frame = 0;
  int loops = 0;

  while(defaultCycle(&currentState, &frame, &loops)){

  }
  
}

void initPins(Motor* motor){
  gpio_init(motor->pin_a);
  gpio_set_dir(motor->pin_a, GPIO_OUT);
  
  gpio_init(motor->pin_b);
  gpio_set_dir(motor->pin_b, GPIO_OUT);
}
void init(){
  stdio_init_all();
  initPins(&head_motor);
  initPins(&hand_l_motor);
  initPins(&hand_r_motor);
}

void setMotor(Motor* motor, MotorState state)
{
    switch(state)
    {
        case MOTOR_STOP:
            gpio_put(motor->pin_a, 0);
            gpio_put(motor->pin_b, 0);
            break;

        case MOTOR_UP:
            gpio_put(motor->pin_a, 1);
            gpio_put(motor->pin_b, 0);
            break;

        case MOTOR_DOWN:
            gpio_put(motor->pin_a, 0);
            gpio_put(motor->pin_b, 1);
            break;
    }
}

int updateFrame(Animation* cycle, int* frame){
    if(*frame < cycle->length)
    {
        setFrame(cycle->frames[*frame]);
        sleep_ms(cycle->frames[*frame].duration);
        (*frame)++;

        return 1;
    }

    return 0;
}
void setFrame(AnimationFrame frame){
    setMotor(&head_motor, frame.head_motion);
    setMotor(&hand_l_motor, frame.hand_motion);
    setMotor(&hand_r_motor, frame.hand_motion);
}

int defaultCycle(CycleState* state, int* frame, int* loops){
  switch(*state){
    case CYCLE_INIT:
      if (updateFrame(&init_animation,
                          frame)) {
        
        return 1;
      }

      (*frame) = 0;
      (*loops) = 0;
      (*state) = CYCLE_LOOPED;
    return 1;

    case CYCLE_LOOPED:
      if((*loops) < LOOP_CYCLES){
        if (updateFrame(&loop_animation,
                          frame)) {
          
          return 1;
        }

        (*loops)++;
        (*frame) = 0;
        return 1;
      }
      
      (*frame) = 0;
      (*state) = CYCLE_END;
    return 1;

    case CYCLE_END:
      if (updateFrame(&end_animation,
                          frame)) {
        
        return 1;
      }
    
    (*frame) = 0;
    break;

  }
  
  return 0;
}