/**
 * @~japanese
 * @file openEL_utrx-17-1.c
 * @brief		OpenEL UTRX-17-1(PCA9685)(0000000020000001) ソースファイル
 * @Version 3.0.0
 *
 * @~english
 * @file openEL_utrx-17-1.c
 * @brief		OpenEL UTRX-17-1(PCA9685)(0000000020000001) source file
 * @Version 3.0.0
 */
/*

Copyright (c) 2017,2018 Japan Embedded Systems Technology Association(JASA)
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

    Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
    Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
    Neither the name of the Association nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#ifdef __cplusplus
namespace el {
extern "C" {
#endif /* __cplusplus */
/*
#define OPENEL_SW_SURFACE_FRIEND 0
*/
#include "openEL.h"
#include "openEL_utrx-17-1.h"
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <math.h>
#include <linux/i2c-dev.h>
#include <fcntl.h>
#include <sys/ioctl.h>

#define PCA9685_SUBADR1 0x2
#define PCA9685_SUBADR2 0x3
#define PCA9685_SUBADR3 0x4

#define PCA9685_MODE1 0x0
#define PCA9685_PRESCALE 0xFE

#define LED0_ON_L 0x6
#define LED0_ON_H 0x7
#define LED0_OFF_L 0x8
#define LED0_OFF_H 0x9

#define ALLLED_ON_L 0xFA
#define ALLLED_ON_H 0xFB
#define ALLLED_OFF_L 0xFC
#define ALLLED_OFF_H 0xFD

#define PWM_FREQUENCY 60                    //60Hz 16.7ms
#define PWM_PULSE_WIDTH_MAX 12000           //12ms

void PCA9685_init(float freq);
uint8_t PCA9685_read(uint8_t adr);
void PCA9685_write(uint8_t adr, uint8_t dat);
void PCA9685_pwmWrite(uint8_t ch, double pulseWidth_usec);
void PCA9685_setPWM(uint8_t ch, uint16_t onTime, uint16_t offTime);

static int i2c;
int once = 1;

enum ReturnCode Init_0000000200000001(){
  printf("Init_0000000200000001\n");
  if (once) {
		char i2cFileName[] = "/dev/i2c-1";
		int driverAddress = 0x40;
		int i;
		double dfVal;

		if((i2c = open(i2cFileName, O_RDWR)) < 0){
			printf("I2C open err\n");
			return -1;
		}

		if(ioctl(i2c, I2C_SLAVE, driverAddress) < 0){
			printf("ioctl err\n");
			return -1;
		}

		PCA9685_init(PWM_FREQUENCY);

		once = 0;
	}

	return HAL_OK;
}

enum ReturnCode ReInit_0000000200000001(){
  printf("ReInit_0000000200000001\n");
	return HAL_OK;
}

enum ReturnCode Finalize_0000000200000001(){
  printf("Finalize_0000000200000001\n");
  if (once == 0) {
		close(i2c);
		once = 1;
	}
	return HAL_OK;
}

enum ReturnCode AddObserver_0000000200000001(){
  return HAL_OK;
};

enum ReturnCode RemoveObserver_0000000200000001(){
  return HAL_OK;
};

enum ReturnCode GetProperty_0000000200000001(){
  return HAL_OK;
};

enum ReturnCode GetTime_0000000200000001(){
  return HAL_OK;
};

enum ReturnCode SetPosition_0000000200000001(HALCOMPONENT_T **halComponent, halfloat position){
  printf("SetPosition_0000000200000001, position=%f\n", position);
  (*halComponent)->positionControlActuator.actualPosition = position;
  int16_t _minAngle = 0;
  int16_t _maxAngle = 180;
  int16_t _minServoPL = 143;
  int16_t _maxServoPL = 471;
  uint16_t pulseLen = (position-_minAngle)*(_maxServoPL-_minServoPL)/(_maxAngle-_minAngle) + _minServoPL;
  PCA9685_setPWM(Device_ID - 1, 0, pulseLen);
	return HAL_OK;
}

enum ReturnCode GetPosition_0000000200000001(HALCOMPONENT_T **halComponent, halfloat **position){
  printf("GetPosition_0000000200000001\n");
  *position = &(*halComponent)->positionControlActuator.actualPosition;
	return HAL_OK;
}

enum ReturnCode Dummy(){
  return HAL_OK;
};

