#include "yocto_api.h"
#include "yocto_motor.h"
#include "yocto_temperature.h"
#include "yocto_current.h"
#include "yocto_voltage.h"
#include <iostream>
#include <ctype.h>
#include <stdlib.h>

using namespace std;

static void usage(void)
{
    cout << "usage: demo <serial_number> power" << endl;
    cout << "       demo <logical_name>  power" << endl;
    cout << "       demo any power       (use any discovered device)" << endl;
    cout << "       power is a integer between -100 and 100%%" << endl;
    cout << "Example:" << endl;
    cout << "       demo any 75" << endl;

    u64 now = yGetTickCount(); 
    while (yGetTickCount()-now<3000) {
        // wait 3 sec to show the message
    }
    exit(1);
}

int main(int argc, const char * argv[])
{
    string  target,errmsg;
    int power;
    YMotor *motor;
    YCurrent *current; 
    YVoltage *voltage;
    YTemperature *temperature;

      // parse command line
    if (argc < 2) {
        usage();
    }
    target = (string) argv[1];
    power = atoi(argv[2]);

    // Setup the API to use local USB devices
    if (YAPI::RegisterHub("usb", errmsg) != YAPI_SUCCESS) {
        cerr << "RegisterHub error: " << errmsg << endl;
        return 1;
    }
    
    if (target == "any") {
        // find the serial# of the first available motor
        motor =  YMotor::FirstMotor();        
        if (motor == NULL) {
            cout << "No module connected (check USB cable)" << endl;
            return 1;
        }
        target= motor->get_module()->get_serialNumber();
    }

    // retreive motor, current, voltage and temperature features from the device
    motor       = YMotor::FindMotor(target + ".motor");
    current     = YCurrent::FindCurrent(target + ".current");
    voltage     = YVoltage::FindVoltage(target + ".voltage");
    temperature = YTemperature::FindTemperature(target + ".temperature");

    // lets start the motor
    if (motor->isOnline()) {  
        // if motor is in error state, reset it.
        if (motor->get_motorStatus()>=YMotor::MOTORSTATUS_LOVOLT) { 
            motor->resetStatus();
        }
        motor->drivingForceMove(power,2000);  // ramp up to power in 2 seconds
        while (1) {
            // display motor status
            cout << "Status=" << motor->get_advertisedValue() << "  "
                 << "Voltage=" << voltage->get_currentValue() << "V  " 
                 << "Current=" << current->get_currentValue()/1000 << "A  "
                 << "Temp=" << temperature->get_currentValue() << "deg C" << endl;
            YAPI::Sleep(1000, errmsg); // wait for one second
        }
    } else {
        cout << "Module not connected (check identification and USB cable)"<< endl;
    }
    return 0; 
}
