#pragma once
#include "pico/stdlib.h"

#define SENSOR_STONE_PIN 6 
#define SENSOR_Z_AXIS_PIN 7

#define MOTOR_CHOPPER1_PIN 11
#define MOTOR_CHOPPER2_PIN 12
#define MOTOR_Z_AXIS_PIN 13

#define LED1_PIN 25
#define LED2_PIN 16
#define BUTTON_PIN 20

// GoRPC Config
#define GORPC_INT_PIN 2
#define GORPC_SDA_PIN 4
#define GORPC_SCL_PIN 5

#define I2C_ADDR 0x22

#define CORE_MASK_GOBOTRPC 0b01
#define CORE_MASK_HEAD     0b01

// Pwm Config

#define PWM_TARGET_FREQ_HZ 100000.f
#define PWM_CLK_DIV ((float)(SYS_CLK_HZ) / PWM_TARGET_FREQ_HZ)
// 10us per Cycle -> 20ms / 10us = 2000
#define PWM_TOTAL 11080

#define PWM_CALC_CYCLE(n) ((int)((PWM_TOTAL/20.f) * (n)))

#define MOTOR_MS18_MIN PWM_CALC_CYCLE(0.48f)
#define MOTOR_MS18_MAX PWM_CALC_CYCLE(2.5f)
#define MOTOR_MS18_MID (MOTOR_MS18_MAX + MOTOR_MS18_MIN) / 2

#define MOTOR_MG996R_MIN PWM_CALC_CYCLE(0.36f)
#define MOTOR_MG996R_MAX PWM_CALC_CYCLE(2.6f)
#define MOTOR_MG966R_HALF_RANGE ((MOTOR_MG996R_MAX - MOTOR_MG996R_MIN) / 2)
#define MOTOR_MG996R_MID (MOTOR_MG996R_MAX + MOTOR_MG996R_MIN) / 2

#define PWM_INVERT(n) ((PWM_TOTAL + 1) - (n))

// Chopper - Motor Mapping
#define MOTOR_CHOPPER1 0
#define MOTOR_CHOPPER2 1

// Chopper States
#define CHOPPER1_OPEN  MOTOR_MS18_MID - 400
#define CHOPPER1_CLOSE MOTOR_MS18_MID + 20
#define CHOPPER2_OPEN  MOTOR_MS18_MID + 375
#define CHOPPER2_CLOSE MOTOR_MS18_MID + 30

// Z Axis Config
#define Z_AXIS_OFF  MOTOR_MS18_MID
#define Z_AXIS_UP   MOTOR_MS18_MID + ((int)(MOTOR_MG966R_HALF_RANGE) * 0.9f)
#define Z_AXIS_DOWN MOTOR_MS18_MID - ((int)(MOTOR_MG966R_HALF_RANGE) * 0.9f)
#define Z_AXIS_DOWN_TIME 600
#define Z_AXIS_UP_TIME 600

#define Z_AXIS_IS_UP() (gpio_get(SENSOR_Z_AXIS_PIN) == 1)