#include "Arduino.h"
#include "wiring_private.h"

// only pin 5 and 6 support tone function
#define PWMTMR_TONE     (0x104)
#define MAX_TONE_FREQ   (100000) //100kHz

static const char *tone_dev = "/dev/pwmtimer";

typedef struct tagTone_Config {
    uint8_t pin;
    unsigned int clksrc_div;
    unsigned int active_cycle;
} Tone_Config,*pTone_Config;

/*
const float pwm_source = 24000000.0;
int pwm_divider[10] = {120, 180, 240, 360, 480, 12000, 24000, 36000, 48000, 72000};
int pwm_max_cycle[10] = {128, 128, 128, 128, 128, 128, 128, 128, 128, 128};

    
void findPWMSetting(int in_freq, int* pwm_source_div, int* pwm_active_cycle)
{
    double need = in_freq;
    double delta = in_freq / 100 + 1;
    *pwm_source_div = 0;
    *pwm_active_cycle = 0;
    int i, j;
    for(i=0; i<10; i++)
    {
        for(j=1; j<pwm_max_cycle[i]+1; j++)
        {
            float freq = pwm_source / (pwm_divider[i] * j * 2);
            if( abs(freq - need) < delta )
            {
                delta = abs(freq - need);
                *pwm_source_div = pwm_divider[i];
                *pwm_active_cycle = j;
            }
        }
    }   
}
*/
struct tone_freq{int f;int div;int cycle;};
struct tone_freq t[]= {
        {100000,120,2},
        {66667,180,2},
        {50000,240,2},
        {33333,360,2},
        {25000,480,2},
        {22222,180,6},
        {20000,120,10},
        {16667,360,4},
        {14286,120,14},
        {13333,180,10},
        {12500,480,4},
        {11111,360,6},
        {10000,240,10},
        {9524,180,14},
        {9091,120,22},
        {8333,480,6},
        {7692,120,26},
        {7407,180,18},
        {7143,240,14},
        {6667,360,10},
        {6250,480,8},
        {6061,180,22},
        {5882,120,34},
        {5556,360,12},
        {5263,120,38},
        {5128,180,26},
        {5000,480,10},
        {4762,360,14},
        {4545,240,22},
        {4444,180,30},
        {4348,120,46},
        {4167,480,12},
        {4000,120,50},
        {3922,180,34},
        {3846,240,26},
        {3704,360,18},
        {3571,480,14},
        {3509,180,38},
        {3448,120,58},
        {3333,360,20},
        {3226,120,62},
        {3175,180,42},
        {3125,480,16},
        {3030,360,22},
        {2941,240,34},
        {2899,180,46},
        {2857,120,70},
        {2778,480,18},
        {2703,120,74},
        {2667,180,50},
        {2632,240,38},
        {2564,360,26},
        {2500,480,20},
        {2469,180,54},
        {2439,120,82},
        {2381,360,28},
        {2326,120,86},
        {2299,180,58},
        {2273,480,22},
        {2222,360,30},
        {2174,240,46},
        {2151,180,62},
        {2128,120,94},
        {2083,480,24},
        {2041,120,98},
        {2020,180,66},
        {2000,240,50},
        {1961,360,34},
        {1923,480,26},
        {1905,180,70},
        {1887,120,106},
        {1852,360,36},
        {1818,120,110},
        {1802,180,74},
        {1786,480,28},
        {1754,360,38},
        {1724,240,58},
        {1709,180,78},
        {1695,120,118},
        {1667,480,30},
        {1639,120,122},
        {1626,180,82},
        {1613,240,62},
        {1587,360,42},
        {1563,480,32},
        {1550,180,86},
        {1538,120,130},
        {1515,360,44},
        {1493,120,134},
        {1481,180,90},
        {1471,480,34},
        {1449,360,46},
        {1429,240,70},
        {1418,180,94},
        {1408,120,142},
        {1389,480,36},
        {1370,120,146},
        {1361,180,98},
        {1351,240,74},
        {1333,360,50},
        {1316,480,38},
        {1307,180,102},
        {1299,120,154},
        {1282,360,52},
        {1266,120,158},
        {1258,180,106},
        {1250,480,40},
        {1235,360,54},
        {1220,240,82},
        {1212,180,110},
        {1205,120,166},
        {1190,480,42},
        {1176,120,170},
        {1170,180,114},
        {1163,240,86},
        {1149,360,58},
        {1136,480,44},
        {1130,180,118},
        {1124,120,178},
        {1111,360,60},
        {1099,120,182},
        {1093,180,122},
        {1087,480,46},
        {1075,360,62},
        {1064,240,94},
        {1058,180,126},
        {1053,120,190},
        {1042,480,48},
        {1031,120,194},
        {1026,180,130},
        {1020,240,98},
        {1010,360,66},
        {1000,12000,2},
        {995,180,134},
        {990,120,202},
        {980,360,68},
        {971,120,206},
        {966,180,138},
        {962,480,52},
        {952,360,70},
        {943,240,106},
        {939,180,142},
        {935,120,214},
        {926,480,54},
        {917,120,218},
        {913,180,146},
        {909,240,110},
        {901,360,74},
        {893,480,56},
        {889,180,150},
        {885,120,226},
        {877,360,76},
        {870,120,230},
        {866,180,154},
        {862,480,58},
        {855,360,78},
        {847,240,118},
        {844,180,158},
        {840,120,238},
        {833,480,60},
        {826,120,242},
        {823,180,162},
        {820,240,122},
        {813,360,82},
        {806,480,62},
        {803,180,166},
        {800,120,250},
        {794,360,84},
        {787,120,254},
        {784,180,170},
        {781,480,64},
        {775,360,86},
        {769,240,130},
        {766,180,174},
        {758,480,66},
        {749,180,178},
        {746,240,134},
        {741,360,90},
        {735,480,68},
        {733,180,182},
        {725,360,92},
        {717,180,186},
        {714,480,70},
        {709,360,94},
        {704,240,142},
        {702,180,190},
        {694,480,72},
        {687,180,194},
        {685,240,146},
        {680,360,98},
        {676,480,74},
        {673,180,198},
        {667,360,100},
        {660,180,202},
        {658,480,76},
        {654,360,102},
        {649,240,154},
        {647,180,206},
        {641,480,78},
        {635,180,210},
        {633,240,158},
        {629,360,106},
        {625,480,80},
        {623,180,214},
        {617,360,108},
        {612,180,218},
        {610,480,82},
        {606,360,110},
        {602,240,166},
        {601,180,222},
        {595,480,84},
        {590,180,226},
        {588,240,170},
        {585,360,114},
        {581,480,86},
        {580,180,230},
        {575,360,116},
        {570,180,234},
        {568,480,88},
        {565,360,118},
        {562,240,178},
        {560,180,238},
        {556,480,90},
        {551,180,242},
        {549,240,182},
        {546,360,122},
        {543,480,92},
        {542,180,246},
        {538,360,124},
        {533,180,250},
        {532,480,94},
        {529,360,126},
        {526,240,190},
        {525,180,254},
        {521,480,96},
        {515,240,194},
        {513,360,130},
        {510,480,98},
        {505,360,132},
        {500,24000,2},
        {498,360,134},
        {495,240,202},
        {490,480,102},
        {485,240,206},
        {483,360,138},
        {481,480,104},
        {476,360,140},
        {472,480,106},
        {469,360,142},
        {467,240,214},
        {463,480,108},
        {459,240,218},
        {457,360,146},
        {455,480,110},
        {450,360,148},
        {446,480,112},
        {444,360,150},
        {442,240,226},
        {439,480,114},
        {435,240,230},
        {433,360,154},
        {431,480,116},
        {427,360,156},
        {424,480,118},
        {422,360,158},
        {420,240,238},
        {417,480,120},
        {413,240,242},
        {412,360,162},
        {410,480,122},
        {407,360,164},
        {403,480,124},
        {402,360,166},
        {400,240,250},
        {397,480,126},
        {394,240,254},
        {392,360,170},
        {391,480,128},
        {388,360,172},
        {385,480,130},
        {383,360,174},
        {379,480,132},
        {375,360,178},
        {373,480,134},
        {370,360,180},
        {368,480,136},
        {366,360,182},
        {362,480,138},
        {358,360,186},
        {357,480,140},
        {355,360,188},
        {352,480,142},
        {351,360,190},
        {347,480,144},
        {344,360,194},
        {342,480,146},
        {340,360,196},
        {338,480,148},
        {337,360,198},
        {333,36000,2},
        {330,360,202},
        {329,480,152},
        {327,360,204},
        {325,480,154},
        {324,360,206},
        {321,480,156},
        {317,360,210},
        {316,480,158},
        {314,360,212},
        {313,480,160},
        {312,360,214},
        {309,480,162},
        {306,360,218},
        {305,480,164},
        {303,360,220},
        {301,480,166},
        {300,360,222},
        {298,480,168},
        {295,360,226},
        {294,480,170},
        {292,360,228},
        {291,480,172},
        {290,360,230},
        {287,480,174},
        {285,360,234},
        {284,480,176},
        {282,360,236},
        {281,480,178},
        {280,360,238},
        {278,480,180},
        {275,480,182},
        {273,360,244},
        {272,480,184},
        {271,360,246},
        {269,480,186},
        {267,360,250},
        {266,480,188},
        {265,360,252},
        {263,480,190},
        {262,360,254},
        {260,480,192},
        {258,480,194},
        {255,480,196},
        {253,480,198},
        {250,48000,2},
        {248,480,202},
        {245,480,204},
        {243,480,206},
        {240,480,208},
        {238,480,210},
        {236,480,212},
        {234,480,214},
        {231,480,216},
        {229,480,218},
        {227,480,220},
        {225,480,222},
        {223,480,224},
        {221,480,226},
        {219,480,228},
        {217,480,230},
        {216,480,232},
        {214,480,234},
        {212,480,236},
        {210,480,238},
        {208,480,240},
        {207,480,242},
        {205,480,244},
        {203,480,246},
        {202,480,248},
        {200,12000,10},
        {198,480,252},
        {197,480,254},
        {195,480,256},
        {167,72000,2},
        {143,12000,14},
        {125,48000,4},
        {111,36000,6},
        {100,24000,10},
        {91,12000,22},
        {83,72000,4},
        {77,12000,26},
        {71,24000,14},
        {67,36000,10},
        {63,48000,8},
        {59,12000,34},
        {56,72000,6},
        {53,12000,38},
        {50,48000,10},
        {48,36000,14},
        {45,24000,22},
        {43,12000,46},
        {42,72000,8},
        {40,12000,50},
        {38,24000,26},
        {37,36000,18},
        {36,48000,14},
        {34,12000,58},
        {33,72000,10},
        {32,12000,62},
        {31,48000,16},
        {30,36000,22},
        {29,24000,34},
        {28,72000,12},
        {27,12000,74},
        {26,36000,26},
        {25,48000,20},
        {24,72000,14},
        {23,48000,22},
        {22,36000,30},
        {21,72000,16},
        {20,36000,34},
        {19,72000,18},
        {18,48000,28},
        {17,72000,20},
        {16,48000,32},
        {15,72000,22},
        {14,72000,24},
        {13,72000,26},
        {12,72000,28},
        {11,72000,30},
        {10,72000,34},
        {9,72000,38},
        {8,72000,44},
        {7,72000,50},
        {6,72000,60},
        {5,72000,74},
        {4,72000,94},
        {3,72000,132},
        {2,72000,222},
        {1,72000,256},
};

