#include <Wire.h>
#include <PWM.h>

// --- Motor Control Pins ---
#define BRAKE         8
#define PWM_PIN       9 
#define DIRECTION     4

// --- Encoder Pins ---
#define ENCODER_A_PIN 2
#define ENCODER_B_PIN 3

// --- Encoder Variables ---
volatile long encoderCount = 0;

const uint16_t PWM_FREQUENCY = 20000;
const uint16_t PWMVALUE = F_CPU / PWM_FREQUENCY / 2;

void setup() {
  // put your setup code here, to run once:
armLenght = 5; //mm center to center
wireDiameter = 0.2;
coilHeight = 8;
coilOD = 36;
coilID = 12;

encoderSPR = 400;


numRevMax = (coilOD - coilID) / wireDiameter;

Serial.begin(115200);
  Serial.println("setup starting");

  // --- Pin Configuration ---
  pinMode(PWM_PIN, OUTPUT);
  pinMode(DIRECTION, OUTPUT);
  pinMode(BRAKE, OUTPUT);

  // --- PWM Setup ---
  TCCR1B = (1 << WGM13) | (1 << CS10);
  ICR1 = PWMVALUE;
  TCCR1A = (1 << COM1A1) | (1 << COM1B1);

  setPWM(PWMVALUE); // Initialize PWM to PWMVALUE (effectively "off" in inverted logic for setPWM)

  digitalWrite(BRAKE, HIGH);
  digitalWrite(DIRECTION, LOW); // Initial direction

  Serial.println("Motor control initialized.");

  // --- Encoder Setup ---
  pinMode(ENCODER_A_PIN, INPUT_PULLUP);
  pinMode(ENCODER_B_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENCODER_A_PIN),  handleEncoderA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_B_PIN),  handleEncoderB, CHANGE);
}

void loop() {

while numRev < numRevMax {
  encoderVal = encoderCount;
  numRev = encoderSPR / EncoderVal;
  Motor_control(20);

  }

}

void setPWM(uint16_t dutyCycle) {
  OCR1A = dutyCycle;
}

void Motor_control(int pwm) {
  uint16_t pwm_output_value; // Use uint16_t for setPWM dutyCycle
  if (pwm > 0) {
    digitalWrite(DIRECTION, HIGH); // Forward
    pwm_output_value = map(pwm, 0, 255, PWMVALUE, 0); // Map 0-255 input to PWMVALUE-0 output for setPWM
  } else if (pwm < 0) {
    digitalWrite(DIRECTION, LOW);  // Reverse
    pwm_output_value = map(abs(pwm), 0, 255, PWMVALUE, 0); // Map abs(0 to -255) input to PWMVALUE-0 output for setPWM
  } else {
    pwm_output_value = PWMVALUE;      // Stop (Inverted PWM "off" for setPWM is PWMVALUE)
  }
  setPWM(pwm_output_value); // Use setPWM with the correctly mapped value
}


// --- Interrupt Service Routines (ISRs) for Encoder ---
void handleEncoderA() {
  bool encoderBState = digitalRead(ENCODER_B_PIN);
  if (digitalRead(ENCODER_A_PIN) == HIGH) {
    if (encoderBState == LOW) {
      encoderCount++;
    } else {
      encoderCount--;
    }
  } else {
    if (encoderBState == HIGH) {
      encoderCount--;
    } else {
      encoderCount--;
    }
  }
}

void handleEncoderB() {
  bool encoderAState = digitalRead(ENCODER_A_PIN);
   if (digitalRead(ENCODER_B_PIN) == HIGH) {
    if (encoderAState == HIGH) {
      encoderCount++;
    } else {
      encoderCount--;
    }
  } else {
    if (encoderAState == LOW) {
      encoderCount++;
    } else {
      encoderCount--;
    }
  }
}