//--------------------------------------------------------------
//-- Oscillator.pde
//-- Generate sinusoidal oscillations in the servos
//--------------------------------------------------------------
//-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011
//-- GPL license
//--------------------------------------------------------------

#include "Arduino.h"
#include "Oscillator.h"

#if defined(ESP32)
  #include <ESP32_Servo.h>
#else
  #include <Servo.h>
#endif

//-- This function returns true if another sample
//-- should be taken (i.e. the TS time has passed since
//-- the last sample was taken
bool Oscillator::next_sample()
{

  //-- Read current time
  _currentMillis = millis();

  //-- Check if the timeout has passed
  if(_currentMillis - _previousMillis > _TS) {
    _previousMillis = _currentMillis;

    return true;
  }

  return false;
}

//-- Attach an oscillator to a servo
//-- Input: pin is the arduino pin were the servo
//-- is connected
void Oscillator::attach(int pin, bool rev)
{
  //-- If the oscillator is detached, attach it.

    //-- Attach the servo and move it to the home position
      _servo.attach(pin);
      _servo.write(90);

      //-- Initialization of oscilaltor parameters
      _TS=30;
      _T=2000;
      __N = _T/_TS;
      _inc = 2*M_PI/__N;

      _previousMillis=0;

      //-- Default parameters
      _A=45;
      _phase=0;
      _phase0=0;
      _O=0;
      _stop=false;

      //-- Reverse mode
      _rev = rev;

}

//-- Detach an oscillator from his servo
void Oscillator::detach()
{
   //-- If the oscillator is attached, detach it.
  if(_servo.attached())
        _servo.detach();

}

/*************************************/
/* Set the oscillator period, in ms  */
/*************************************/
void Oscillator::SetT(unsigned int T)
{
  //-- Assign the new period
  _T=T;

  //-- Recalculate the parameters
  __N = _T/_TS;
  _inc = 2*M_PI/__N;
};

/*******************************/
/* Manual set of the position  */
/******************************/

void Oscillator::SetPosition(int position)
{
  _servo.write(position+_trim);
};


/*******************************************************************/
/* This function should be periodically called                     */
/* in order to maintain the oscillations. It calculates            */
/* if another sample should be taken and position the servo if so  */
/*******************************************************************/
void Oscillator::refresh()
{

  //-- Only When TS milliseconds have passed, the new sample is obtained
  if (next_sample()) {

      //-- If the oscillator is not stopped, calculate the servo position
      if (!_stop) {
        //-- Sample the sine function and set the servo pos
        _pos = round(_A * sin(_phase + _phase0) + _O);
	if (_rev) _pos=-_pos;
		Serial.println(_pos+90+_trim);
        _servo.write(_pos+90+_trim);
      }

      //-- Increment the phase
      //-- It is always increased, even when the oscillator is stop
      //-- so that the coordination is always kept
      _phase = _phase + _inc;

  }
}