void findPWMSetting(unsigned int in_freq, unsigned int* pwm_source_div, unsigned int* pwm_active_cycle)
{
    int i = 0;
    int delta1, delta2;
    int size = sizeof(t)/sizeof(tone_freq);
    for(i=0; i<size-1; i++)
    {
        delta1 = t[i].f - in_freq;
        if(delta1 < 0)
            break;
        delta2 = t[i+1].f - in_freq;
        if(delta2 < 0)
        {   
            if(abs(delta2) < delta1)
                i++; 
            break;
        }
    }
    *pwm_source_div = t[i].div;
    *pwm_active_cycle = t[i].cycle / 2;
}

void tone(uint8_t _pin, unsigned int frequency, unsigned long duration)
{
     int ret = -1;
     int fd = -1;
     Tone_Config toneconfig;
         
     if ( (_pin == 5 || _pin == 6) && 
         (frequency >= 0 && frequency <= MAX_TONE_FREQ) )
     {         
         toneconfig.pin = _pin;
         if (frequency == 0) 
         {
            toneconfig.clksrc_div = 0;
            toneconfig.active_cycle = 0;
         }
         else
         {
            findPWMSetting(frequency, &toneconfig.clksrc_div, &toneconfig.active_cycle);
         }
         //printf("frequency =%d Hz, clk prediv=%d, active_cycle=%d \r\n", frequency, toneconfig.clksrc_div, toneconfig.active_cycle);

         fd = open(tone_dev, O_RDONLY);
         if (fd < 0)
             pabort("open tone device fail");
                
         ret = ioctl(fd, PWMTMR_TONE, &toneconfig);
         if (ret < 0)
             pabort("can't set PWMTMR_TONE");  

         if(fd)
            close(fd);
     }
     else
     {
         fprintf(stderr, "%s ERROR: invalid pin/frequency, pin=%d\n", __FUNCTION__, _pin);
         exit(-1);
     }   
}

void noTone(uint8_t _pin)
{
     int ret = -1;
     int fd = -1;
     Tone_Config toneconfig;
         
     if ( (_pin == 5 || _pin == 6) )
     {         
         toneconfig.pin = _pin;
         toneconfig.clksrc_div = 0;
         toneconfig.active_cycle = 0;

         fd = open(tone_dev, O_RDONLY);
         if (fd < 0)
             pabort("open tone device fail");
                
         ret = ioctl(fd, PWMTMR_TONE, &toneconfig);
         if (ret < 0)
             pabort("can't set PWMTMR_TONE");  

         if(fd)
            close(fd);
     }
     else
     {
         fprintf(stderr, "%s ERROR: invalid pin, pin=%d\n", __FUNCTION__, _pin);
         exit(-1);
     }   
}