///
/// @file       rotor.cpp
/// @copyright  Christian Obersteiner, Andreas Müller - CC-BY-SA 4.0
///
/// @brief  Contains all kind of rotor functions
///

#include "rotor.h"

///
/// @brief  Constructor
///

Rotor::Rotor()
{
    initRotor();
}

///
/// @brief  Initialize all ADC Values and set Arduino Pins to a defined value.
///

void Rotor::initRotor()
{
    if(AZIM)
    {
        //Pin Settings
        pinMode(AZIM_RE_CW, OUTPUT);
        pinMode(AZIM_RE_CCW, OUTPUT);
        pinMode(AZIM_RE_BREAK, OUTPUT);
        //All Relais Off
        digitalWrite(AZIM_RE_CW, HIGH); 
        digitalWrite(AZIM_RE_CCW, HIGH); 
        digitalWrite(AZIM_RE_BREAK, HIGH);
        //Initial Values
        m_bAzimuthBreakReleased = 0;
        m_bAzimuthCurrentlyRotating = 0;
        m_bAzimuthShouldRotate = 0;
    }
    if(ELEV)
    {
        //Pin Settings
        pinMode(ELEV_RE_UP, OUTPUT);
        pinMode(ELEV_RE_DWN, OUTPUT);
        pinMode(ELEV_RE_BREAK, OUTPUT);
        //All Relais Off
        digitalWrite(ELEV_RE_UP, HIGH);
        digitalWrite(ELEV_RE_DWN, HIGH);
        digitalWrite(ELEV_RE_BREAK, HIGH);
        //Initial Values
        m_bElevationBreakReleased = 0;
        m_bElevationCurrentlyRotating = 0;
        m_bElevationShouldRotate = 0;
    }
}

///
/// @brief  Does all the rotor movement stuff
///

void Rotor::doRotor()
{
    int diff_elev, diff_azim;
    readRotSensors();
    if(AZIM && m_bAzimuthShouldRotate)
    {
        diff_azim = m_uActualAzimuth - m_uSetAzimuth;
        if(m_bAzimuthCurrentlyRotating) {
            if(abs(diff_azim) < AZIM_RES)
                stopAzimuth();
            else {
              if(diff_azim > 0)
                rotateCW();
              else
                rotateCCW();
            }
        }
        else {
            if(abs(diff_azim) > AZIM_SPAN) {
              releaseBreakAzimuth();
              if(diff_azim > 0)
                rotateCW();
              else
                rotateCCW();
            }
        }
    }
    if(ELEV && m_bElevationShouldRotate)
    {
        diff_elev = m_uActualElevation - m_uSetElevation;
        if(m_bElevationCurrentlyRotating) {
            if(abs(diff_elev) < ELEV_RES)
                stopElevation();
            else {
              if(diff_elev > 0)
                rotateDown();
              else
                rotateUp();
            }
        }
        else {
            if(abs(diff_elev) > ELEV_SPAN) {
              releaseBreakElevation();
              if(diff_elev > 0)
                rotateDown();
              else
                rotateUp();
            }
        }
    }

    if ( !m_bAzimuthCurrentlyRotating && !m_bElevationCurrentlyRotating ) {
        tightenBreakAzimuth();
        tightenBreakElevation();
    }
}

void Rotor::releaseBreakAzimuth()
{
    m_bAzimuthBreakReleased = 1;
    digitalWrite(AZIM_RE_BREAK, LOW);
}

void Rotor::releaseBreakElevation()
{
    m_bElevationBreakReleased = 1;
    digitalWrite(ELEV_RE_BREAK, LOW);
}

void Rotor::tightenBreakAzimuth()
{
    m_bAzimuthBreakReleased = 0;
    digitalWrite(AZIM_RE_BREAK, HIGH);
}

void Rotor::tightenBreakElevation()
{
    m_bElevationBreakReleased = 0;
    digitalWrite(ELEV_RE_BREAK, HIGH);
}

void Rotor::stopAzimuth()
{
    digitalWrite(AZIM_RE_CW, HIGH);
    digitalWrite(AZIM_RE_CCW, HIGH);
    m_bAzimuthCurrentlyRotating = 0;
    m_bAzimuthShouldRotate = 0;
}

void Rotor::stopElevation()
{
    digitalWrite(ELEV_RE_UP, HIGH);
    digitalWrite(ELEV_RE_DWN, HIGH);
    m_bElevationCurrentlyRotating = 0;
    m_bElevationShouldRotate = 0;
}