/* Constant Table (Global scope) */
/*---------------------------------------------------------------------------*/
static const ELMOTOR_FNC_TBL_T motorFncTbl_0000000200000001 = {
        /* 0x00 */ Init_0000000200000001, /**< Initialize */
        /* 0x01 */ ReInit_0000000200000001, /**< ReInit */
        /* 0x02 */ Finalize_0000000200000001, /**< Finalize */
        /* 0x03 */ AddObserver_0000000200000001, /**< AddObserver */
        /* 0x04 */ RemoveObserver_0000000200000001, /**< RemoveObserver */
        /* 0x05 */ GetProperty_0000000200000001, /**< GetProperty */
        /* 0x06 */ GetTime_0000000200000001, /**< GetTime */
        /* 0x07 */ Dummy, /**< Reserved */

        /* 0x08 */ SetPosition_0000000200000001,
        /* 0x09 */ GetPosition_0000000200000001,
        /* 0x0A */ Dummy, /**< Reserved */
        /* 0x0B */ Dummy, /**< Reserved */
        /* 0x0C */ Dummy, /**< Reserved */
        /* 0x0D */ Dummy, /**< Reserved */
        /* 0x0E */ Dummy, /**< Reserved */
        /* 0x0F */ Dummy, /**< Reserved */

        /* 0x10 */ Dummy, /**< Reserved */
        /* 0x11 */ Dummy, /**< Reserved */
        /* 0x12 */ Dummy, /**< Reserved */
        /* 0x13 */ Dummy, /**< Reserved */
        /* 0x14 */ Dummy, /**< Reserved */
        /* 0x15 */ Dummy, /**< Reserved */
        /* 0x16 */ Dummy, /**< Reserved */
        /* 0x17 */ Dummy, /**< Reserved */

        /* 0x18 */ Dummy, /**< Reserved */
        /* 0x19 */ Dummy, /**< Reserved */
        /* 0x1A */ Dummy, /**< Reserved */
        /* 0x1B */ Dummy, /**< Reserved */
        /* 0x1C */ Dummy, /**< Reserved */
        /* 0x1D */ Dummy, /**< Reserved */
        /* 0x1E */ Dummy, /**< Reserved */
        /* 0x1F */ Dummy, /**< Reserved */
};

const EL_CMN_FNC_TBL_T component_0000000200000001 = {(void *)(&motorFncTbl_0000000200000001)};

void PCA9685_init(float freq)
{
    float prescaleval = 25000000;

    PCA9685_write(PCA9685_MODE1, 0x0);
    usleep(100000);//100ms
    uint8_t prescale = 101;
    uint8_t oldmode = PCA9685_read(PCA9685_MODE1);
    uint8_t newmode = (oldmode&0x7F) | 0x10;
    PCA9685_write(PCA9685_MODE1, newmode);
    PCA9685_write(PCA9685_PRESCALE, prescale);
    PCA9685_write(PCA9685_MODE1, oldmode);
    sleep(5);
    PCA9685_write(PCA9685_MODE1, oldmode | 0xa1);
}

void PCA9685_pwmWrite(uint8_t ch, double pulseWidth_usec)
{
    double pulselength;
    double pulseWidth;

    pulselength = 1000000 / PWM_FREQUENCY;
    pulselength /= 4096;
    pulseWidth = pulseWidth_usec / pulselength;

    PCA9685_setPWM(ch, 0, pulseWidth);
}

void PCA9685_setPWM(uint8_t ch, uint16_t onTime, uint16_t offTime)
{
    uint8_t sendData[5];

    sendData[0] = LED0_ON_L + 4 * ch;
    sendData[1] = (uint8_t)(0x00ff & onTime);
    sendData[2] = (uint8_t)((0xff00 & onTime) >> 8);
    sendData[3] = (uint8_t)(0x00ff & offTime);
    sendData[4] = (uint8_t)((0xff00 & offTime) >> 8);

    if(write(i2c, sendData, 5) != 5){
        printf("PCA9685_setPWM() err\n");
    }
}

uint8_t PCA9685_read(uint8_t adr)
{
    uint8_t sendData;
    uint8_t readData;

    sendData = adr;
    if(write(i2c, &sendData, 1) != 1){
        printf("PCA9685_read() err1\n");
    }
    else{
        if(read(i2c, &readData, 1) != 1){
            printf("PCA9685_read() err2\n");
        }
     }

    return readData;
}

void PCA9685_write(uint8_t adr, uint8_t dat)
{
    uint8_t buf[2];

    buf[0] = adr;
    buf[1] = dat;
    if(write(i2c, buf, 2) != 2){
        printf("PCA9685_write() err\n");
    }
}

#ifdef __cplusplus
} /* extern "C" */
} /* namespace el */
#endif /* __cplusplus */
