/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

/*
 * File:   main.cpp
 * Author: pi
 *
 * Created on 2018. május 14., 10:10
 */

#include <cstdlib>
#include "joystick.hh"
#include <unistd.h>
#include <wiringPi.h>
#include <fcntl.h>
//#include <softPwm.h>
//#include "rpiServo.h"

#define PIN0 0
#define PIN1 2
#define PIN2 3
#define PWM_RANGE 100
#define SERVO_PWM_RANGE 50
#define PWM_INIT_VALUE 0

using namespace std;

/*
 *
 */
int main(int argc, char** argv)
{
    wiringPiSetup();
    
    char cmd[100];
    
    //pinMode(0, PWM_OUTPUT);
    int pwm0, pwm1, pwm2;
    pwm0 = 0;
    pwm1 = 0;
    pwm2 = 0;
    
    //int pin0 = softPwmCreate(PIN0, PWM_INIT_VALUE, PWM_RANGE);
    //int pin1 = softPwmCreate(PIN1, PWM_INIT_VALUE, PWM_RANGE);
    //int pin2 = softPwmCreate(PIN2, PWM_INIT_VALUE, SERVO_PWM_RANGE);
    
    //softPwmWrite(0, 256);
    
    //pwmWrite(PIN0, 128);
    
    Joystick joy("/dev/input/js0");
    //rpiServo servo;
    //servo.setAngle(30);
    
    while (true)
    {
        // Restrict rate
        usleep(100);
        
        // Attempt to sample an event from the joystick
        JoystickEvent event;
        if (joy.sample(&event))
        {
            if (event.isButton())
            {
                printf("Button %u is %s\n", event.number, event.value == 0 ? "up" : "down");
            }
            else if (event.isAxis())
            {
                printf("Axis %u is at position %d\n", event.number, event.value);
                if(event.number == 4) //forward
                {
                    if(pwm1 == 0)
                    {
                        pwm0 = ((event.value+32768)/2620);
                        //softPwmWrite(PIN0, pwm0);
                        snprintf(cmd, 100, "echo 1=%d > /dev/servoblaster",-pwm0+150);
                        system(cmd);
                    }
                }
                if(event.number == 5) //backward
                {
                    if(pwm0 == 0)
                    {
                        pwm1 = ((event.value+32768)/2620);
                        //softPwmWrite(PIN2, pwm1);
                        snprintf(cmd, 100, "echo 1=%d > /dev/servoblaster",pwm1+150);
                        system(cmd);
                    }
                }
                if(event.number == 0) //servo
                {
                    pwm2 = (event.value/1092)+152;
                    //fprintf(fp, "2=%d",pwm2);
                    //fflush(fp);
                    snprintf(cmd, 100, "echo 2=%d > /dev/servoblaster",pwm2);
                    system(cmd);
                    std::cout << "PWM: " << pwm2 << std::endl;
                }
                
            }
        }
    }
    return 0;
}