#include <AccelStepper.h>
#include <math.h> // Include the math library for fabs
#include <SPI.h>
#include <stdio.h>
#include <stdlib.h>

#define stepPin 3
#define dirPin 2
#define motorInterfaceType 1
#define csPin 10 //MCP3301 CS pin



#define degreePrStep  0.1125
#define degreesToSteps(degrees) ((degrees) / degreePrStep)
#define SWEEP_ANGLE 26
#define ACCELL_ANGLE 12
#define SWEEP_TIME 0.2


AccelStepper stepper(motorInterfaceType, stepPin, dirPin);
static void printBinary16(uint16_t num);
static void printBinary8(uint8_t num);




void calculateMotionParameters(float sweepAngle, float accelAngle, float sweepTime, float& maxSpeed, float& acceleration) {
    double totalSteps = degreesToSteps(sweepAngle);
    double accelDecelSteps = degreesToSteps(accelAngle);
    double constantSpeedSteps = totalSteps - 2 * accelDecelSteps;

    // Initial estimate of maxSpeed (assuming no acceleration/deceleration)
    maxSpeed = totalSteps / sweepTime;

    // Tolerance for the difference in calculated time and desired time
    double tolerance = 0.01; 
    double totalTimeCalculated;

    // Iteratively adjust maxSpeed
    do {
        // Calculate the difference between the calculated time and the desired time
        double timeDifference = fabs(totalTimeCalculated - sweepTime);
        Serial.print("timeDifference: "); Serial.println(timeDifference);

        // Dynamically adjust the increment value based on the time difference
        double increment = fmax(10, timeDifference * 100); // Adjust the scaling factor as needed
        Serial.print("increment: "); Serial.println(increment);
        // Increase maxSpeed by the increment value
        maxSpeed += increment;
        Serial.print("maxSpeed: "); Serial.println(maxSpeed);
        // Calculate acceleration (a = v^2 / (2 * s))
        acceleration = maxSpeed * maxSpeed / (2.0 * accelDecelSteps);
        Serial.print("acceleration: "); Serial.println(acceleration);
        // Time for acceleration and deceleration (t = sqrt(2 * s / a))
        double timeForAccelDecel = sqrt(2.0 * accelDecelSteps / acceleration) * 2; // Multiply by 2 for both accel and decel
        Serial.print("timeForAccelDecel: "); Serial.println(timeForAccelDecel);
        // Time for constant speed (t = s / v)
        double timeForConstantSpeed = constantSpeedSteps / maxSpeed;
        Serial.print("timeForConstantSpeed: "); Serial.println(timeForConstantSpeed);
        // Total time
        totalTimeCalculated = timeForAccelDecel + timeForConstantSpeed;
      Serial.print("totalTimeCalculated: "); Serial.println(totalTimeCalculated);
    } while (fabs(totalTimeCalculated - sweepTime) > tolerance);

    // Print the calculated values for diagnostics
    Serial.print("Final maxSpeed: "); Serial.println(maxSpeed);
    Serial.print("Acceleration: "); Serial.println(acceleration);
}





void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200); // Any baud rate should work  
   

  
  pinMode(csPin, OUTPUT);
  digitalWrite(csPin, HIGH); // Deselect the MCP3301

  SPI.begin();
  // Set SPI clock to 100 kHz
  SPI.beginTransaction(SPISettings(100000, MSBFIRST, SPI_MODE0));


  float maxSpeed, acceleration;
  calculateMotionParameters(SWEEP_ANGLE, ACCELL_ANGLE, SWEEP_TIME, maxSpeed, acceleration);

  // Now use maxSpeed and acceleration to set up your stepper
  stepper.setMaxSpeed(maxSpeed);
  stepper.setSpeed(maxSpeed);
  stepper.setAcceleration(acceleration);

}

void loop() {
  static bool movingForward = true;
  static unsigned long lastTime = 0; // Variable to store the last time we printed

  // Check if the motor has reached its target
  if (stepper.distanceToGo() == 0) {
    unsigned long currentTime = millis(); // Get the current time
    if (lastTime != 0) { // Check if this is not the first run
      unsigned long timeTaken = currentTime - lastTime; // Calculate the time taken
      Serial.print("Time taken for sweep: "); Serial.print(timeTaken); Serial.println(" ms");
    }
    lastTime = currentTime; // Update the last time

    if (movingForward) {
      // Once reached the forward position, set a new target backward
      stepper.moveTo(degreesToSteps(0));
      movingForward = false;
    } else {
      // Once reached the backward position, set a new target forward
      stepper.moveTo(degreesToSteps(SWEEP_ANGLE));
      movingForward = true;
    }
  }

  // Read ADC value at each step
  if (stepper.currentPosition() % static_cast<int>(round(degreesToSteps(1))) == 0) {
    int adcValue = readMCP3301();
    Serial.print("Step: "); Serial.print(stepper.currentPosition());
    Serial.print(", ADC Value: "); Serial.println(adcValue);
    Serial.println("------------");
  }

  // Run the stepper motor continuously
  stepper.run();
}





int readMCP3301() {
  digitalWrite(csPin, LOW); // Select the MCP3301

  // Perform the first 8-bit SPI transfer
  int firstByte = SPI.transfer(0); // Contains null bit, sign bit, and highest order 4 bits
  printBinary8(firstByte);
  // Perform the second 8-bit SPI transfer
  int secondByte = SPI.transfer(0); // Contains the lowest order 8 data bits
  printBinary8(secondByte);
  digitalWrite(csPin, HIGH); // Deselect the MCP3301

  // Combine the two parts to form the 13-bit ADC value
  int adcValue = ((firstByte & 0x1F) << 8) | secondByte; // Mask out the leading zeros and null bit, then combine
  adcValue &= 0x1FFF; // Mask the 13-bit data


  Serial.print("Raw ADC Value: "); Serial.println(adcValue); // Debug print
  printBinary16(adcValue);

  
  // Convert adcValue to signed integer
  if (adcValue & 0x1000) { // Check if the sign bit is set
    adcValue = adcValue - 0x2000;
  }
  return adcValue;
}




void printBinary16(uint16_t num) {
    int bits = sizeof(num) * 8; // Total bits in the number
    char binary[bits + 1];      // +1 for null terminator
    binary[bits] = '\0';        // Null-terminate the string

    for (int i = bits - 1; i >= 0; i--) {
        binary[i] = (num & 1) ? '1' : '0'; // Check the least significant bit
        num >>= 1;                         // Shift right by one bit
    }

    Serial.println(binary);
}

void printBinary8(uint8_t num) {
    int bits = sizeof(num) * 8; // Total bits in the number
    char binary[bits + 1];      // +1 for null terminator
    binary[bits] = '\0';        // Null-terminate the string

    for (int i = bits - 1; i >= 0; i--) {
        binary[i] = (num & 1) ? '1' : '0'; // Check the least significant bit
        num >>= 1;                         // Shift right by one bit
    }

    Serial.println(binary);
}