#include "wokwi-api.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define Hmax 50000// altura máxima (mm/10)
#define Hmin 0// altura minima (mm)
#define velocidad 100 // velocidad (cm/s)
#define velinc 1 // velocidad incremental por periodo en (mm/10)
int posicion=0; //posicion en mm 
int P0,P1;
float w2 = 0, w1 = 0, w0 = 0;
float alpha = 0;
long int alphaA1 = 0, alphaA2 = 0;
long int alphaB1 = 0, alphaB2 = 0;
int ENC_A = 0, ENC_B = 0;

typedef struct {
  uint8_t r;
  uint8_t g;
  uint8_t b;
  uint8_t a;
} rgba_t;

typedef struct {
  pin_t pin_P0;
  pin_t pin_P1;
  pin_t pin_Ms;
  pin_t pin_Mb;
  buffer_t framebuffer;
  uint32_t width;
  uint32_t height;
  uint32_t row;
  } chip_state_t;

static void chip_timer_event(void *user_data);

void chip_init(void) {
  chip_state_t *chip = malloc(sizeof(chip_state_t));
  chip->pin_Ms = pin_init("Ms", INPUT);
  chip->pin_Mb = pin_init("Mb", INPUT);
  chip->pin_P0 = pin_init("P0",OUTPUT_LOW);
  chip->pin_P1 = pin_init("P1",OUTPUT_LOW);
  chip->row = 50;
  chip->framebuffer = framebuffer_init(&chip->width, &chip->height);
  const timer_config_t timer_config = {
    .callback = chip_timer_event,
    .user_data = chip,
  };
  timer_t timer_id = timer_init(&timer_config);
  timer_start(timer_id, 100, true);
}
void draw_line(chip_state_t *chip, uint32_t row, rgba_t color) {
  uint32_t offset = chip->width * 4 * row;
  for (int x = 0; x < chip->width *4* 4; x += 4) {
    buffer_write(chip->framebuffer, offset + x, (uint8_t*)&color, sizeof(color));
  }
}
void draw_point(chip_state_t *chip, uint32_t row, rgba_t color) {
  uint32_t offset = chip->width * 4 * row;
  rgba_t black = { .g=0x00, .a = 0xff };
  for (int x = 0; x < chip->width *4* 4; x += 4) {
    if ((x/4 % chip->width)<8)
    buffer_write(chip->framebuffer, offset + x, (uint8_t*)&color, sizeof(color));
  }
}

void chip_timer_event(void *user_data) {
  chip_state_t *chip = (chip_state_t*)user_data;
  if ((pin_read(chip->pin_Ms)>0)&&(pin_read(chip->pin_Mb)==0))
   posicion=posicion+velinc;
  if ((pin_read(chip->pin_Ms)==0)&&(pin_read(chip->pin_Mb)>0))
   posicion=posicion-velinc;
  if ((posicion<Hmax)&&(posicion>Hmin)) 
  { P0=0;
  P1=0;
  }
  if (posicion>=Hmax)
  { posicion=Hmax;
  P1=1;
  P0=0;
  }
  if (posicion<=Hmin)
  { posicion=Hmin;
  P0=1;
  P1=0;
  }
  pin_write(chip->pin_P0,P0);
  pin_write(chip->pin_P1,P1);
  rgba_t black = { .g=0x00, .a = 0xff };
  draw_line(chip, chip->row, black);
  chip->row = (50-posicion/1000);
  rgba_t purple = { .r = 0xff, .g=0xff, .b=0xff, .a=0xff };
  rgba_t P1color = { .r = 0xff, .g=0x00, .b=0x00, .a=0xff };
  rgba_t P0color = { .r = 0x00, .g=0xff, .b=0x00, .a=0xff };
  draw_line(chip, chip->row, purple);
  if (posicion==Hmax) draw_point(chip, chip->row, P1color);
  if (posicion==Hmin) draw_point(chip, chip->row, P0color);
 }