#include <stdio.h>
#include <string.h>
#include <unistd.h>

#include "motors.h"

void motor_esc_initialize(unsigned int pin) {
    //servo_mv_r(pin, 0);
    //sleep(0.5);
    servo_mv_r(pin, MAIN_PROP_STOP_PPM);
    //sleep(1);
    //servo_mv_r(pin, MAIN_PROP_START_PPM);
}

void all_motors_stop() {
    servo_mv_r(MAIN_PROP_GPIO,  MAIN_PROP_STOP_PPM);
    servo_mv_r(LEFT_PROP_GPIO,  LEFT_PROP_STOP_PPM);
    servo_mv_r(RIGHT_PROP_GPIO, RIGHT_PROP_STOP_PPM);
}
void all_motors_zero() {
    servo_mv_r(MAIN_PROP_GPIO,  0);
    servo_mv_r(LEFT_PROP_GPIO,  0);
    servo_mv_r(RIGHT_PROP_GPIO, 0);
}
void all_motors_start() {
    servo_mv_r(MAIN_PROP_GPIO,  MAIN_PROP_START_PPM);
    servo_mv_r(LEFT_PROP_GPIO,  LEFT_PROP_START_PPM);
    servo_mv_r(RIGHT_PROP_GPIO, RIGHT_PROP_START_PPM);
}

int map_esc_per(unsigned int pin, int *lv) {
    switch(pin) {
        case MAIN_PROP_GPIO:
            *lv = map(*lv, 0, 100, MAIN_PROP_IDLE_PPM, MAIN_PROP_MAX_PPM);
            break;
        case LEFT_PROP_GPIO:
            *lv = map(*lv, 0, 100, LEFT_PROP_IDLE_PPM, LEFT_PROP_MAX_PPM);
            break;
        case RIGHT_PROP_GPIO:
            *lv = map(*lv, 0, 100, RIGHT_PROP_IDLE_PPM, RIGHT_PROP_MAX_PPM);
            break;
        default:
            *lv = map(*lv, 0, 100, ESC_MIN, ESC_MAX);
    }
    return *lv;
}

void motor_process(char*msg){
    char servo_index = msg[4];
    char servo_mode  = msg[5];
    int lv;
    char action[5];
    unsigned int pin;

    static bool started=false;

    sscanf(&msg[3]," %s ", action);
    eprintf("Act: %s\n", action);

    if (strcmp(action, "START") == 0) {
        if (!started) {
            eprintf("Estarting ESCs\n");
            all_motors_stop();  // set all motors to stop position
            usleep(10000);      // wait just a bit (10000 µs or 0,01 secs)
            all_motors_start(); // set all to start speed
            //servo_mv_r(MAIN_PROP_GPIO, MAIN_PROP_START_PPM);
            started=true;

            strcpy(msg, "ESC_STARTED");
        } else {
            eprintf("ESC already started\n");

            strcpy(msg, "ESC_IS_STARTED");
        }
        return;
    } else
    if (strcmp(action, "STOP") == 0) {
        //servo_mv_r(MAIN_PROP_GPIO, MAIN_PROP_STOP_PPM);
        all_motors_stop();  // Stops all ESCs
        started=false;

        strcpy(msg, "ESC_STOPPED");
        return;
    } else {
        sscanf(&msg[6], " %i ", &lv );
    }

    if (servo_index == 'M')
        pin = MAIN_PROP_GPIO;
    else
    if (servo_index == 'L')
        pin = LEFT_PROP_GPIO;
    else
    if (servo_index == 'R')
        pin = RIGHT_PROP_GPIO;
    else {
        eprintf("[motors] index not valid (%c)\n", servo_index);
        strcpy(msg, "ESC_INDX_NOK");
        return;
    }
    
    switch (servo_mode) {
        case ' ':
            if (lv < 500)
                servo_mode = '%';
            else {
                servo_mode = 'R';
                break;
            }
        case '%':
            map_esc_per(pin, &lv);
            break;
        case 'R':
            break;
        default:
            eprintf("[motors] mode not valid (%c)\n", *servo_mode);
            strcpy(msg, "ESC_MODE_NOK");
            return;
    }

    servo_mv_r(pin, lv);
    sprintf(msg, "ESC %c%c %i", servo_index, servo_mode, lv);
}