Rotor::rotorError Rotor::rotateCW()
{
    if ( m_bAzimuthBreakReleased == 0 )
        return Rotor::ROT_BREAK_NOT_RELEASED;
    digitalWrite(AZIM_RE_CW, LOW);
    digitalWrite(AZIM_RE_CCW, HIGH);
    m_bAzimuthCurrentlyRotating = 1;
    return Rotor::ROT_OK;
}

Rotor::rotorError Rotor::rotateCCW()
{
    if ( m_bAzimuthBreakReleased == 0 )
        return Rotor::ROT_BREAK_NOT_RELEASED;
    digitalWrite(AZIM_RE_CW, HIGH);
    digitalWrite(AZIM_RE_CCW, LOW);
    m_bAzimuthCurrentlyRotating = 1;
    return Rotor::ROT_OK;
}

Rotor::rotorError Rotor::rotateUp()
{
    if ( m_bElevationBreakReleased == 0 )
        return Rotor::ROT_BREAK_NOT_RELEASED;
    digitalWrite(ELEV_RE_UP, LOW);
    digitalWrite(ELEV_RE_DWN, HIGH);
    m_bElevationCurrentlyRotating = 1;
    return Rotor::ROT_OK;
}

Rotor::rotorError Rotor::rotateDown()
{
    if ( m_bElevationBreakReleased == 0 )
        return Rotor::ROT_BREAK_NOT_RELEASED;
    digitalWrite(ELEV_RE_UP, HIGH);
    digitalWrite(ELEV_RE_DWN, LOW);
    m_bElevationCurrentlyRotating = 1;
    return Rotor::ROT_OK;
}


///
/// @brief  Serial Debug output Function
///

void Rotor::debugOut()
{
    //debug Out
    readRotSensors();
    Serial.println("-----------------");
    Serial.print("Azim Value: ");
    Serial.println(m_uActualAzimuth);
    Serial.print("Elev Value: ");
    Serial.println(m_uActualElevation);
}

///
/// @brief Returns the actual Azimuth value in degree
///
/// @return uint16 actual Azimuth value in degree
///

uint16 Rotor::getActualAzimuth() const
{
    return m_uActualAzimuth;
}

///
/// @brief Returns the actual Elevation value in degree
///
/// @return actual Elevation value in degree
///

uint16 Rotor::getActualElevation() const
{
    return m_uActualElevation;
}

///
/// @brief Sets the actual Azimuth value in degree and checks valid range
///
/// @param pAzimuth Azimuth value to be set
///
/// @return Error Code
///

Rotor::rotorError Rotor::setAzimuth(uint16 pAzimuth)
{
    if(pAzimuth >= AZIM_MIN && pAzimuth <= AZIM_MAX)
    {
        m_uSetAzimuth = pAzimuth;
        m_bAzimuthShouldRotate = 1;
        return Rotor::ROT_OK;
    }
    else
    {
        return Rotor::ROT_VAL_OUT_OF_RANGE;
    }
}

///
/// @brief Sets the actual Elevation value in degree and checks valid range
///
/// @param pElevation Elevation value to be set
///
/// @return Error Code
///

Rotor::rotorError Rotor::setElevation(uint16 pElevation)
{
    if(pElevation >= ELEV_MIN && pElevation <= ELEV_MAX)
    {
        m_uSetElevation = pElevation;
        m_bElevationShouldRotate = 1;
        return Rotor::ROT_OK;
    }
    else
    {
        return Rotor::ROT_VAL_OUT_OF_RANGE;
    }
}

///
/// @brief Reads out the Sensor values and stores them
///
/// @return Error Code
///

Rotor::rotorError Rotor::readRotSensors()
{
    if(AZIM)
      m_uActualAzimuth = (uint16)(readADC(AZIM_POT)/AZIM_DEG);
    if(ELEV)
      m_uActualElevation = (uint16)(readADC(ELEV_POT)/ELEV_DEG);
    
    return Rotor::ROT_OK;
}

///
/// @brief Reads out an ADC Pin 4 times and returns the average
///
/// @param AdcPin ADC Pin to read
///
/// @return average ADC value
///

uint16 Rotor::readADC(int AdcPin)
{
    //Read ADC 4 times and calculate average
    uint32_t adcValue = 0;
    for(int i = 0; i < 128; i++)
        adcValue += analogRead(AdcPin);
        delay(1);
    adcValue = adcValue/128;
    return (uint16)adcValue;
}