/*
 Rocrail - Model Railroad Software
 Copyright (c) 2002-2022 Robert Jan Versluis, Rocrail.net
 All rights reserved.
*/


#include "RCAN_dfs.h"
#include "servos.h"
#include "trace.h"
#include "prot.h"
#include "cv.h"
#include "io.h"


Servos::Servos() {
  trace("Servo option");
  memset( m_ServoCnf, 0, sizeof(struct _SERVOCNF) * MAX_SERVO );

/* D3, D5, D6, D9
  m_Pins[ 1] = 3; // PWM J3-4
  m_Pins[ 3] = 5; // PWM J3-6
  m_Pins[ 4] = 6; // PWM J3-7
  m_Pins[ 7] = 9; // PWM J3-10
*/

#ifdef ARDUINO_ARCH_RP2040
  m_ServoPins[0] = 6;
  m_ServoPins[1] = 7;
  m_ServoPins[2] = 8;
  m_ServoPins[3] = 9;
#else
  // Overwrite EEProm configuration
  CV::set(EE_PORTCFG+1, PORTCFG_OUT);
  CV::set(EE_PORTCFG+3, PORTCFG_OUT);
  CV::set(EE_PORTCFG+4, PORTCFG_OUT);
  CV::set(EE_PORTCFG+7, PORTCFG_OUT);
  m_ServoPins[0] = 3;
  m_ServoPins[1] = 5;
  m_ServoPins[2] = 6;
  m_ServoPins[3] = 9;
#endif

  for( byte i = 0; i < MAX_SERVO; i++ ) {
    byte pos = CV::get(EE_SERVO_POS+i);
    if( pos > 180 ) pos = 90;
    m_ServoCnf[i].gotopos = pos;
    m_ServoCnf[i].curpos  = pos;
    pinMode(m_ServoPins[i], OUTPUT);
    m_Servo[i] = new Servo();
    m_Servo[i]->write(m_ServoCnf[i].curpos);  
    m_Servo[i]->attach(m_ServoPins[i]);
  }
  
}


bool Servos::tick10ms(iCANFrame frame, iCmdData cmd) {
  bool report = false;
  
  for( byte i = 0; i < MAX_SERVO; i++ ) {
    if( m_ServoCnf[i].gotopos != m_ServoCnf[i].curpos ) {
      m_ServoCnf[i].step++;
      if( m_ServoCnf[i].step >= m_ServoCnf[i].delay ) {
        m_ServoCnf[i].step = 0;
        if( m_ServoCnf[i].gotopos > m_ServoCnf[i].curpos )
          m_ServoCnf[i].curpos++;
        else
          m_ServoCnf[i].curpos--;

        m_Servo[i]->write(m_ServoCnf[i].curpos);  
   
        if( m_ServoCnf[i].gotopos == m_ServoCnf[i].curpos ) {
          // save and report
          CV::set(EE_SERVO_POS+i, m_ServoCnf[i].curpos );
          report = true;

          cmd->cmd    = EVT_SENSOR;
          cmd->addr   = i;
          cmd->value  = m_ServoCnf[i].val;
          cmd->ivalue = 0;
          
          break; // stop for-loop because only one servo can be reported...
        }
      }
    }
  }  
  
  return report;
}


bool Servos::doServo(iCANFrame frame, int addr, byte pos, byte tick, byte val, byte param) {
  trace("servo %d pos=%d tick=%d val=%d param=%d", addr, pos, tick, val, param );
  if( addr < MAX_SERVO ) {
    m_ServoCnf[addr].gotopos = pos;
    m_ServoCnf[addr].delay   = tick;
    m_ServoCnf[addr].val     = val; // end position on/off reporting
    m_ServoCnf[addr].param   = param;

    //m_Servo[addr]->write(pos);
  }
  return false;
}
